// Copyright (c) 2017-2019 The PIVX developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "libzerocoin/Denominations.h"
#include "libzerocoin/Coin.h"
#include "libzerocoin/CoinRandomnessSchnorrSignature.h"
#include "amount.h"
#include "chainparams.h"
#include "coincontrol.h"
#include "consensus/zerocoin_verify.h"
#include "main.h"
#include "wallet/wallet.h"
#include "wallet/walletdb.h"
#include "txdb.h"
#include "zfls/zflsmodule.h"
#include "wallet/test/wallet_test_fixture.h"
#include <boost/test/unit_test.hpp>
#include <iostream>

BOOST_FIXTURE_TEST_SUITE(zerocoin_transactions_tests, WalletTestingSetup)

BOOST_AUTO_TEST_CASE(zerocoin_spend_test)
{
    SelectParams(CBaseChainParams::MAIN);
    libzerocoin::ZerocoinParams *ZCParams = Params().GetConsensus().Zerocoin_Params(false);
    (void)ZCParams;

    bool fFirstRun;
    pwalletMain->LoadWallet(fFirstRun);
    pwalletMain->zflsTracker = std::unique_ptr<CzFLSTracker>(new CzFLSTracker(pwalletMain));
    CMutableTransaction tx;
    CWalletTx* wtx = new CWalletTx(pwalletMain, tx);
    std::vector<CZerocoinSpend> vSpends;
    std::vector<CZerocoinMint> vMints;
    CAmount nAmount = COIN;

    CZerocoinSpendReceipt receipt;
    std::list<std::pair<CBitcoinAddress*, CAmount>> outputs;
    pwalletMain->SpendZerocoin(nAmount, *wtx, receipt, vMints, outputs);

    BOOST_CHECK_MESSAGE(receipt.GetStatus() == ZFLS_TRX_FUNDS_PROBLEMS, strprintf("Failed Invalid Amount Check: %s", receipt.GetStatusMessage()));

    nAmount = 1;
    CZerocoinSpendReceipt receipt2;
    pwalletMain->SpendZerocoin(nAmount, *wtx, receipt2, vMints, outputs);

    // if using "wallet.dat", instead of "unlocked.dat" need this
    /// BOOST_CHECK_MESSAGE(vString == "Error: Wallet locked, unable to create transaction!"," Locked Wallet Check Failed");

    BOOST_CHECK_MESSAGE(receipt2.GetStatus() == ZFLS_TRX_FUNDS_PROBLEMS, strprintf("Failed Invalid Amount Check: %s", receipt.GetStatusMessage()));

}

BOOST_AUTO_TEST_CASE(zerocoin_schnorr_signature_test)
{
    const int NUM_OF_TESTS = 50;
    SelectParams(CBaseChainParams::MAIN);
    libzerocoin::ZerocoinParams *ZCParams_v1 = Params().GetConsensus().Zerocoin_Params(true);
    (void)ZCParams_v1;
    libzerocoin::ZerocoinParams *ZCParams_v2 = Params().GetConsensus().Zerocoin_Params(false);
    (void)ZCParams_v2;

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

        // mint a v1 coin
        CBigNum s = CBigNum::randBignum(ZCParams_v1->coinCommitmentGroup.groupOrder);
        CBigNum r = CBigNum::randBignum(ZCParams_v1->coinCommitmentGroup.groupOrder);
        CBigNum c = ZCParams_v1->coinCommitmentGroup.g.pow_mod(s, ZCParams_v1->coinCommitmentGroup.modulus).mul_mod(
                ZCParams_v1->coinCommitmentGroup.h.pow_mod(r, ZCParams_v1->coinCommitmentGroup.modulus), ZCParams_v1->coinCommitmentGroup.modulus);
        for (uint32_t attempt = 0; attempt < MAX_COINMINT_ATTEMPTS; attempt++) {
            if (c.isPrime(ZEROCOIN_MINT_PRIME_PARAM)) break;
            CBigNum r_delta = CBigNum::randBignum(ZCParams_v1->coinCommitmentGroup.groupOrder);
            r = (r + r_delta) % ZCParams_v1->coinCommitmentGroup.groupOrder;
            c = ZCParams_v1->coinCommitmentGroup.g.pow_mod(s, ZCParams_v1->coinCommitmentGroup.modulus).mul_mod(
                    ZCParams_v1->coinCommitmentGroup.h.pow_mod(r, ZCParams_v1->coinCommitmentGroup.modulus), ZCParams_v1->coinCommitmentGroup.modulus);
        }
        BOOST_CHECK_MESSAGE(c.isPrime(ZEROCOIN_MINT_PRIME_PARAM), "Unable to mint v1 coin");
        libzerocoin::PrivateCoin privCoin_v1(ZCParams_v1, libzerocoin::CoinDenomination::ZQ_ONE, s, r);
        const CBigNum randomness_v1 = privCoin_v1.getRandomness();
        const CBigNum pubCoinValue_v1 = privCoin_v1.getPublicCoin().getValue();
        const CBigNum serialNumber_v1 = privCoin_v1.getSerialNumber();

        // mint a v2 coin
        libzerocoin::PrivateCoin privCoin_v2(ZCParams_v2, libzerocoin::CoinDenomination::ZQ_ONE, true);
        const CBigNum randomness_v2 = privCoin_v2.getRandomness();
        const CBigNum pubCoinValue_v2 = privCoin_v2.getPublicCoin().getValue();
        const CBigNum serialNumber_v2 = privCoin_v2.getSerialNumber();

        // get a random msghash
        const uint256 msghash = CBigNum::randKBitBignum(256).getuint256();

        // sign the msghash with the randomness of the v1 coin
        libzerocoin::CoinRandomnessSchnorrSignature crss_v1(ZCParams_v1, randomness_v1, msghash);
        CDataStream ser_crss_v1(SER_NETWORK, PROTOCOL_VERSION);
        ser_crss_v1 << crss_v1;

        // sign the msghash with the randomness of the v2 coin
        libzerocoin::CoinRandomnessSchnorrSignature crss_v2(ZCParams_v2, randomness_v2, msghash);
        CDataStream ser_crss_v2(SER_NETWORK, PROTOCOL_VERSION);
        ser_crss_v2 << crss_v2;

        // unserialize the v1 signature into a fresh object and verify it
        libzerocoin::CoinRandomnessSchnorrSignature new_crss_v1(ser_crss_v1);
        BOOST_CHECK_MESSAGE(
                new_crss_v1.Verify(ZCParams_v1, serialNumber_v1, pubCoinValue_v1, msghash),
                "Failed to verify schnorr signature with v1 coin"
                );

        // unserialize the v2 signature into a fresh object and verify it
        libzerocoin::CoinRandomnessSchnorrSignature new_crss_v2(ser_crss_v2);
        BOOST_CHECK_MESSAGE(
                new_crss_v2.Verify(ZCParams_v2, serialNumber_v2, pubCoinValue_v2, msghash),
                "Failed to verify schnorr signature with v2 coin"
                );

        // verify failure on different msghash
        uint256 msghash2;
        do {
            msghash2 = CBigNum::randKBitBignum(256).getuint256();
        } while (msghash2 == msghash);
        BOOST_CHECK_MESSAGE(
            !new_crss_v1.Verify(ZCParams_v1, serialNumber_v1, pubCoinValue_v1, msghash2),
            "schnorr signature with v1 coin verifies on wrong msghash"
            );
        BOOST_CHECK_MESSAGE(
            !new_crss_v2.Verify(ZCParams_v2, serialNumber_v2, pubCoinValue_v2, msghash2),
            "schnorr signature with v2 coin verifies on wrong msghash"
            );

        // verify failure swapping serials
        BOOST_CHECK_MESSAGE(
            !new_crss_v1.Verify(ZCParams_v1, serialNumber_v2, pubCoinValue_v1, msghash),
            "schnorr signature with v1 coin verifies on wrong serial"
            );
        BOOST_CHECK_MESSAGE(
            !new_crss_v2.Verify(ZCParams_v2, serialNumber_v1, pubCoinValue_v2, msghash),
            "schnorr signature with v2 coin verifies on wrong serial"
            );

        // verify failure swapping public coins
        BOOST_CHECK_MESSAGE(
            !new_crss_v1.Verify(ZCParams_v1, serialNumber_v1, pubCoinValue_v2, msghash),
            "schnorr signature with v1 coin verifies on wrong public coin value"
            );
        BOOST_CHECK_MESSAGE(
            !new_crss_v2.Verify(ZCParams_v2, serialNumber_v2, pubCoinValue_v1, msghash),
            "schnorr signature with v2 coin verifies on wrong public coin value"
            );

    }

}

BOOST_AUTO_TEST_CASE(zerocoin_public_spend_test)
{
    SelectParams(CBaseChainParams::MAIN);
    libzerocoin::ZerocoinParams *ZCParams_v1 = Params().GetConsensus().Zerocoin_Params(true);
    libzerocoin::ZerocoinParams *ZCParams_v2 = Params().GetConsensus().Zerocoin_Params(false);
    (void)ZCParams_v1;
    (void)ZCParams_v2;

    // create v1 coin
    CBigNum s = CBigNum::randBignum(ZCParams_v1->coinCommitmentGroup.groupOrder);
    CBigNum r = CBigNum::randBignum(ZCParams_v1->coinCommitmentGroup.groupOrder);
    CBigNum c = ZCParams_v1->coinCommitmentGroup.g.pow_mod(s, ZCParams_v1->coinCommitmentGroup.modulus).mul_mod(
            ZCParams_v1->coinCommitmentGroup.h.pow_mod(r, ZCParams_v1->coinCommitmentGroup.modulus), ZCParams_v1->coinCommitmentGroup.modulus);
    for (uint32_t attempt = 0; attempt < MAX_COINMINT_ATTEMPTS; attempt++) {
        if (c.isPrime(ZEROCOIN_MINT_PRIME_PARAM)) break;
        CBigNum r_delta = CBigNum::randBignum(ZCParams_v1->coinCommitmentGroup.groupOrder);
        r = (r + r_delta) % ZCParams_v1->coinCommitmentGroup.groupOrder;
        c = ZCParams_v1->coinCommitmentGroup.g.pow_mod(s, ZCParams_v1->coinCommitmentGroup.modulus).mul_mod(
                ZCParams_v1->coinCommitmentGroup.h.pow_mod(r, ZCParams_v1->coinCommitmentGroup.modulus), ZCParams_v1->coinCommitmentGroup.modulus);
    }
    BOOST_CHECK_MESSAGE(c.isPrime(ZEROCOIN_MINT_PRIME_PARAM), "Unable to mint v1 coin");
    libzerocoin::PrivateCoin privCoin_v1(ZCParams_v1, libzerocoin::CoinDenomination::ZQ_ONE, s, r);

    CZerocoinMint mint_v1 = CZerocoinMint(
            privCoin_v1.getPublicCoin().getDenomination(),
            privCoin_v1.getPublicCoin().getValue(),
            privCoin_v1.getRandomness(),
            privCoin_v1.getSerialNumber(),
            false,
            1,
            nullptr);

    // create v2 coin
    libzerocoin::PrivateCoin privCoin_v2(ZCParams_v2, libzerocoin::CoinDenomination::ZQ_ONE, true);
    CPrivKey privKey = privCoin_v2.getPrivKey();

    CZerocoinMint mint_v2 = CZerocoinMint(
            privCoin_v2.getPublicCoin().getDenomination(),
            privCoin_v2.getPublicCoin().getValue(),
            privCoin_v2.getRandomness(),
            privCoin_v2.getSerialNumber(),
            false,
            privCoin_v2.getVersion(),
            &privKey);

    // Mint txs
    CTransaction prevTx_v1;
    CTransaction prevTx_v2;

    CScript scriptSerializedCoin_v1 = CScript()
    << OP_ZEROCOINMINT << privCoin_v1.getPublicCoin().getValue().getvch().size() << privCoin_v1.getPublicCoin().getValue().getvch();
    CTxOut out_v1 = CTxOut(libzerocoin::ZerocoinDenominationToAmount(privCoin_v1.getPublicCoin().getDenomination()), scriptSerializedCoin_v1);
    prevTx_v1.vout.push_back(out_v1);

    CScript scriptSerializedCoin_v2 = CScript()
    << OP_ZEROCOINMINT << privCoin_v2.getPublicCoin().getValue().getvch().size() << privCoin_v2.getPublicCoin().getValue().getvch();
    CTxOut out_v2 = CTxOut(libzerocoin::ZerocoinDenominationToAmount(privCoin_v2.getPublicCoin().getDenomination()), scriptSerializedCoin_v2);
    prevTx_v2.vout.push_back(out_v2);

    mint_v1.SetOutputIndex(0);
    mint_v1.SetTxHash(prevTx_v1.GetHash());

    mint_v2.SetOutputIndex(0);
    mint_v2.SetTxHash(prevTx_v2.GetHash());

    // Spend txs
    CMutableTransaction tx1, tx2, tx3;
    tx1.vout.resize(1);
    tx1.vout[0].nValue = 1*CENT;
    tx1.vout[0].scriptPubKey = GetScriptForDestination(CBitcoinAddress("D9Ti4LEhF1n6dR2hGd2SyNADD51AVgva6q").Get());
    tx2.vout.resize(1);
    tx2.vout[0].nValue = 1*CENT;
    tx2.vout[0].scriptPubKey = GetScriptForDestination(CBitcoinAddress("D9Ti4LEhF1n6dR2hGd2SyNADD51AVgva6q").Get());
    tx3.vout.resize(1);
    tx3.vout[0].nValue = 1*CENT;
    tx3.vout[0].scriptPubKey = GetScriptForDestination(CBitcoinAddress("D9Ti4LEhF1n6dR2hGd2SyNADD51AVgva6q").Get());

    CTxIn in1, in2, in3;

    // check spendVersion = 3 for v2 coins
    // -----------------------------------
    int spendVersion = 3;
    BOOST_CHECK_MESSAGE(ZFLSModule::createInput(in1, mint_v2, tx1.GetHash(), spendVersion),
            "Failed to create zc input for mint v2 and spendVersion 3");

    std::cout << "Spend v3 size: " << ::GetSerializeSize(in1, SER_NETWORK, PROTOCOL_VERSION) << " bytes" << std::endl;

    PublicCoinSpend publicSpend1(ZCParams_v2);
    BOOST_CHECK_MESSAGE(ZFLSModule::validateInput(in1, out_v2, tx1, publicSpend1),
            "Failed to validate zc input for mint v2 and spendVersion 3");

    // Verify that it fails with a different denomination
    in1.nSequence = 500;
    PublicCoinSpend publicSpend1b(ZCParams_v2);
    BOOST_CHECK_MESSAGE(!ZFLSModule::validateInput(in1, out_v2, tx1, publicSpend1b), "Different denomination for mint v2 and spendVersion 3");

    // check spendVersion = 4 for v2 coins
    // -----------------------------------
    spendVersion = 4;
    BOOST_CHECK_MESSAGE(ZFLSModule::createInput(in2, mint_v2, tx2.GetHash(), spendVersion),
            "Failed to create zc input for mint v2 and spendVersion 4");

    std::cout << "Spend v4 (coin v2) size: " << ::GetSerializeSize(in2, SER_NETWORK, PROTOCOL_VERSION) << " bytes" << std::endl;

    PublicCoinSpend publicSpend2(ZCParams_v2);
    BOOST_CHECK_MESSAGE(ZFLSModule::validateInput(in2, out_v2, tx2, publicSpend2),
            "Failed to validate zc input for mint v2 and spendVersion 4");

    // Verify that it fails with a different denomination
    in2.nSequence = 500;
    PublicCoinSpend publicSpend2b(ZCParams_v2);
    BOOST_CHECK_MESSAGE(!ZFLSModule::validateInput(in2, out_v2, tx2, publicSpend2b), "Different denomination for mint v2 and spendVersion 4");

    // check spendVersion = 4 for v1 coins
    // -----------------------------------
    BOOST_CHECK_MESSAGE(ZFLSModule::createInput(in3, mint_v1, tx3.GetHash(), spendVersion),
            "Failed to create zc input for mint v1 and spendVersion 4");

    std::cout << "Spend v4 (coin v1) size: " << ::GetSerializeSize(in3, SER_NETWORK, PROTOCOL_VERSION) << " bytes" << std::endl;

    PublicCoinSpend publicSpend3(ZCParams_v1);
    BOOST_CHECK_MESSAGE(ZFLSModule::validateInput(in3, out_v1, tx3, publicSpend3),
            "Failed to validate zc input for mint v1 and spendVersion 4");

    // Verify that it fails with a different denomination
    in3.nSequence = 500;
    PublicCoinSpend publicSpend3b(ZCParams_v1);
    BOOST_CHECK_MESSAGE(!ZFLSModule::validateInput(in3, out_v1, tx3, publicSpend3b), "Different denomination for mint v1 and spendVersion 4");

}

BOOST_AUTO_TEST_SUITE_END()


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_ComptonProfilePolicy_def.hpp
//! \author Alex Robinson
//! \brief  Policies (defs.) for using Compton profiles
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_COMPTON_PROFILE_POLICY_DEF_HPP
#define MONTE_CARLO_COMPTON_PROFILE_POLICY_DEF_HPP

// Std Lib Includes
#include <cmath>

// Boost Includes
#include <boost/units/cmath.hpp>

// FRENSIE Includes
#include "MonteCarlo_ComptonProfile.hpp"
#include "Utility_RandomNumberGenerator.hpp"

namespace MonteCarlo{

// Check if the full Compton profile is valid
/*! \details A full Compton profile must have a range between +/- m_e*c. The
 * upper bound can be greater than m_e*c but no less.
 */
inline bool FullComptonProfilePolicy::isValidProfile(
                                                const ComptonProfile& profile )
{
  return profile.getLowerBoundOfMomentum() == -1.0*ComptonProfile::MomentumUnit()
    && profile.getUpperBoundOfMomentum() >= 1.0*ComptonProfile::MomentumUnit();
}

// Get the lower bound of the momentum
inline ComptonProfile::MomentumQuantity
FullComptonProfilePolicy::getLowerBoundOfMomentum(
                                                const ComptonProfile& profile )
{
  return profile.getLowerBoundOfMomentum();
}

// Get the upper bound of the momentum
inline ComptonProfile::MomentumQuantity
FullComptonProfilePolicy::getUpperBoundOfMomentum(
                                                const ComptonProfile& profile )
{
  return profile.getUpperBoundOfMomentum();
}

// Get lower limit of integration
/*! \details For a full profile the lower limit of integration will always
 * be -1.0
 */
inline ComptonProfile::MomentumQuantity
FullComptonProfilePolicy::getLowerLimitOfIntegration(
                           const ComptonProfile::MomentumQuantity upper_limit )
{
  return -1.0*ComptonProfile::MomentumUnit();
}

// Get upper limit of integration
/*! \details This method simply checks if the upper limit passed is less than
 * the table max. If it is greater, the table max is returned.
 */
inline ComptonProfile::MomentumQuantity
FullComptonProfilePolicy::getUpperLimitOfIntegration(
                           const ComptonProfile& profile,
                           const ComptonProfile::MomentumQuantity upper_limit )
{
  if( upper_limit > profile.getUpperBoundOfMomentum() )
    return profile.getUpperBoundOfMomentum();
  else
    return upper_limit;
}

// Evaluate a full Compton profile
inline ComptonProfile::ProfileQuantity FullComptonProfilePolicy::evaluate(
                              const ComptonProfile& profile,
                              const ComptonProfile::MomentumQuantity momentum )
{
  return profile.evaluate( momentum );
}

// Evaluate a full Compton profile using the max momentum as a limit
/*! \details With a full Compton profile there is no check to see if the
 * momentum is greater than -max_momentum (as with the half profiles).
 */
inline ComptonProfile::ProfileQuantity
FullComptonProfilePolicy::evaluateWithPossibleLimit(
                          const ComptonProfile& profile,
                          const ComptonProfile::MomentumQuantity momentum,
                          const ComptonProfile::MomentumQuantity max_momentum )
{
  if( momentum <= max_momentum )
    return profile.evaluate( momentum );
  else
    return 0.0*ComptonProfile::ProfileUnit();
}

// Sample from a full Compton profile
inline ComptonProfile::MomentumQuantity FullComptonProfilePolicy::sample(
                          const ComptonProfile& profile,
                          const ComptonProfile::MomentumQuantity max_momentum )
{
  // Make sure the max momentum is valid
  testPrecondition( max_momentum >= -1.0*ComptonProfile::MomentumUnit() );

  if( max_momentum >= profile.getUpperBoundOfMomentum() )
    return profile.sample();
  else
    return profile.sampleInSubrange( max_momentum );
}

// Check if the half Compton profile is valid
/*! \details Using only half of the profile is a way to approximate the
 * true profile. The lower momentum bound must be zero. The upper bound
 * can be anything (since this is an approximation).
 */
inline bool HalfComptonProfilePolicyHelper::isValidProfile(
                                               const ComptonProfile& profile )
{
  return profile.getLowerBoundOfMomentum() == 0.0*ComptonProfile::MomentumUnit()
    && profile.getUpperBoundOfMomentum() <= 1.0*ComptonProfile::MomentumUnit();
}

// Get the lower bound of the momentum
inline ComptonProfile::MomentumQuantity
HalfComptonProfilePolicyHelper::getLowerBoundOfMomentum(
                                                const ComptonProfile& profile )
{
  return -profile.getUpperBoundOfMomentum();
}

// Get the upper bound of the momentum
inline ComptonProfile::MomentumQuantity
HalfComptonProfilePolicyHelper::getUpperBoundOfMomentum(
                                                const ComptonProfile& profile )
{
  return profile.getUpperBoundOfMomentum();
}

// Get lower limit of integration
/*! \details For a half profile the lower limit of integration will be
 * -upper_limit unless the upper limit is <= 0.0, in which case it is equal
 * to the upper_limit (which ensures that the integral is zero).
 */
inline ComptonProfile::MomentumQuantity
HalfComptonProfilePolicyHelper::getLowerLimitOfIntegration(
                           const ComptonProfile::MomentumQuantity upper_limit )
{
  if( upper_limit > 0.0*ComptonProfile::MomentumUnit() )
    return -upper_limit;
  else
    return upper_limit;
}

// Get upper limit of integration
/*! \details This method simply checks if the upper limit passed is less than
 * the table max. If it is greater, the table max is returned.
 */
inline ComptonProfile::MomentumQuantity
HalfComptonProfilePolicyHelper::getUpperLimitOfIntegration(
                           const ComptonProfile& profile,
                           const ComptonProfile::MomentumQuantity upper_limit )
{
  if( upper_limit > profile.getUpperBoundOfMomentum() )
    return profile.getUpperBoundOfMomentum();
  else
    return upper_limit;
}

// Sample from a half Compton profile
/*! \details Using only half of the profile is a way to approximate the
 * true profile. The momentum sampled from the half distribution will be
 * randomly multiplied by -1 to account for the other half of the distribution
 * (part of it at least). As the max momentum approaches zero this
 * approximation of the true distribution becomes poor. If the max momentum
 * is negative, which is physically acceptable, a momentum of zero is returned,
 * which is the value associated with no Doppler broadening (it corresponds
 * to the Compton line photon energy).
 */
inline ComptonProfile::MomentumQuantity HalfComptonProfilePolicyHelper::sample(
                          const ComptonProfile& profile,
                          const ComptonProfile::MomentumQuantity max_momentum )
{
  // Make sure the max momentum is valid
  testPrecondition( max_momentum >= -1.0*ComptonProfile::MomentumUnit() );

  if( max_momentum > 0.0*ComptonProfile::MomentumUnit() )
  {
    ComptonProfile::MomentumQuantity pz;

    if( max_momentum >= profile.getUpperBoundOfMomentum() )
      pz = profile.sample();
    else
      pz = profile.sampleInSubrange( max_momentum );

    if( Utility::RandomNumberGenerator::getRandomNumber<double>() <= 0.5 )
      pz *= -1.0;

    return pz;
  }
  else
    return 0.0*ComptonProfile::MomentumUnit();
}

// Evaluate a half Compton profile
/*! \details The absolute value of the momentum will be used to
 * evaluate the profile.
 */
inline ComptonProfile::ProfileQuantity HalfComptonProfilePolicy::evaluate(
                              const ComptonProfile& profile,
                              const ComptonProfile::MomentumQuantity momentum )
{
  return profile.evaluate( boost::units::fabs( momentum ) );
}

// Evaluate a half Compton profile using the max momentum as a limit
/*! \details The half Compton profile will only be evaluated between
 * [-max_momentum,max_momentum]. If max_momentum is < 0.0, a value of
 * 0.0 will be returned.
 */
inline ComptonProfile::ProfileQuantity
HalfComptonProfilePolicy::evaluateWithPossibleLimit(
                          const ComptonProfile& profile,
                          const ComptonProfile::MomentumQuantity momentum,
                          const ComptonProfile::MomentumQuantity max_momentum )
{
  if( max_momentum >= 0.0*ComptonProfile::MomentumUnit() )
  {
    if( momentum <= max_momentum && momentum >= -max_momentum )
      return profile.evaluate( boost::units::fabs( momentum ) );
    else
      return 0.0*ComptonProfile::ProfileUnit();
  }
  else
    return 0.0*ComptonProfile::ProfileUnit();
}

// Evaluate a double half Compton profile
/*! \details The doubled half Compton profile has been doubled so that it
 * remains normalized. The evaluated profile value will be divided by two to
 * account for the doubling. The absolute value of the momentum will be used
 * to evaluate the profile.
 */
inline ComptonProfile::ProfileQuantity
DoubledHalfComptonProfilePolicy::evaluate(
                              const ComptonProfile& profile,
                              const ComptonProfile::MomentumQuantity momentum )
{
  return profile.evaluate( boost::units::fabs( momentum ) )/2.0;
}

// Evaluate a doubled half Compton profile using the max momentum as a limit
/*! \details The half Compton profile will only be evaluated between
 * [-max_momentum,max_momentum]. If max_momentum is < 0.0, a value of
 * 0.0 will be returned.
 */
inline ComptonProfile::ProfileQuantity
DoubledHalfComptonProfilePolicy::evaluateWithPossibleLimit(
                          const ComptonProfile& profile,
                          const ComptonProfile::MomentumQuantity momentum,
                          const ComptonProfile::MomentumQuantity max_momentum )
{
  if( max_momentum >= 0.0*ComptonProfile::MomentumUnit() )
  {
    if( momentum <= max_momentum && momentum >= -max_momentum )
      return profile.evaluate( boost::units::fabs( momentum ) )/2.0;
    else
      return 0.0*ComptonProfile::ProfileUnit();
  }
  else
    return 0.0*ComptonProfile::ProfileUnit();
}

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_COMPTON_PROFILE_POLICY_DEF_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_ComptonProfilePolicy_def.hpp
//---------------------------------------------------------------------------//


#ifndef REGION_SET_HPP_INCLUDED
#define REGION_SET_HPP_INCLUDED

#include <map>
#include <set>
#include <utility>
#include <vector>

#include <boost/python.hpp>
#include <boost/python/numpy.hpp>

namespace python = boost::python;
namespace numpy = boost::python::numpy;

namespace grid {
struct region_less_compare;
using numeric_type_t = double;
using region_element = std::pair<numeric_type_t, numeric_type_t>;
using region = std::vector<region_element>;
using point = std::vector<numeric_type_t>;
} // namespace grid

bool operator<(grid::point const &, grid::region const &);
bool operator<(grid::region const &, grid::point const &);
bool operator<(grid::region const &, grid::region const &);
bool operator<(grid::point const &, grid::point const &);

bool operator<(numpy::ndarray const &, numpy::ndarray const &);

namespace grid {
struct region_less_compare {
  using is_transparent = void;
  bool operator()(grid::point const &p, grid::region const &r) const {
    return p < r;
  }
  bool operator()(grid::region const &r, grid::point const &p) const {
    return r < p;
  }
  bool operator()(grid::region const &r1, grid::region const &r2) const {
    return r1 < r2;
  }
  bool operator()(grid::point const &p1, grid::point const &p2) const {
    return p1 < p2;
  }
};

struct ndarray_less_compare {
    using is_transparent = void;
    bool operator()(numpy::ndarray const& a, numpy::ndarray const& b) {
        return a < b;
    }
};
} // namespace grid

using region_set = std::set<grid::region, grid::region_less_compare>;
using region_key_point_map =
    std::map<grid::region, grid::point, grid::region_less_compare>;
using region_stack = std::vector<grid::region>;
using point_stack = std::vector<grid::point>;
using point_set = std::set<numpy::ndarray, grid::ndarray_less_compare>;

struct RegionSet {
  region_set region_set_internal;
  bool insert(numpy::ndarray const &, numpy::ndarray const &);
  std::size_t size();
  python::tuple get_and_remove_region_containing_point(numpy::ndarray const &);
  python::tuple pop_random();
};

struct PointSet {
    point_set point_set_internal;
    bool insert(numpy::ndarray const&);
    std::size_t size();
    point_set::iterator begin();
    point_set::iterator end();
};

#endif


// Copyright (C) 2011 by the BEM++ Authors
//
// 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 "fiber/piecewise_constant_scalar_basis.hpp"
#include "../type_template.hpp"

#include <algorithm>
#include "common/armadillo_fwd.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/version.hpp>

// Tests

BOOST_AUTO_TEST_SUITE(PiecewiseConstantScalarBasis)

BOOST_AUTO_TEST_CASE_TEMPLATE(size_is_1, ValueType, basis_function_types)
{
    Fiber::PiecewiseConstantScalarBasis<ValueType> basis;
    BOOST_CHECK_EQUAL(basis.size(), 1);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(order_is_0, ValueType, basis_function_types)
{
    Fiber::PiecewiseConstantScalarBasis<ValueType> basis;
    BOOST_CHECK_EQUAL(basis.order(), 0);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_values_works_for_all_dofs,
                              ValueType, basis_function_types)
{
    typedef Fiber::PiecewiseConstantScalarBasis<ValueType> Basis;
    Basis basis;
    arma::Mat<typename Basis::CoordinateType> points(3, 4);
    srand(1);
    points.randu();
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::VALUES, points, Fiber::ALL_DOFS, data);

    Fiber::_3dArray<ValueType> expected(1, 1, points.n_cols);
    std::fill(expected.begin(), expected.end(), 1.);

    BOOST_CHECK_EQUAL(data.values.extent(0), 1u); // 1 component
    BOOST_CHECK_EQUAL(data.values.extent(1), 1u); // 1 basis function
    BOOST_CHECK_EQUAL(data.values.extent(2), points.n_cols);
    BOOST_CHECK_EQUAL_COLLECTIONS(data.values.begin(),
                                  data.values.end(),
                                  expected.begin(),
                                  expected.end());
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_values_works_for_one_dof,
                              ValueType, basis_function_types)
{
    typedef Fiber::PiecewiseConstantScalarBasis<ValueType> Basis;
    Basis basis;
    arma::Mat<typename Basis::CoordinateType> points(3, 4);
    srand(1);
    points.randu();
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::VALUES, points, 0 /* dof number */, data);

    Fiber::_3dArray<ValueType> expected(1, 1, points.n_cols);
    std::fill(expected.begin(), expected.end(), 1.);

    BOOST_CHECK_EQUAL(data.values.extent(0), 1u); // 1 component
    BOOST_CHECK_EQUAL(data.values.extent(1), 1u); // 1 basis function
    BOOST_CHECK_EQUAL(data.values.extent(2), points.n_cols);
    BOOST_CHECK_EQUAL_COLLECTIONS(data.values.begin(),
                                  data.values.end(),
                                  expected.begin(),
                                  expected.end());
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_derivatives_works_for_all_dofs,
                              ValueType, basis_function_types)
{
    typedef Fiber::PiecewiseConstantScalarBasis<ValueType> Basis;
    Basis basis;
    arma::Mat<typename Basis::CoordinateType> points(3, 4);
    srand(1);
    points.randu();
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::DERIVATIVES, points, Fiber::ALL_DOFS, data);

    Fiber::_4dArray<ValueType> expected(1,  // 1 component
                                        points.n_rows,
                                        1,  // 1 basis functions
                                        points.n_cols);
    std::fill(expected.begin(), expected.end(), 0.);
    BOOST_CHECK_EQUAL(data.derivatives.extent(0), expected.extent(0));
    BOOST_CHECK_EQUAL(data.derivatives.extent(1), expected.extent(1));
    BOOST_CHECK_EQUAL(data.derivatives.extent(2), expected.extent(2));
    BOOST_CHECK_EQUAL(data.derivatives.extent(3), expected.extent(3));
    BOOST_CHECK_EQUAL_COLLECTIONS(data.derivatives.begin(),
                                  data.derivatives.end(),
                                  expected.begin(),
                                  expected.end());
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_derivatives_works_for_one_dof,
                              ValueType, basis_function_types)
{
    typedef Fiber::PiecewiseConstantScalarBasis<ValueType> Basis;
    Basis basis;
    arma::Mat<typename Basis::CoordinateType> points(3, 4);
    srand(1);
    points.randu();
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::DERIVATIVES, points, 0 /* dof number */, data);

    Fiber::_4dArray<ValueType> expected(1,  // 1 component
                                        points.n_rows,
                                        1,  // 1 basis functions
                                        points.n_cols);
    std::fill(expected.begin(), expected.end(), 0.);
    BOOST_CHECK_EQUAL(data.derivatives.extent(0), expected.extent(0));
    BOOST_CHECK_EQUAL(data.derivatives.extent(1), expected.extent(1));
    BOOST_CHECK_EQUAL(data.derivatives.extent(2), expected.extent(2));
    BOOST_CHECK_EQUAL(data.derivatives.extent(3), expected.extent(3));
    BOOST_CHECK_EQUAL_COLLECTIONS(data.derivatives.begin(),
                                  data.derivatives.end(),
                                  expected.begin(),
                                  expected.end());
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluate_values_and_derivatives_works_for_all_dofs,
                              ValueType, basis_function_types)
{
    typedef Fiber::PiecewiseConstantScalarBasis<ValueType> Basis;
    Basis basis;
    arma::Mat<typename Basis::CoordinateType> points(3, 4);
    srand(1);
    points.randu();
    Fiber::BasisData<ValueType> data;
    basis.evaluate(Fiber::VALUES | Fiber::DERIVATIVES, points, 0 /* dof number */, data);

    {
        Fiber::_3dArray<ValueType> expected(1, 1, points.n_cols);
        std::fill(expected.begin(), expected.end(), 1.);
        BOOST_CHECK_EQUAL(data.values.extent(0), 1); // 1 component
        BOOST_CHECK_EQUAL(data.values.extent(1), 1); // 1 basis function
        BOOST_CHECK_EQUAL(data.values.extent(2), points.n_cols);
        BOOST_CHECK_EQUAL_COLLECTIONS(data.values.begin(),
                                      data.values.end(),
                                      expected.begin(),
                                      expected.end());
    }

    {
        Fiber::_4dArray<ValueType> expected(1,  // 1 component
                                            points.n_rows,
                                            1,  // 1 basis functions
                                            points.n_cols);
        std::fill(expected.begin(), expected.end(), 0.);
        BOOST_CHECK_EQUAL(data.derivatives.extent(0), expected.extent(0));
        BOOST_CHECK_EQUAL(data.derivatives.extent(1), expected.extent(1));
        BOOST_CHECK_EQUAL(data.derivatives.extent(2), expected.extent(2));
        BOOST_CHECK_EQUAL(data.derivatives.extent(3), expected.extent(3));
        BOOST_CHECK_EQUAL_COLLECTIONS(data.derivatives.begin(),
                                      data.derivatives.end(),
                                      expected.begin(),
                                      expected.end());
    }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef TEST_UNIT_UTIL_HPP
#define TEST_UNIT_UTIL_HPP

#include <stan/math/prim/fun/Eigen.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/typeof/typeof.hpp>
#include <test/unit/pretty_print_types.hpp>
#include <gtest/gtest.h>
#include <type_traits>
#include <string>

/**
 * Tests for  exact elementwise equality of the input matrices
 * with the EXPECT_EQ macro from GTest.
 *
 * @param A first input matrix to compare
 * @param B second input matrix to compare
 */
#define EXPECT_MATRIX_EQ(A, B)                                        \
  {                                                                   \
    using T_A = std::decay_t<decltype(A)>;                            \
    using T_B = std::decay_t<decltype(B)>;                            \
    const Eigen::Matrix<typename T_A::Scalar, T_A::RowsAtCompileTime, \
                        T_A::ColsAtCompileTime>                       \
        A_eval = A;                                                   \
    const Eigen::Matrix<typename T_B::Scalar, T_B::RowsAtCompileTime, \
                        T_B::ColsAtCompileTime>                       \
        B_eval = B;                                                   \
    EXPECT_EQ(A_eval.rows(), B_eval.rows());                          \
    EXPECT_EQ(A_eval.cols(), B_eval.cols());                          \
    for (int i = 0; i < A_eval.size(); i++)                           \
      EXPECT_EQ(A_eval(i), B_eval(i));                                \
  }

/**
 * Tests for elementwise equality of the input matrices
 * of doubles with the EXPECT_FLOAT_EQ macro from
 * GTest.
 *
 * @param A first input matrix to compare
 * @param B second input matrix to compare
 */
#define EXPECT_MATRIX_FLOAT_EQ(A, B)         \
  {                                          \
    const Eigen::MatrixXd& A_eval = A;       \
    const Eigen::MatrixXd& B_eval = B;       \
    EXPECT_EQ(A_eval.rows(), B_eval.rows()); \
    EXPECT_EQ(A_eval.cols(), B_eval.cols()); \
    for (int i = 0; i < A_eval.size(); i++)  \
      EXPECT_FLOAT_EQ(A_eval(i), B_eval(i)); \
  }

/**
 * Tests for elementwise equality of the input std::vectors
 * of any type with the EXPECT_FLOAT_EQ macro from GTest.
 *
 * @param A first input vector to compare
 * @param B second input vector to compare
 */
#define EXPECT_STD_VECTOR_FLOAT_EQ(A, B) \
  EXPECT_EQ(A.size(), B.size());         \
  for (int i = 0; i < A.size(); ++i)     \
    EXPECT_FLOAT_EQ(A[i], B[i]);

/**
 * Tests for elementwise equality of the input std::vectors
 * of any type with the EXPECT_EQ macro from GTest.
 *
 * @param A first input vector of integers to compare
 * @param B second input vector of integers to compare
 */
#define EXPECT_STD_VECTOR_EQ(A, B)   \
  EXPECT_EQ(A.size(), B.size());     \
  for (int i = 0; i < A.size(); ++i) \
    EXPECT_EQ(A[i], B[i]);

/**
 * Tests if any elementwise difference of the input matrices
 * of doubles is greater than DELTA. This uses the
 * EXPECT_NEAR macro from GTest.
 *
 * @param A first input matrix to compare
 * @param B second input matrix to compare
 * @param DELTA the maximum allowed difference
 */
#define EXPECT_MATRIX_NEAR(A, B, DELTA)                               \
  {                                                                   \
    using T_A = std::decay_t<decltype(A)>;                            \
    using T_B = std::decay_t<decltype(B)>;                            \
    const Eigen::Matrix<typename T_A::Scalar, T_A::RowsAtCompileTime, \
                        T_A::ColsAtCompileTime>                       \
        A_eval = A;                                                   \
    const Eigen::Matrix<typename T_B::Scalar, T_B::RowsAtCompileTime, \
                        T_B::ColsAtCompileTime>                       \
        B_eval = B;                                                   \
    EXPECT_EQ(A_eval.rows(), B_eval.rows());                          \
    EXPECT_EQ(A_eval.cols(), B_eval.cols());                          \
    for (int i = 0; i < A_eval.size(); i++)                           \
      EXPECT_NEAR(A_eval(i), B_eval(i), DELTA);                       \
  }

/**
 * Tests if given types are the same type.
 *
 * @param a first type
 * @param b second type (code for this one can contain commas)
 **/
#define EXPECT_SAME_TYPE(a, ...)                                           \
  EXPECT_TRUE((std::is_same<a, __VA_ARGS__>::value))                       \
      << "Type a is" << stan::math::test::type_name<a>() << ". Type b is " \
      << stan::math::test::type_name<__VA_ARGS__>();

/**
 * Tests if given value is of given type.
 *
 * @param type type
 * @param value value (code for this one can contain commas)
 **/
#define EXPECT_TYPE(type, ...) EXPECT_SAME_TYPE(type, decltype(__VA_ARGS__))

/**
 * Count the number of times a substring is found in
 * a supplied string.
 *
 * @param target substring to match in s
 * @param s string to match count occurrences
 * @return number of found occurrences of target in s
 */
int count_matches(const std::string& target, const std::string& s) {
  if (target.size() == 0)
    return -1;  // error
  int count = 0;
  for (size_t pos = 0; (pos = s.find(target, pos)) != std::string::npos;
       pos += target.size())
    ++count;
  return count;
}

/**
 * Tests if the expression throws the expected
 * exception with a specific number of occurrences of
 * the expected message in the throw message.
 *
 * @param expr expression to test
 * @param T_e type of exception
 * @param msg expected message
 * @param count the expected number of occurrences of msg
 * in the message of the exception
 */
#define EXPECT_THROW_MSG_WITH_COUNT(expr, T_e, msg, count) \
  EXPECT_THROW(expr, T_e);                                 \
  try {                                                    \
    expr;                                                  \
  } catch (const T_e& e) {                                 \
    EXPECT_EQ(count, count_matches(msg, e.what()))         \
        << "expected message: " << msg << std::endl        \
        << "found message:    " << e.what();               \
  }

/**
 * Tests if the expression throws the expected
 * exception with the expected message.
 *
 * @param expr expression to test
 * @param T_e type of exception
 * @param msg expected message
 */
#define EXPECT_THROW_MSG(expr, T_e, msg) \
  EXPECT_THROW_MSG_WITH_COUNT(expr, T_e, msg, 1)

namespace stan {
namespace test {

auto make_sparse_matrix_random(int rows, int cols) {
  using eigen_triplet = Eigen::Triplet<double>;
  boost::mt19937 gen;
  boost::random::uniform_real_distribution<double> dist(0.0, 1.0);
  std::vector<eigen_triplet> tripletList;
  for (int i = 0; i < rows; ++i) {
    for (int j = 0; j < cols; ++j) {
      auto v_ij = dist(gen);
      if (v_ij < 0.1) {
        tripletList.push_back(eigen_triplet(i, j, v_ij));
      }
    }
  }
  Eigen::SparseMatrix<double> mat(rows, cols);
  mat.setFromTriplets(tripletList.begin(), tripletList.end());
  return mat;
}

}  // namespace test
}  // namespace stan

#endif


/**
 * $Id$
 *
 * Copyright (C)
 * 2014 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#include <boost/shared_ptr.hpp>
#include <boost/python.hpp>

#include <boost/numpy/ndarray_accessor_return.hpp>

#include <ndhist/ndtable.hpp>

namespace bp = boost::python;
namespace bn = boost::numpy;

namespace ndhist {

void register_ndtable()
{
    bp::class_<ndtable, boost::shared_ptr<ndtable> >("ndtable"
        , "The ndtable class holds data of a multi-dimensional tabularized \n"
          "function."
        , bp::init<
              bn::ndarray const &
            , bn::dtype const &
          >((bp::arg("shape"), bp::arg("dtype")))
    )
    .add_property("data", bp::make_function(&ndtable::GetNDArray, bn::ndarray_accessor_return()
        , (bp::arg("self")))
        , "The ndarray holding the table data.")
    ;
}

}// namespace ndhist


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

/** \author Justin Carpentier */

#define BOOST_TEST_MODULE FCL_HEIGHT_FIELDS
#include <boost/test/included/unit_test.hpp>
#include <boost/filesystem.hpp>

#include "fcl_resources/config.h"

#include <hpp/fcl/collision.h>
#include <hpp/fcl/hfield.h>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/mesh_loader/assimp.h>
#include <hpp/fcl/mesh_loader/loader.h>

#include <hpp/fcl/collision.h>

#include "utility.h"
#include <iostream>

using namespace hpp::fcl;

template<typename BV>
void test_constant_hfields(const Eigen::DenseIndex nx, const Eigen::DenseIndex ny,
                           const FCL_REAL min_altitude, const FCL_REAL max_altitude)
{
  const FCL_REAL x_dim = 1., y_dim = 2.;
  const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude);

  HeightField<BV> hfield(x_dim,y_dim,heights,min_altitude);

  BOOST_CHECK(hfield.getXDim() == x_dim);
  BOOST_CHECK(hfield.getYDim() == y_dim);

  const VecXf & x_grid = hfield.getXGrid();
  BOOST_CHECK(x_grid[0] == -x_dim/2.);
  BOOST_CHECK(x_grid[nx-1] == x_dim/2.);

  const VecXf & y_grid = hfield.getYGrid();
  BOOST_CHECK(y_grid[0] == y_dim/2.);
  BOOST_CHECK(y_grid[ny-1] == -y_dim/2.);

  // Test local AABB
  hfield.computeLocalAABB();

  for(Eigen::DenseIndex i = 0; i < nx; ++i)
  {
    for(Eigen::DenseIndex j = 0; j < ny; ++j)
    {
      Vec3f point(x_grid[i],y_grid[j],heights(j,i));
      BOOST_CHECK(hfield.aabb_local.contain(point));
    }
  }

  // Test clone
  {
    HeightField<BV> * hfield_clone = hfield.clone();
    hfield_clone->computeLocalAABB();
    BOOST_CHECK(*hfield_clone == hfield);

    delete hfield_clone;
  }

  // Build equivalent object
  const Box equivalent_box(x_dim,y_dim,max_altitude - min_altitude);
  const Transform3f box_placement(Matrix3f::Identity(),
                                  Vec3f(0.,0.,(max_altitude + min_altitude)/2.));

  // Test collision
  const Sphere sphere(1.);
  static const Transform3f IdTransform;

  const Box box(Vec3f::Ones());

  Transform3f M_sphere, M_box;

  // No collision case
  {
    const FCL_REAL eps_no_collision = +0.1*(max_altitude - min_altitude);
    M_sphere.setTranslation(Vec3f(0.,0.,max_altitude + sphere.radius + eps_no_collision));
    M_box.setTranslation(Vec3f(0.,0.,max_altitude + box.halfSide[2] + eps_no_collision));
    CollisionRequest request;

    CollisionResult result;
    collide(&hfield,IdTransform,
            &sphere,M_sphere,
            request,result);

    BOOST_CHECK(!result.isCollision());

    CollisionResult result_check_sphere;
    collide(&equivalent_box,IdTransform*box_placement,
            &sphere,M_sphere,
            request,result_check_sphere);

    BOOST_CHECK(!result_check_sphere.isCollision());

    CollisionResult result_check_box;
    collide(&equivalent_box,IdTransform*box_placement,
            &box,M_box,
            request,result_check_box);

    BOOST_CHECK(!result_check_box.isCollision());
  }

  // Collision case
  {
    const FCL_REAL eps_collision = -0.1*(max_altitude - min_altitude);
    M_sphere.setTranslation(Vec3f(0.,0.,max_altitude + sphere.radius + eps_collision));
    M_box.setTranslation(Vec3f(0.,0.,max_altitude + box.halfSide[2] + eps_collision));
    CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1)));

    CollisionResult result;
    collide(&hfield,IdTransform,
            &sphere,M_sphere,
            request,result);

    BOOST_CHECK(result.isCollision());

    CollisionResult result_check;
    collide(&equivalent_box,IdTransform*box_placement,
            &sphere,M_sphere,
            request,result_check);

    BOOST_CHECK(result_check.isCollision());

    CollisionResult result_check_box;
    collide(&equivalent_box,IdTransform*box_placement,
            &box,M_box,
            request,result_check_box);

    BOOST_CHECK(result_check_box.isCollision());
  }

  // Update height
  hfield.updateHeights(MatrixXf::Constant(ny, nx, max_altitude/2.)); // We change nothing

  // No collision case
  {
    const FCL_REAL eps_no_collision = +0.1*(max_altitude - min_altitude);
    M_sphere.setTranslation(Vec3f(0.,0.,max_altitude + sphere.radius + eps_no_collision));
    M_box.setTranslation(Vec3f(0.,0.,max_altitude + box.halfSide[2] + eps_no_collision));
    CollisionRequest request;

    CollisionResult result;
    collide(&hfield,IdTransform,
            &sphere,M_sphere,
            request,result);

    BOOST_CHECK(!result.isCollision());

    CollisionResult result_check_sphere;
    collide(&equivalent_box,IdTransform*box_placement,
            &sphere,M_sphere,
            request,result_check_sphere);

    BOOST_CHECK(!result_check_sphere.isCollision());

    CollisionResult result_check_box;
    collide(&equivalent_box,IdTransform*box_placement,
            &box,M_box,
            request,result_check_box);

    BOOST_CHECK(!result_check_box.isCollision());
  }

  // Collision case
  {
    const FCL_REAL eps_collision = -0.1*(max_altitude - min_altitude);
    M_sphere.setTranslation(Vec3f(0.,0.,max_altitude + sphere.radius + eps_collision));
    M_box.setTranslation(Vec3f(0.,0.,max_altitude + box.halfSide[2] + eps_collision));
    CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1)));

    CollisionResult result;
    collide(&hfield,IdTransform,
            &sphere,M_sphere,
            request,result);

    BOOST_CHECK(!result.isCollision());

    CollisionResult result_check;
    collide(&equivalent_box,IdTransform*box_placement,
            &sphere,M_sphere,
            request,result_check);

    BOOST_CHECK(result_check.isCollision());

    CollisionResult result_check_box;
    collide(&equivalent_box,IdTransform*box_placement,
            &box,M_box,
            request,result_check_box);

    BOOST_CHECK(result_check_box.isCollision());
  }

  // Restore height
  hfield.updateHeights(MatrixXf::Constant(ny, nx, max_altitude)); // We change nothing

  // Collision case
  {
    const FCL_REAL eps_collision = -0.1*(max_altitude - min_altitude);
    M_sphere.setTranslation(Vec3f(0.,0.,max_altitude + sphere.radius + eps_collision));
    M_box.setTranslation(Vec3f(0.,0.,max_altitude + box.halfSide[2] + eps_collision));
    CollisionRequest request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1)));

    CollisionResult result;
    collide(&hfield,IdTransform,
            &sphere,M_sphere,
            request,result);

    BOOST_CHECK(result.isCollision());

    CollisionResult result_check;
    collide(&equivalent_box,IdTransform*box_placement,
            &sphere,M_sphere,
            request,result_check);

    BOOST_CHECK(result_check.isCollision());

    CollisionResult result_check_box;
    collide(&equivalent_box,IdTransform*box_placement,
            &box,M_box,
            request,result_check_box);

    BOOST_CHECK(result_check_box.isCollision());
  }

}

BOOST_AUTO_TEST_CASE(building_constant_hfields)
{
  const FCL_REAL max_altitude = 1., min_altitude = 0.;

  test_constant_hfields<OBBRSS>(2,2,min_altitude,max_altitude); // Simple case
  test_constant_hfields<OBBRSS>(20,2,min_altitude,max_altitude);
  test_constant_hfields<OBBRSS>(100,100,min_altitude,max_altitude);
//  test_constant_hfields<OBBRSS>(1000,1000,min_altitude,max_altitude);

  test_constant_hfields<AABB>(2,2,min_altitude,max_altitude); // Simple case
  test_constant_hfields<AABB>(20,2,min_altitude,max_altitude);
  test_constant_hfields<AABB>(100,100,min_altitude,max_altitude);
}

BOOST_AUTO_TEST_CASE(hfield_with_square_hole)
{
  const Eigen::DenseIndex nx = 100, ny = 100;

  typedef AABB BV;
  const MatrixXf X = Eigen::RowVectorXd::LinSpaced(nx,-1.,1.).replicate(ny,1);
  const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny,1.,-1.).replicate(1,nx);

  const FCL_REAL dim_square = 0.5;

  const Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> hole = (X.array().abs() < dim_square) && (Y.array().abs() < dim_square);

  const MatrixXf heights = MatrixXf::Ones(ny,nx) - hole.cast<double>().matrix();

  const HeightField<BV> hfield(2.,2.,heights,-10.);

  Sphere sphere(0.48);
  const Transform3f sphere_pos(Vec3f(0.,0.,0.5));
  const Transform3f hfield_pos;

  const CollisionRequest request;

  CollisionResult result;
  {
    collide(&hfield,hfield_pos,
            &sphere,sphere_pos,
            request,result);

    BOOST_CHECK(!result.isCollision());
  }

  sphere.radius = 0.51;

  {
    CollisionResult result;
    const Sphere sphere2(0.51);
    collide(&hfield,hfield_pos,
            &sphere2,sphere_pos,
            request,result);

    BOOST_CHECK(result.isCollision());
  }
}


#define _REGEX_MAX_STACK_COUNT 5000
// #if defined(_MSC_VER)
// #include <boost/regex.hpp>
// using namespace boost;
// #else
#include <regex>
// #endif
#include <string>
#include <map>
#include <algorithm>
#include <iostream>
#include <fstream>
#include <vector>
#include "readreftable.hpp"
#include "tqdm.hpp"
#include <range/v3/all.hpp>

#include <stdio.h>
#ifdef __APPLE__
        #include <sys/uio.h>
#elif defined(_MSC_VER)
        #include <io.h>
#else
        #include <sys/io.h>
#endif

// using namespace boost;
// using namespace ranges;

template<class A, class B>
B readAndCast(ifstream& f) {
    A t;
    f.read(reinterpret_cast<char *>(&t),sizeof(A));
    return static_cast<B>(t);
}

using namespace std;

Reftable<MatrixXd> readreftable(string headerpath, string reftablepath, size_t N, bool quiet, std::string groups_opt) {
    ///////////////////////////////////////// read headers
    if (!quiet) cout << "///////////////////////////////////////// read headers" << endl;

    ifstream headerStream(headerpath,ios::in);
    if (headerStream.fail()){
        cout << "No header file, exiting" << endl;
        exit(1);
    }
    headerStream >> noskipws;
    const std::string hS(istream_iterator<char>{headerStream}, {});

    const regex scen_re(R"#(\bscenario\s+(\d+)\s+.*?\n((?:(?!(?:scenario|\n)).*?\n)+))#");
    sregex_token_iterator itscen(begin(hS), end(hS), scen_re, {1,2});
    //    print(itscen, {});

    auto nscenh = distance(itscen, {})/2;
    vector<string> scendesc(nscenh);
    sregex_token_iterator endregexp;

    auto it = itscen;
    while (it != endregexp) {
        const string num(*it++);
        const string desc(*it++);
        scendesc[stoi(num)-1] = desc;
    }
    //cout << scendesc[0];
    const regex nparamtot(R"#(historical parameters priors \((\d+)\D)#");
    smatch base_match;
    regex_search(begin(hS), end(hS),base_match,nparamtot);
    auto nparamtoth = stoi(base_match[1]);
//    cout << nparamtoth << endl;
    string reparamlistrestr = R"#(\bhistorical parameters priors.*?\n((?:\w+\W[^\n]*?\n){)#" + to_string(nparamtoth) + "})";
    const regex reparamlist(reparamlistrestr);
    smatch base_match2;
    regex_search(begin(hS), end(hS),base_match2,reparamlist);
    //cout << base_match[1] << endl;
    const string paramlistmatch = base_match2[1];
    const regex reparam(R"#((\w+)\W+\w\W+\w\w\[((?:\d|\.)+?)\W*,((?:\d|\.)+?)(?:,.+?)?\]?\n)#");
    sregex_token_iterator reparamit(begin(paramlistmatch),end(paramlistmatch), reparam, {1,2,3});
    it = reparamit;
    size_t reali = 1;
    map<string,size_t> paramdesc;
    while (it != endregexp) {
        const string param = *it++;
        const double mini = stod(*it++);
        const double maxi = stod(*it++);
        if (maxi != 0.0) {
            if ((maxi-mini)/maxi > 0.000001) {
                paramdesc[param] = reali;
                reali++;
            }
        }
    }
    size_t realparamtot = reali - 1;
    vector<vector<size_t>> parambyscenh(nscenh);
    const regex splitre(R"#(\W)#");
    for(auto i = 0; i < nscenh; i++) {
        sregex_token_iterator it(begin(scendesc[i]),end(scendesc[i]),splitre,-1);
        for(; it != endregexp; ++it) {
            const string term = *it;
            if (paramdesc.count(term) > 0) {
                const size_t nterm = paramdesc[term];
                if (find(parambyscenh[i].begin(),parambyscenh[i].end(),nterm)  == parambyscenh[i].end())
                    parambyscenh[i].push_back(nterm);
            }
        }
    }
    // for (auto scen : parambyscenh) {
    //     for(auto p : scen) cout << p << " ";
    //     cout << endl;
    // }
    smatch base_match3;
    const regex restatsname(R"#(\n\s*\nscenario\s+)#");
    regex_search(begin(hS), end(hS),base_match3,restatsname);
    const string allstatsname = base_match3.suffix();
    const regex splitre2(R"#(\s+)#");
    vector<string> allcolspre;
    for(sregex_token_iterator it(allstatsname.begin(),allstatsname.end(),splitre2,-1); it != endregexp; it++)
        allcolspre.push_back(*it);

    if (!quiet) cout << "read headers done." << endl;
    headerStream.close();
     ///////////////////////////////////////// read reftable
    if (!quiet) cout << "///////////////////////////////////////// read reftable" << endl;

    ifstream reftableStream(reftablepath,ios::in|ios::binary);
    if (reftableStream.fail()){
        cout << "No Reftable, exiting" << endl;
        exit(1);
    }
    size_t realnrec = readAndCast<int,size_t>(reftableStream);
    // reftableStream.read(reinterpret_cast<char *>(realnrec_i),sizeof(realnr));
    size_t nrec = N > 0 ? min(realnrec,N) : realnrec;
    size_t nscen = readAndCast<int,size_t>(reftableStream);
    vector<size_t> nrecscen(nscen);
    for(auto& r : nrecscen) r = readAndCast<int,size_t>(reftableStream);
    vector<size_t> nparam(nscen);
    for(auto& r : nparam) r = readAndCast<int,size_t>(reftableStream);
    size_t nstat = readAndCast<int,size_t>(reftableStream);
    vector<string> params_names { allcolspre.begin(), allcolspre.begin() + (allcolspre.size() - nstat) };
    vector<string> stats_names  { allcolspre.begin()+ (allcolspre.size() - nstat),  allcolspre.end() };
    vector<double> scenarios(nrec);
    size_t nmutparams = params_names.size() - realparamtot;
    MatrixXd params = MatrixXd::Constant(nrec,params_names.size(),NAN);
    // for(auto r : statsname) cout << r << endl;
    MatrixXd stats = MatrixXd::Constant(nrec,nstat,NAN);
    // DataDouble data(stats,statsname,nrec,nstat + 1);
    // bool hasError;
    std::vector<int> groups;
    if (groups_opt.length() > 0) {
        groups = std::vector<int>(nscen,0);
        auto groupstr = groups_opt
            | ranges::views::split(';')
            | ranges::views::transform([](auto&& s) { return s
                | ranges::views::split(',')
                | ranges::views::transform([](auto&& si){ return si | ranges::to<std::string>; });
                    }
                )
            | ranges::views::enumerate
            | ranges::to<std::vector>;
        for(auto&& s: groupstr)
            for(auto&& si: s.second)
                groups[std::stoi(si)-1] = static_cast<double>(s.first + 1);

        std::vector<size_t> new_nrecscen = std::vector<size_t>(groupstr.size());
        for(auto i = 0; i < nscen; i++)
            if (groups[i] > 0) new_nrecscen[groups[i] - 1] += nrecscen[i];
        nrecscen = new_nrecscen;
        // myread.scenarios |= actions::transform([&groups](auto&& si){ return groups[si-1]; });
    }
    size_t rcount = 0;
    tqdm bar;
    for(auto i = 0; i < nrec; i++) {
        bool to_skip = false;
        if (!quiet) {
            if (isatty(fileno(stdin)))
                bar.progress(i,nrec);
            else
                cout << "read " << (i + 1) << "/" << nrec << std::endl;
        }
        size_t scen = readAndCast<int,size_t>(reftableStream);
        if (groups.size() > 0) {
            if (groups[scen-1] == 0) to_skip = true;
            else scenarios[rcount] = static_cast<double>(groups[scen-1]);
        } else scenarios[rcount] = static_cast<double>(scen);
//        reftableStream.read(reinterpret_cast<char *>(&scen),4);

        // data.set(nstat,i,static_cast<double>(scen),hasError);
        scen--;
        vector<float> lparam(nparam[scen]);
        for(auto& r: lparam) {
            reftableStream.read(reinterpret_cast<char *>(&r),sizeof(r));
        }
        if (!to_skip) {
            for(auto j = 0; j < parambyscenh[scen].size(); j++)
                params(rcount,parambyscenh[scen][j] - 1) = lparam[j];
            for(auto j = nparam[scen] - nmutparams; j < nparam[scen]; j++)
                params(rcount,j) = lparam[j];
        }
        for(auto j = 0; j < nstat; j++) {
            float r;
            reftableStream.read(reinterpret_cast<char *>(&r),4);
            // col * num_rows + row
            if (!to_skip) stats(rcount,j) = r;
            // data.set(j,i,r,hasError);
        }
        if (!to_skip) rcount++;
    }
    if (rcount < nrec) {
        stats.conservativeResize(rcount,NoChange);
        params.conservativeResize(rcount,NoChange);
        scenarios.resize(rcount);
    }
    reftableStream.close();
    if (!quiet) cout << endl << "read reftable done." << endl;
    Reftable reftable(rcount,nrecscen, nparam, params_names, stats_names, stats,params, scenarios);
    return reftable;
}

#define READSCEN_BUFFER_SIZE 1000
Reftable<MatrixXd> readreftable_scen(string headerpath, string reftablepath, size_t sel_scen, size_t N, bool quiet) {
    ///////////////////////////////////////// read headers
    if (!quiet) cout << "///////////////////////////////////////// read headers" << endl;

    ifstream headerStream(headerpath,ios::in);
    if (headerStream.fail()){
        cout << "No header file, exiting" << endl;
        exit(1);
    }
    headerStream >> noskipws;
    const std::string hS(istream_iterator<char>{headerStream}, {});

    const regex scen_re(R"#(\bscenario\s+(\d+)\s+.*?\n((?:(?!(?:scenario|\n)).*?\n)+))#");
    sregex_token_iterator itscen(begin(hS), end(hS), scen_re, {1,2});
    //    print(itscen, {});

    auto nscenh = distance(itscen, {})/2;
    vector<string> scendesc(nscenh);
    sregex_token_iterator endregexp;

    auto it = itscen;
    while (it != endregexp) {
        const string num(*it++);
        const string desc(*it++);
        scendesc[stoi(num)-1] = desc;
    }
    //cout << scendesc[0];
    const regex nparamtot(R"#(historical parameters priors \((\d+)\D)#");
    smatch base_match;
    regex_search(begin(hS), end(hS),base_match,nparamtot);
    auto nparamtoth = stoi(base_match[1]);
//    cout << nparamtoth << endl;
    string reparamlistrestr = R"#(\bhistorical parameters priors.*?\n((?:\w+\W[^\n]*?\n){)#" + to_string(nparamtoth) + "})";
    const regex reparamlist(reparamlistrestr);
    smatch base_match2;
    regex_search(begin(hS), end(hS),base_match2,reparamlist);
    //cout << base_match[1] << endl;
    const string paramlistmatch = base_match2[1];
    const regex reparam(R"#((\w+)\W+\w\W+\w\w\[((?:\d|\.)+?)\W*,((?:\d|\.)+?)(?:,.+?)?\]?\n)#");
    sregex_token_iterator reparamit(begin(paramlistmatch),end(paramlistmatch), reparam, {1,2,3});
    it = reparamit;
    size_t reali = 1;
    map<string,size_t> paramdesc;
    while (it != endregexp) {
        const string param = *it++;
        const double mini = stod(*it++);
        const double maxi = stod(*it++);
        if (maxi != 0.0) {
            if ((maxi-mini)/maxi > 0.000001) {
                paramdesc[param] = reali;
                reali++;
            }
        }
    }
    size_t realparamtot = reali - 1;
    vector<vector<size_t>> parambyscenh(nscenh);
    const regex splitre(R"#(\W)#");
    for(auto i = 0; i < nscenh; i++) {
        sregex_token_iterator it(begin(scendesc[i]),end(scendesc[i]),splitre,-1);
        for(; it != endregexp; ++it) {
            const string term = *it;
            if (paramdesc.count(term) > 0) {
                const size_t nterm = paramdesc[term];
                if (find(parambyscenh[i].begin(),parambyscenh[i].end(),nterm)  == parambyscenh[i].end())
                    parambyscenh[i].push_back(nterm);
            }
        }
    }
    // for (auto scen : parambyscenh) {
    //     for(auto p : scen) cout << p << " ";
    //     cout << endl;
    // }
    smatch base_match3;
    const regex restatsname(R"#(\n\s*\nscenario\s+)#");
    regex_search(begin(hS), end(hS),base_match3,restatsname);
    const string allstatsname = base_match3.suffix();
    const regex splitre2(R"#(\s+)#");
    vector<string> allcolspre;
    for(sregex_token_iterator it(allstatsname.begin(),allstatsname.end(),splitre2,-1); it != endregexp; it++)
        allcolspre.push_back(*it);

    if (!quiet) cout << "read headers done." << endl;
    headerStream.close();
     ///////////////////////////////////////// read reftable
    if (!quiet) cout << "///////////////////////////////////////// read reftable" << endl;

    ifstream reftableStream(reftablepath,ios::in|ios::binary);
    if (reftableStream.fail()){
        cout << "No Reftable, exiting" << endl;
        exit(1);
    }
    size_t realnrec = readAndCast<int,size_t>(reftableStream);
    // reftableStream.read(reinterpret_cast<char *>(realnrec_i),sizeof(realnr));
    if (N == 0) N = realnrec;
    size_t nrec = realnrec;
    size_t nscen = readAndCast<int,size_t>(reftableStream);
    vector<size_t> nrecscen(nscen);
    for(auto& r : nrecscen) r = readAndCast<int,size_t>(reftableStream);
    vector<size_t> nparam(nscen);
    for(auto& r : nparam) r = readAndCast<int,size_t>(reftableStream);
    size_t nstat = readAndCast<int,size_t>(reftableStream);
    vector<string> params_names { allcolspre.begin(), allcolspre.begin() + (allcolspre.size() - nstat) };
    vector<string> stats_names  { allcolspre.begin()+ (allcolspre.size() - nstat),  allcolspre.end() };
    vector<double> scenarios(nrec);
    size_t nmutparams = params_names.size() - realparamtot;
    MatrixXd params = MatrixXd::Constant(0,params_names.size(),NAN);
    // for(auto r : statsname) cout << r << endl;
    MatrixXd stats = MatrixXd::Constant(0,nstat,NAN);
    // DataDouble data(stats,statsname,nrec,nstat + 1);
    // bool hasError;
    size_t ncount = 0;
    tqdm bar;
    for(auto i = 0; (i < nrec) && (ncount < N); i++) {
        if (!quiet) {
            if (isatty(fileno(stdin)))
                bar.progress(i,nrec);
            else
                cout << "read " << (i + 1) << "/" << nrec << std::endl;
        }
        size_t scen = readAndCast<int,size_t>(reftableStream);
//        reftableStream.read(reinterpret_cast<char *>(&scen),4);
        bool matched = (scen == sel_scen);
        if (matched && ncount == stats.rows()) {
            stats.conservativeResize(stats.rows() + READSCEN_BUFFER_SIZE,NoChange);
            params.conservativeResize(params.rows() + READSCEN_BUFFER_SIZE,NoChange);
        }
        scenarios[i] = static_cast<double>(scen);
        // data.set(nstat,i,static_cast<double>(scen),hasError);
        scen--;
        vector<float> lparam(nparam[scen]);
        for(auto& r: lparam) {
            reftableStream.read(reinterpret_cast<char *>(&r),sizeof(r));
        }
        if (matched) {
            for(auto j = 0; j < parambyscenh[scen].size(); j++)
                params(ncount,parambyscenh[scen][j] - 1) = lparam[j];
            for(auto j = nparam[scen] - nmutparams; j < nparam[scen]; j++)
                params(ncount,j) = lparam[j];
        }
        for(auto j = 0; j < nstat; j++) {
            float r;
            reftableStream.read(reinterpret_cast<char *>(&r),4);
            // col * num_rows + row
            if (matched) stats(ncount,j) = r;
            // data.set(j,i,r,hasError);
        }
        if (matched) ncount++;
    }

    reftableStream.close();
    if (!quiet) cout << endl << "read reftable done. " << ncount << " samples of total " << nrec << ", " << N << " asked and " << nrecscen[sel_scen-1] << " avalaible." << endl;
    if (!quiet && (N > nrecscen[sel_scen-1])) cout << "Warning : asked for more samples than available." << endl;
    stats.conservativeResize(ncount,NoChange);
    params.conservativeResize(ncount,NoChange);
   std::vector<size_t> uniqrec  = { nrecscen[sel_scen-1] };
    Reftable reftable(ncount,uniqrec, nparam, params_names, stats_names, stats,params, scenarios);
    return reftable;
}


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

double exp(double x) // the functor we want to apply
{
    std::setprecision(5);
        return std::trunc(x);
}

void MatrixPowersPolynomials()
{

    //Rectangular Diagonal
    // rectangular diagonal  matrix is an n × d matrix in which each entry (i, j) has a non-zero value if and  only if i = j.
    // diagonal matrix is a matrix in which the entries outside the main diagonal are all zero;

    //A block diagonal matrix contains square blocks B 1…B r of (possibly) nonzero entries along the diagonal. All other entries are zero. Although each
    //block is square, they need not be of the same size.

    //Upper and Lower Triangular Matrix) A square matrix is an upper triangular matrix if all entries (i, j) below its main diagonal (i.e.,
    //satisfying i > j) are zeros.

    // The product of uppertriangular matrices is upper triangular.
    //c(i,j)=0 if i>j c(i,j)=sum(a(i,k)*b(k,j))
    //i>j
    //if i>k a(i,k)=0
    //if i<k -> j<k -> b(k,j)=0

    //Inverse of Triangular Matrix Is Triangula

    //Strictly Triangular Matrix) A matrix is said to be strictlytriangular if it is triangularandall its diagonal elements are zeros.

    //The zeroth power of a matrix is defined to be the identity matrix
    //When a matrix satisfies A^k = 0 for some integer k, it is referred to as nilpotent.
    //all strictly triangular matrices (triangular with zero main diagonal)of size d × d satisfy A^d = 0.
    //product of two upper triangular is a triangular matrix

    //polynomial function f(A) of a square matrix in  much the same way as one computes polynomials of scalars.
    //f(x) = 3x^2 + 5x + 2 -> f(A) = 3A^2 + 5A + 2
    //Two    polynomials f(A) and g(A) of the same matrix A will always commute f(A)g(A)=g(A)f(A)

    //Commutativity of Multiplication with Inverse if AB=I then BA=I
    //When the inverse of a matrix exists, it is always unique
    //Inverse of Triangular Matrix Is Triangular
    //Inv(A^n)=Inv(A)^n

    //An orthogonal matrix is a square matrix whose inverse is its transpose: A*A^T=A^T*A=I
    //all column columns/ rows are perpendicular

    //The multiplication of an n × d matrix A with a d-dimensional column
    //vector to create an n-dimensional column vector is often interpreted as
    //a linear transformation from d-dimensional space to n-dimensional space
/*
    a11   a12         a11      a12
    a21   a22  x1 =x1 a21 + x2 a22
    a31   a32  x2     a31      a32

Therefore, the n × d matrix A is occasionally represented in terms of its ordered set of ndimensional columns
A nxd= [a1 a2 ... ad]

 low-rank update
 Linear regression least-squares classification, support-vector machines, and logistic regression
 Matrix factorization is an alternative term for matrix decomposition
 recommender systems

Regression
the only difference from classification is that the array contains numerical values (rather than categorical ones)
The dependent
variable is also referred to as a response variable, target variable, or regressand in the case of regression
The independent variables are also referred to as regressors.more than two classes like {Red,
Green, Blue} cannot be ordered, and are therefore different from regression

convex objective functions like linear regression

https://medium.com/@wisnutandaseru/proving-eulers-identity-using-taylor-series-2771089cd780
Householder Reflections
Directional derivative
chain rule for multivariale derivative
multivariant fourier transform

inverse of I+A?
https://math.stackexchange.com/questions/298616/what-is-inverse-of-ia/298623
https://en.wikipedia.org/wiki/Woodbury_matrix_identity

https://en.m.wikipedia.org/wiki/Computational_complexity_of_mathematical_operations#Matrix_algebra
*/

}

int main(int argc, char *argv[])
{
    return 0;
}


#include <boost/hana/fold_right.hpp>


// -*-C++-*--------------------------------------------------------------------
// Copyright Bernd Pfrommer
// $Id$

#include "tagslam/profiler.h"
#include <math.h>
#include <limits>
#include <iostream>
#include <boost/foreach.hpp>
#include <map>
#include <iomanip>

using namespace boost::chrono;
using namespace std;

namespace tagslam {
  std::ostream &
  operator<<(std::ostream& os, const Profiler &p)  {
    std::multimap<int64_t, const char *> avgToStr;
    size_t maxlen(0);
    for (const Profiler::ProfilerMap::value_type &v: p.map_) {
      const Profiler::PTimer &pt = v.second.timer;
      int64_t dn = (pt.count > 0) ? (pt.duration / pt.count).count() : -1;
      avgToStr.insert(std::pair<int64_t, const char *>(dn, v.first));
      maxlen = std::max(maxlen, strlen(v.first));
    }

    for (const auto &mi: avgToStr) {
      Profiler::ProfilerMap::const_iterator it = p.map_.find(mi.second);
      if (it == p.map_.end()) {
        std::cout << "ERROR: cannot find key: " << mi.second << std::endl;
        continue;
      }
      const Profiler::PTimer &pt = it->second.timer;
      int64_t dn = (pt.count > 0) ? (pt.duration / pt.count).count() : -1;
      int64_t sumdelta = pt.duration.count();
      // too much roundoff error: int64_t sqdn = (pt.count > 0) ? ((pt.sqduration / pt.count) - dn * dn): 0;
      int64_t sqdn = (pt.count > 0) ? ((pt.sqduration - (sumdelta * sumdelta) / pt.count) / pt.count): 0;
      float stddev = sqrt((double) sqdn);
      int64_t dmin = (pt.count > 0) ? pt.min.count() : -1;
      int64_t dmax = (pt.count > 0) ? pt.max.count() : -1;
      os << std::setw(maxlen + 1) << std::left << it->first << "= tot: "
         << std::setw(9) << std::right << pt.duration.count() << "us per: "
         << dn << "+-" << (int)stddev << "(" << dmin << "-" << dmax << ")"
         << " count: " << pt.count << " " << std::endl;
    }
    return os;
  }

  Profiler::PTimer::PTimer(const Duration &d, const PTimer &oldTimer, int ncount) {
    duration = oldTimer.duration + d;
    sqduration = oldTimer.sqduration + (int64_t)(d.count() * d.count()) / (int64_t) ncount;
    Duration dn = d / ncount;
    min = (dn <  oldTimer.min) ? dn : oldTimer.min;
    max = (dn >  oldTimer.max) ? dn : oldTimer.max;
    count = oldTimer.count + ncount;
  }

  Profiler::PTimer::PTimer() :
    duration(0), sqduration(0),
    min(std::numeric_limits<int64_t>::max()),
    max(std::numeric_limits<int64_t>::min()), count(0) {
  }

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MAT_MAT_TIMES_EXPR_INCLUDE
#define MTL_MAT_MAT_TIMES_EXPR_INCLUDE

#include <boost/mpl/if.hpp>
#include <boost/mpl/and.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/type_traits/is_base_of.hpp>

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/matrix/mat_mat_op_expr.hpp>
#include <boost/numeric/mtl/operation/sfunctor.hpp>
#include <boost/numeric/mtl/operation/compute_factors.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/concept/std_concept.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>

namespace mtl { namespace mat {

template <typename E1, typename E2>
struct mat_mat_times_expr
  : public mat_mat_op_expr< E1, E2, mtl::sfunctor::times<typename Collection<E1>::value_type, typename Collection<E2>::value_type> >,
    public mat_expr< mat_mat_times_expr<E1, E2> >
{
    typedef mat_mat_op_expr< E1, E2, mtl::sfunctor::times<typename Collection<E1>::value_type, typename Collection<E2>::value_type> > op_base;
    typedef mat_expr< mat_mat_times_expr<E1, E2> >                                                       crtp_base;
    typedef mat_mat_times_expr                   self;
    typedef E1                                   first_argument_type ;
    typedef E2                                   second_argument_type ;
    typedef typename E1::orientation             orientation;
    typedef mtl::non_fixed::dimensions           dim_type;
    typedef typename E1::key_type                key_type;

    typedef typename Collection<E1>::value_type  first_value_type;
    typedef typename Collection<E2>::value_type  second_value_type;
    typedef typename Multiplicable<first_value_type, second_value_type>::result_type result_value_type;

#if 0 // Just an idea
    typedef typename boost::mpl::if_<
                boost::mpl::and_<
	            boost::is_base_of<tag::sparse, typename traits::category<E1>::type>
	          , boost::is_base_of<tag::sparse, typename traits::category<E2>::type>
		>
	      , compressed2D<result_value_type>
	      , dense2D<result_value_type, parameters<> >
            >::type                                       evaluated_result_type;

    // Convert into matrix

    operator evaluated_result_type() const
    {
	return evaluated_result_type(first * second);
    }
#endif

    mat_mat_times_expr( E1 const& v1, E2 const& v2 )
      : op_base( v1, v2 ), crtp_base(*this), first(v1), second(v2)
    {}

    // To prevent that cout << A * B prints the element-wise product, suggestion by Hui Li
    // It is rather inefficient, esp. for multiple products (complexity increases with the number of arguments :-!)
    //    or sparse matrices.
    // Better compute your product first and print it then when compute time is an issue,
    // this is ONLY for convenience.
    result_value_type
    operator()(std::size_t r, std::size_t c) const
    {
	using math::zero;
	MTL_THROW_IF(num_cols(first) != num_rows(second), incompatible_size());

	result_value_type ref, sum(zero(ref));
	for (std::size_t i= 0; i < num_cols(first); i++)
	    sum+= first(r, i) * second(i, c);
	return sum;
    }

    result_value_type
    operator()(std::size_t r, std::size_t c)
    {
	return (*const_cast<const self*>(this))(r, c);
    }

    first_argument_type const&  first ;
    second_argument_type const& second ;
};

template <typename E1, typename E2>
std::size_t inline num_rows(const mat_mat_times_expr<E1, E2>& expr)
{ return num_rows(expr.first); }

template <typename E1, typename E2>
std::size_t inline num_cols(const mat_mat_times_expr<E1, E2>& expr)
{ return num_cols(expr.second); }

template <typename E1, typename E2>
std::size_t inline size(const mat_mat_times_expr<E1, E2>& expr)
{ return num_rows(expr) * num_cols(expr); }

}} // Namespace mtl::matrix

#endif // MTL_MAT_MAT_TIMES_EXPR_INCLUDE


// ROS
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Pose.h>

// KDL
#include <kdl/kdl.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/jacobian.hpp>
#include <armadillo>

// Useful defines
#define DQ_MAX 360*M_PI/180       // maximum joint angular velocity

// Reminder: declare global variables here as 'extern' instead of including the header -- that would result in
// multiple declaration errors when linking
extern int num_joints_arm;
extern std::vector<std_msgs::Float64> left_command;
extern std::vector<std_msgs::Float64> right_command;
extern std::vector<ros::Publisher> left_controller_pub;
extern std::vector<ros::Publisher> right_controller_pub;
extern bool g_quit;
extern sensor_msgs::JointState joints_state;
extern std::vector<std_msgs::Float64> left_joint_pos;
extern std::vector<std_msgs::Float64> right_joint_pos;
extern std::string joint_solver;        // to choose what IK solver to use or joint space tracking instead
extern std::string control_mode;        // position or velocity
extern KDL::Vector desired_pos_left;
extern KDL::Vector desired_pos_right;
extern KDL::Rotation rot_left_desired;		// the object is initialized with identity
extern KDL::Rotation rot_right_desired;
extern geometry_msgs::Pose left_hand_desired_pose;
extern geometry_msgs::Pose right_hand_desired_pose;

using namespace std;

void publish_commands()
{
	// The urdf model has joint 7 right after joint 2 and the rest shifted forward
	for(uint i=0;i<2;i++)
	{
		left_controller_pub.at(i).publish(left_command.at(i));
		right_controller_pub.at(i).publish(right_command.at(i));
	}
	for(uint i=2;i<num_joints_arm-1;i++)
	{
		left_controller_pub.at(i).publish(left_command.at(i+1));
		right_controller_pub.at(i).publish(right_command.at(i+1));
	}
	left_controller_pub.at(num_joints_arm-1).publish(left_command.at(2));
	right_controller_pub.at(num_joints_arm-1).publish(right_command.at(2));
}

void rearrange_YuMi_joints(std::vector<std_msgs::Float64>& joint_pos)
{
    double tmp_joint;
    tmp_joint = joint_pos[6].data;
    for(uint i=6;i>2;i--)
        joint_pos[i].data = joint_pos[i-1].data;
    joint_pos[2].data = tmp_joint;
}

void pose_msg_to_KDL(const geometry_msgs::Pose& pose_msg, KDL::Vector& pos, KDL::Rotation& rot)
{
    pos(0) = pose_msg.position.x;
    pos(1) = pose_msg.position.y;
    pos(2) = pose_msg.position.z;
    rot = KDL::Rotation::Quaternion(pose_msg.orientation.x, pose_msg.orientation.y, pose_msg.orientation.z, pose_msg.orientation.w);
    //std::cout<<"new pose set"<<std::endl;
}

// -- Subscriber callbacks
void joint_states_callback(const sensor_msgs::JointState &msg)
{
    /*joints_state.name = msg.name;
    joints_state.position = msg.position;
    joints_state.velocity = msg.velocity;
    joints_state.effort = msg.effort;*/

    for(uint i=0;i<num_joints_arm;i++)
    {
        /*left_joint_pos[i].data = joints_state.position[2*(i+1)];
        right_joint_pos[i].data = joints_state.position[2*(i+1)+1];*/
        left_joint_pos[i].data = msg.position[2*(i+1)];
        right_joint_pos[i].data = msg.position[2*(i+1)+1];
    }

    rearrange_YuMi_joints(left_joint_pos);
    rearrange_YuMi_joints(right_joint_pos);
}

void joint_solver_callback(const std_msgs::String& msg)
{
    joint_solver = msg.data;
    cout << "Switched control mode to "<< joint_solver;
}

void left_pose_callback(const geometry_msgs::Pose& pose_msg)
{
    //ROS_INFO("Left Hand pose update!");
    pose_msg_to_KDL(pose_msg, desired_pos_left, rot_left_desired);
}

void right_pose_callback(const geometry_msgs::Pose& pose_msg)
{
    //ROS_INFO("Right Hand pose update!");
    pose_msg_to_KDL(pose_msg, desired_pos_right, rot_right_desired);
}

// -- Printing stuff to stdout
void print_joint_values(const KDL::JntArray& q)
{
	cout << "[";
	for(uint i=0;i<num_joints_arm;i++)
		cout << q(i) << " ";
	cout << "]" << endl;
}

void print_joint_values(const arma::vec& q)
{
	cout << "[";
	for(uint i=0;i<num_joints_arm;i++)
		cout << q(i) << " ";
	cout << "]" << endl;
}

void print_Jacobian(KDL::Jacobian J)
{
	for(uint i=0;i<J.rows();i++)
	{
		for(uint j=0;j<J.columns();j++)
			cout << " " << J(i,j);
		cout << endl;
	}
	cout << endl;
}

void quitRequested(int sig)
{
	g_quit = true;
	for(uint i=0;i<num_joints_arm;i++)
	{
        if(control_mode=="position")
        {
            left_command.at(i).data = left_joint_pos[i].data;
            right_command.at(i).data = right_joint_pos[i].data;
        }
        else if(control_mode=="velocity")
        {
            left_command.at(i).data = 0.0;
            right_command.at(i).data = 0.0;
        }
	}
	publish_commands();
	ros::shutdown();
}

void saturate(KDL::JntArray& dq)
{
    for(uint i=0; i<num_joints_arm ;i++)
    {
        //cout << i << " " << dq(i) << endl;
        dq(i) = fabs(dq(i)) > DQ_MAX ? (dq(i) > 0 ? DQ_MAX : -DQ_MAX) : dq(i);
    }
}

void update_JntArray(KDL::JntArray& l_arm, KDL::JntArray& r_arm)
{
	for(int i=0;i<num_joints_arm;i++){
		l_arm(i) = left_joint_pos[i].data;
		r_arm(i) = right_joint_pos[i].data;
	}
}

void KDL_to_arma(const KDL::Jacobian& KDL_jac, arma::mat& arma_jac)
{
	for(unsigned int i = 0; i<KDL_jac.rows(); i++){
		for(unsigned int j = 0; j<KDL_jac.columns(); j++){
			arma_jac(i,j) = KDL_jac(i,j);
		}
	}
}

void KDL_to_arma(const KDL::Twist& KDL_twist, arma::vec& arma_twist)
{
	for(unsigned int i=0; i<6; i++)
		arma_twist(i) = KDL_twist(i);
}

void arma_to_KDL(const arma::vec& arma_jnt, KDL::JntArray& jnt_q)
{
	for(unsigned int i=0; i<num_joints_arm; i++)
		jnt_q(i) = arma_jnt(i);
}


//////////////////////////////////////////////////////////////////////////////
// data::field::estimation::fields::include.hpp								//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DATA_FIELD_ESTIMATION_FIELDS_INCLUDE_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_DATA_FIELD_ESTIMATION_FIELDS_INCLUDE_HPP_ER_2010

#include <boost/statistics/detail/data/field/estimation/fields/estimate.hpp>
#include <boost/statistics/detail/data/field/estimation/fields/input.hpp>
#include <boost/statistics/detail/data/field/estimation/fields/output.hpp>

#endif


//----------------------------------*-C++-*----------------------------------//
/**
 *  @file   Mesh3D.hh
 *  @author Jeremy Roberts
 *  @date   Mar 23, 2012
 */
//---------------------------------------------------------------------------//

#ifndef detran_geometry_MESH3D_HH_
#define detran_geometry_MESH3D_HH_

#include "Mesh.hh"
#ifdef DETRAN_ENABLE_BOOST
#include <boost/serialization/access.hpp>
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/export.hpp>
#endif

namespace detran_geometry
{

//---------------------------------------------------------------------------//
/**
 *  @class Mesh3D
 *  @brief Three-dimensional Cartesian mesh.
 *
 *  This is mostly a convenience interface.
 */
//---------------------------------------------------------------------------//
class GEOMETRY_EXPORT Mesh3D : public Mesh
{

public:

  typedef Mesh            Base;
  typedef Base::SP_mesh   SP_mesh;

  /**
   *  @brief Constructor.
   *
   *  @param    xfm         Fine meshes per coarse mesh in x dimension.
   *  @param    yfm         Fine meshes per coarse mesh in y dimension.
   *  @param    zfm         Fine meshes per coarse mesh in z dimension.
   *  @param    xcme        Coarse mesh edges x dimension.
   *  @param    ycme        Coarse mesh edges y dimension.
   *  @param    zcme        Coarse mesh edges z dimension.
   *  @param    mat_map     Coarse mesh material map.
   */
  Mesh3D(vec_int xfm,  vec_int yfm,  vec_int zfm,
         vec_dbl xcme, vec_dbl ycme, vec_dbl zcme,
         vec_int mat_map);

  /**
   *  @brief Constructor.
   *
   *  @param    xfme        Fine mesh edges x dimension.
   *  @param    yfme        Fine mesh edges y dimension.
   *  @param    zfme        Fine mesh edges z dimension.
   *  @param    mat_map     Fine mesh material map.
   */
   Mesh3D(vec_dbl xfme, vec_dbl yfme, vec_dbl zfme, vec_int mat_map);

  /**
   *  @brief SP Constructor.
   *
   *  @param    xfm         Fine meshes per coarse mesh in x dimension.
   *  @param    yfm         Fine meshes per coarse mesh in y dimension.
   *  @param    zfm         Fine meshes per coarse mesh in z dimension.
   *  @param    xcme        Coarse mesh edges x dimension.
   *  @param    ycme        Coarse mesh edges y dimension.
   *  @param    zcme        Coarse mesh edges z dimension.
   *  @param    mat_map     Coarse mesh material map.
   */
  static SP_mesh
  Create(vec_int xfm, vec_int yfm, vec_int zfm, vec_dbl xcme, vec_dbl ycme,
         vec_dbl zcme, vec_int mat_map)
  {
    SP_mesh p;
    p = new Mesh3D(xfm, yfm, zfm, xcme, ycme, zcme, mat_map);
    return p;
  }

  /**
   *  @brief SP Constructor.
   *
   *  @param    xfme        Fine mesh edges x dimension.
   *  @param    yfme        Fine mesh edges y dimension.
   *  @param    zfme        Fine mesh edges z dimension.
   *  @param    mat_map     Fine mesh material map.
   */
  static SP_mesh
  Create(vec_dbl xfme, vec_dbl yfme, vec_dbl zfme, vec_int mat_map)
  {
    SP_mesh p;
    p = new Mesh3D(xfme, yfme, zfme, mat_map);
    return p;
  }

protected:

  /**
   *   We keep this as an option in the event inherited meshes need
   *   more flexibility.
   */
  Mesh3D() : Mesh(3) {}

private:

#ifdef DETRAN_ENABLE_BOOST
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int version)
  {
    ar & boost::serialization::base_object<Base>(*this);
  }
#endif

};

} // end namespace detran_geometry

#ifdef DETRAN_ENABLE_BOOST
BOOST_CLASS_EXPORT_KEY(detran_geometry::Mesh3D)
#endif

#endif /* detran_geometry_MESH3D_HH_ */

//---------------------------------------------------------------------------//
//              end of Mesh3D.hh
//---------------------------------------------------------------------------//


#include <graphlab/util/hopscotch_table.hpp>
#include <graphlab/util/hopscotch_map.hpp>
#include <graphlab/util/cuckoo_map_pow2.hpp>
#include <boost/unordered_set.hpp>
#include <boost/bind.hpp>
#include <graphlab/logger/assertions.hpp>
#include <graphlab/parallel/pthread_tools.hpp>

#include <graphlab/util/memory_info.hpp>
#include <graphlab/macros_def.hpp>

const size_t NINS = 1500000;
boost::unordered_set<uint32_t> um;
graphlab::hopscotch_table<uint32_t> cm(1.2 * NINS);

void parallel_inserter(size_t start, size_t end) {
  for (size_t i = start; i < end; ++i) {
    assert(cm.put_sync(17 * i));
  }
}

void parallel_erase(size_t start, size_t end) {
  for (size_t i = start; i < end; i+=2) {
    assert(cm.erase_sync(17 * i));
    assert(cm.put_sync(17 * i));
    assert(cm.get_sync(17 * i).first);
    assert(cm.erase_sync(17 * i));
    assert(cm.get_sync(17 * i).first == false);
    assert(cm.put_sync(17 * i));
    assert(cm.get_sync(17 * i).first);
    assert(cm.erase_sync(17 * i));
    assert(cm.get_sync(17 * i).first == false);
    assert(cm.put_sync(17 * i));
    assert(cm.get_sync(17 * i).first);
    assert(cm.erase_sync(17 * i));
    assert(cm.get_sync(17 * i).first == false);
  }
}

void sanity_checks() {
 ASSERT_TRUE(cm.begin() == cm.end());

  for (size_t i = 0;i < NINS; ++i) {
    um.insert(17 * i);
  }

  graphlab::thread_group thrgroup;
  for (size_t i = 0; i < 10; ++i) {
    thrgroup.launch(boost::bind(parallel_inserter,
                                i * NINS/10,
                                (i + 1) * NINS / 10));

  }

  thrgroup.join();

  std::cout << "Size: " << cm.size() << std::endl;
  std::cout << "Capacity: " << cm.capacity() << std::endl;
  std::cout << "Load Factor: " << cm.load_factor() << std::endl;

  for (size_t i = 0;i < NINS; ++i) {
    if (cm.get_sync(17 * i).first == false) {
      std::cout << cm.count(17 * i) << "\n";
      std::cout << "Failure on: " << 17 * i << std::endl;
      assert(cm.get_sync(17 * i).first == true);
    }
    assert(um.count(17 * i) == 1);
  }
  assert(cm.size() == NINS);
  assert(um.size() == NINS);

  for (size_t i = 0;i < NINS; i+=2) {
    um.erase(17*i);
  }

  for (size_t i = 0; i < 10; ++i) {
    thrgroup.launch(boost::bind(parallel_erase,
                                i * NINS/10,
                                (i + 1) * NINS / 10));
  }

  thrgroup.join();

  for (size_t i = 0;i < NINS; i+=2) {
    assert(cm.get_sync(17*i).first == (bool)(i % 2));
    assert(um.count(17*i) == i % 2);
  }

  assert(cm.size() == NINS / 2);
  assert(um.size() == NINS / 2);
}

boost::unordered_map<uint32_t, uint32_t> um2;
graphlab::hopscotch_map<uint32_t, uint32_t> cm2;

void hopscotch_map_sanity_checks() {
  ASSERT_TRUE(cm2.begin() == cm2.end());
  for (size_t i = 0;i < NINS; ++i) {
    cm2[17 * i] = i;
    um2[17 * i] = i;
  }

  for (size_t i = 0;i < NINS; ++i) {
    assert(cm2[17 * i] == i);
    assert(um2[17 * i] == i);
  }
  assert(cm2.size() == NINS);
  assert(um2.size() == NINS);

  for (size_t i = 0;i < NINS; i+=2) {
    cm2.erase(17*i);
    um2.erase(17*i);
  }
  for (size_t i = 0;i < NINS; i+=2) {
    assert(cm2.count(17*i) == i % 2);
    assert(um2.count(17*i) == i % 2);
    if (cm2.count(17*i)) {
      assert(cm2.find(17*i)->second == i);
    }
  }

  assert(cm2.size() == NINS / 2);
  assert(um2.size() == NINS / 2);

  typedef graphlab::hopscotch_map<uint32_t, uint32_t>::value_type vpair;
  {
    size_t cnt = 0;
    foreach(vpair &v, cm2) {
      ASSERT_EQ(v.second, um2[v.first]);
      ++cnt;
    }
    ASSERT_EQ(cnt, NINS / 2);
  }
  {
    size_t cnt = 0;
    foreach(const vpair &v, cm2) {
      ASSERT_EQ(v.second, um2[v.first]);
      ++cnt;
    }
    ASSERT_EQ(cnt, NINS / 2);
  }

  std::stringstream strm;
  graphlab::oarchive oarc(strm);
  oarc << cm2;
  strm.flush();

  cm2.clear();
  ASSERT_EQ(cm2.size(), 0);
  graphlab::iarchive iarc(strm);
  iarc >> cm2;
  ASSERT_EQ(cm2.size(), NINS / 2);

}

void parallel_map_inserter(graphlab::hopscotch_map<uint32_t, uint32_t>* cm,
                        std::vector<uint32_t>* v,
                       size_t start, size_t end) {
  for (size_t i = start; i < end; ++i) {
    cm->put_sync((*v)[i], i);
  }
}

void benchmark() {
  graphlab::timer ti;

  size_t NUM_ELS = 10000000;

  std::vector<uint32_t> v;
  uint32_t u = 0;
  for (size_t i = 0;i < NUM_ELS; ++i) {
    v.push_back(u);
    u += 1 + rand() % 8;
  }
  std::random_shuffle(v.begin(), v.end());
  graphlab::memory_info::print_usage();

  {
    boost::unordered_map<uint32_t, uint32_t> um;
    ti.start();
    for (size_t i = 0;i < NUM_ELS; ++i) {
      um[v[i]] = i;
    }
    std::cout <<  NUM_ELS / 1000000 << "M unordered map inserts in " << ti.current_time() << " (Load factor = " << um.load_factor() << ")" << std::endl;

    graphlab::memory_info::print_usage();

    ti.start();
    for (size_t i = 0;i < 10000000; ++i) {
      size_t t = um[v[i]];
      assert(t == i);
    }
    std::cout << "10M unordered map successful probes in " << ti.current_time() << std::endl;
    um.clear();
  }

  {
    graphlab::cuckoo_map_pow2<uint32_t, uint32_t, 3, uint32_t> cm(-1, 128);

    //cm.reserve(102400);
    ti.start();
    for (size_t i = 0;i < NUM_ELS; ++i) {
      cm[v[i]] = i;
  //    if (i % 1000000 == 0) std::cout << cm.load_factor() << std::endl;

    }
    std::cout << NUM_ELS / 1000000 << "M cuckoo map pow2 inserts in " << ti.current_time() << " (Load factor = " << cm.load_factor() << ")" << std::endl;

    graphlab::memory_info::print_usage();

    ti.start();
    for (size_t i = 0;i < 10000000; ++i) {
      size_t t = cm[v[i]];
      assert(t == i);
    }
    std::cout << "10M cuckoo map pow2 successful probes in " << ti.current_time() << std::endl;
  }

{
    graphlab::hopscotch_map<uint32_t, uint32_t> cm;
    ti.start();
    for (size_t i = 0;i < NUM_ELS; ++i) {
      cm[v[i]] = i;
//      if (i % 1000000 == 0) std::cout << cm.load_factor() << std::endl;

    }
    std::cout << NUM_ELS / 1000000 << "M hopscotch inserts in " << ti.current_time() << " (Load factor = " << cm.load_factor() << ")" << std::endl;

    graphlab::memory_info::print_usage();

    ti.start();
    for (size_t i = 0;i < 10000000; ++i) {
      size_t t = cm[v[i]];
      assert(t == i);
    }
    std::cout << "10M hopscotch successful probes in " << ti.current_time() << std::endl;

  }

{
    graphlab::hopscotch_map<uint32_t, uint32_t> cm;
    graphlab::thread_group thrgroup;
    ti.start();
    for (size_t i = 0;i < 4; ++i) {
      thrgroup.launch(boost::bind(parallel_map_inserter, &cm, &v,
                                  i * NUM_ELS / 4, (i + 1) * NUM_ELS / 4));
    }
    thrgroup.join();
    std::cout << NUM_ELS / 1000000 << "M hopscotch parallel inserts in " << ti.current_time() << " (Load factor = " << cm.load_factor() << ")" << std::endl;

    graphlab::memory_info::print_usage();

    ti.start();
    for (size_t i = 0;i < 10000000; ++i) {
      std::pair<bool, uint32_t> res = cm.get_sync(v[i]);
      assert(res.first && res.second == i);
    }
    std::cout << "10M hopscotch successful synchronized probes in " << ti.current_time() << std::endl;

  }
}

int main(int argc, char** argv) {
  std::cout << "Hopscotch Table Parallel Access Sanity Checks... \n";
  sanity_checks();

  std::cout << "Hopscotch Map Sequential Access Sanity Checks... \n";
  hopscotch_map_sanity_checks();

  std::cout << "Map Benchmarks... \n";
  benchmark();
  std::cout << "Done" << std::endl;
}


#define BOOST_TEST_MODULE TEST_MODEL_CLASS
#include "test_data.hpp"
#include "logging.h"
#include <boost/test/unit_test.hpp>
#include "libs/ml_models/EM.h"
#include <iostream>

struct disable_log
{
    disable_log() {
        sl::set_normal_log_file("/dev/null");
        sl::set_special_log_file("/dev/null");
        sl::disable_logging();
    }
};

void do_disable_log()
{
    static const disable_log rr = disable_log{};
    (void)rr;
}

BOOST_AUTO_TEST_CASE(empty_data_set_test)
{
    do_disable_log();
    mlmodels::expectation_maximization model;
    auto m = model.train(mlmodels::training_data{},
            mlmodels::class_data{},
            mlmodels::expectation_maximization::args{}
        );
    BOOST_TEST((m.get() == nullptr), "the model that we should get back from empty data set should be empty as well");
    mlmodels::testing_result out;
    auto r = model.predict(m, mlmodels::testing_data{}, out);
    BOOST_TEST(r == false, "should failed because training did not run");
    BOOST_TEST(out.empty(), "there should not be any data from the test");
}

BOOST_AUTO_TEST_CASE(test_model_create)
{
    const auto& samples = ut_data::unittest_data();
    const auto& classes = ut_data::classes4ut();
    BOOST_TEST_REQUIRE(mlmodels::rows(samples) == classes.size());

    mlmodels::expectation_maximization em_m;
    auto m = em_m.train(samples,
                classes,
                mlmodels::expectation_maximization::args{}
        );
    BOOST_TEST_REQUIRE((m.get() != nullptr), "we are expecting that this train would succeed");
    const auto& test_samples = ut_data::samples4test();
    BOOST_TEST_REQUIRE(mlmodels::rows(test_samples) < mlmodels::rows(samples));
    BOOST_TEST_REQUIRE(mlmodels::columns(test_samples) == mlmodels::columns(samples));
    auto p = em_m.test(m, test_samples, ut_data::NUMBER_OF_CLASSES);
    BOOST_TEST_REQUIRE(!p.empty(), "we are expecting to get entries from the test");
    BOOST_TEST_REQUIRE(p.size() == mlmodels::rows(test_samples));
    //mlmodels::save_model(m);
#ifdef PRINT_MODEL_RESULTS
    std::cout<<std::endl<<"----------------------------\ndata for train\n--------------------------------"<<std::endl;
    for (auto r : samples) {
        for (auto c : r) {
            std::cout<<c<<" ";
        }
        std::cout<<std::endl;
    }
    std::cout<<"\n------------------data test\n-----------------------"<<std::endl;
    for (auto r : test_samples) {
        for (auto c : r) {
            std::cout<<c<<" ";
        }
        std::cout<<std::endl;
    }

    std::cout<<"\n--------------------\npredictions\n----------------"<<std::endl;
    for (auto f : p) {
        std::cout<<"["<<f<<"]";
    }
    std::cout<<std::endl;
#endif
}