#define BOOST_TEST_MODULE "test_omp_bond_length_interaction"

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

#include <test/util/utility.hpp>

#include <mjolnir/math/math.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/local/HarmonicPotential.hpp>
#include <mjolnir/omp/System.hpp>
#include <mjolnir/omp/BondLengthInteraction.hpp>
#include <mjolnir/util/make_unique.hpp>

BOOST_AUTO_TEST_CASE(omp_BondLength_calc_force)
{
    namespace test = mjolnir::test;

    constexpr double tol = 1e-8;
    mjolnir::LoggerManager::set_default_logger("test_omp_bond_length_interaction.log");

    using seq_traits_type  = mjolnir::SimulatorTraits      <double, mjolnir::UnlimitedBoundary>;
    using omp_traits_type  = mjolnir::OpenMPSimulatorTraits<double, mjolnir::UnlimitedBoundary>;

    using real_type        = typename omp_traits_type::real_type;
    using coordinate_type  = typename omp_traits_type::coordinate_type;
    using boundary_type    = typename omp_traits_type::boundary_type;

    using potential_type   = mjolnir::HarmonicPotential<real_type>;

    using omp_system_type      = mjolnir::System<omp_traits_type>;
    using omp_interaction_type = mjolnir::BondLengthInteraction<omp_traits_type, potential_type>;

    using seq_system_type      = mjolnir::System<seq_traits_type>;
    using seq_interaction_type = mjolnir::BondLengthInteraction<seq_traits_type, potential_type>;

    const int max_number_of_threads = omp_get_max_threads();
    BOOST_TEST_WARN(max_number_of_threads > 2);
    BOOST_TEST_MESSAGE("maximum number of threads = " << max_number_of_threads);

    potential_type   potential(/* k = */100.0, /* native length = */ 5.0);
    std::mt19937 rng(123456789);

    for(int num_thread=1; num_thread<=max_number_of_threads; ++num_thread)
    {
        omp_set_num_threads(num_thread);
        BOOST_TEST_MESSAGE("maximum number of threads = " << omp_get_max_threads());

        omp_system_type omp_sys(10, boundary_type{});
        test::clear_everything(omp_sys);

        for(std::size_t i=0; i<omp_sys.size(); ++i)
        {
            omp_sys.position(i) = mjolnir::math::make_coordinate<coordinate_type>(i*5.0, 0.0, 0.0);
        }
        test::apply_random_perturbation(omp_sys, rng, 0.1);

        // init sequential one with the same coordinates
        seq_system_type seq_sys(10, boundary_type{});
        for(std::size_t i=0; i<seq_sys.size(); ++i)
        {
            seq_sys.mass(i)     = omp_sys.mass(i);
            seq_sys.position(i) = omp_sys.position(i);
            seq_sys.velocity(i) = omp_sys.velocity(i);
            seq_sys.force(i)    = omp_sys.force(i);
            seq_sys.name(i)     = omp_sys.name(i);
            seq_sys.group(i)    = omp_sys.group(i);
        }

        omp_interaction_type omp_interaction(/*topol = */"none", {
                {std::array<std::size_t, 2>{{0, 1}}, potential},
                {std::array<std::size_t, 2>{{1, 2}}, potential},
                {std::array<std::size_t, 2>{{2, 3}}, potential},
                {std::array<std::size_t, 2>{{3, 4}}, potential},
                {std::array<std::size_t, 2>{{4, 5}}, potential},
                {std::array<std::size_t, 2>{{5, 6}}, potential},
                {std::array<std::size_t, 2>{{6, 7}}, potential},
                {std::array<std::size_t, 2>{{7, 8}}, potential},
                {std::array<std::size_t, 2>{{8, 9}}, potential}
            });

        seq_interaction_type seq_interaction("none", {
                {std::array<std::size_t, 2>{{0, 1}}, potential},
                {std::array<std::size_t, 2>{{1, 2}}, potential},
                {std::array<std::size_t, 2>{{2, 3}}, potential},
                {std::array<std::size_t, 2>{{3, 4}}, potential},
                {std::array<std::size_t, 2>{{4, 5}}, potential},
                {std::array<std::size_t, 2>{{5, 6}}, potential},
                {std::array<std::size_t, 2>{{6, 7}}, potential},
                {std::array<std::size_t, 2>{{7, 8}}, potential},
                {std::array<std::size_t, 2>{{8, 9}}, potential}
            });

        omp_interaction.initialize(omp_sys);
        seq_interaction.initialize(seq_sys);

        test::check_force_consistency(omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_force_and_energy_consistency(omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_force_and_virial_consistency(omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_force_energy_virial_consistency(omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
        test::check_energy_consistency(omp_sys, omp_interaction, seq_sys, seq_interaction, tol);
    }
}


#include "channel_encryption.hpp"

#include <boost/algorithm/hex.hpp>
#include <openssl/evp.h>
#include <openssl/rand.h>
#include <sodium.h>

#include "utils.hpp"

#include <exception>
#include <string>

#include <iostream>

std::vector<uint8_t> hexToBytes(const std::string& hex) {
    std::vector<uint8_t> temp;
    boost::algorithm::unhex(hex, std::back_inserter(temp));
    return temp;
}

template <typename T>
ChannelEncryption<T>::ChannelEncryption(const std::vector<uint8_t>& private_key)
    : private_key_(private_key) {}

// Derive shared secret from our (ephemeral) `seckey` and the other party's `pubkey`
static std::vector<uint8_t>
calculate_shared_secret(const std::vector<uint8_t>& seckey,
                        const std::vector<uint8_t>& pubkey) {

    std::vector<uint8_t> secret(crypto_scalarmult_BYTES);
    if (pubkey.size() != crypto_scalarmult_curve25519_BYTES) {
        throw std::runtime_error("Bad pubKey size");
    }

    if (crypto_scalarmult(secret.data(), seckey.data(), pubkey.data()) != 0) {
        throw std::runtime_error(
            "Shared key derivation failed (crypto_scalarmult)");
    }
    return secret;
}

static std::vector<uint8_t>
derive_symmetric_key(const std::vector<uint8_t>& seckey,
                     const std::vector<uint8_t>& pubkey) {

    const std::vector<uint8_t> sharedKey = calculate_shared_secret(seckey, pubkey);

    std::vector<uint8_t> derived_key(32);

    const std::string salt_str = "WORKTIPS";
    const auto salt = reinterpret_cast<const unsigned char*>(salt_str.data());

    crypto_auth_hmacsha256_state state;

    crypto_auth_hmacsha256_init(&state, salt, salt_str.size());
    crypto_auth_hmacsha256_update(&state, sharedKey.data(), sharedKey.size());
    crypto_auth_hmacsha256_final(&state, derived_key.data());

    return derived_key;
}

template <typename T>
T ChannelEncryption<T>::encrypt_cbc(const T& plaintext,
                                    const std::string& pubKey) const {
    const std::vector<uint8_t> pubKeyBytes = hexToBytes(pubKey);
    const std::vector<uint8_t> sharedKey =
        calculate_shared_secret(this->private_key_, pubKeyBytes);

    // Initialise cipher
    const EVP_CIPHER* cipher = EVP_aes_256_cbc();
    const int ivLength = EVP_CIPHER_iv_length(cipher);

    // Generate IV
    unsigned char iv[ivLength];
    if (RAND_bytes(iv, ivLength) != 1) {
        throw std::runtime_error("Could not generate IV");
    }

    // Initialise cipher context
    EVP_CIPHER_CTX* ctx = EVP_CIPHER_CTX_new();
    if (EVP_EncryptInit_ex(ctx, cipher, NULL, sharedKey.data(), iv) <= 0) {
        throw std::runtime_error("Could not initialise encryption context");
    }

    int len;
    size_t ciphertext_len = 0;
    auto p = reinterpret_cast<const unsigned char*>(plaintext.data());
    const size_t plaintext_len = plaintext.size();

    // Add some padding of 'blockSize' as upper limit
    const int blockSize = EVP_CIPHER_CTX_block_size(ctx);
    T output;
    output.resize(plaintext_len + blockSize);
    auto o = reinterpret_cast<unsigned char*>(&output[0]);

    // Encrypt every full blocks
    if (EVP_EncryptUpdate(ctx, o, &len, p, plaintext_len) <= 0) {
        throw std::runtime_error("Could not encrypt plaintext");
    }
    ciphertext_len += len;

    // Encrypt any remaining partial blocks
    if (EVP_EncryptFinal_ex(ctx, o + len, &len) <= 0) {
        throw std::runtime_error("Could not finalise encryption");
    }
    ciphertext_len += len;

    // Remove excess padding
    output.resize(ciphertext_len);

    // Insert iv at the start
    output.insert(output.begin(), iv, iv + ivLength);

    EVP_CIPHER_CTX_free(ctx);

    return output;
}

template <typename T>
T ChannelEncryption<T>::encrypt_gcm(const T& plaintext,
                                    const std::string& pubKey) const {
    const std::vector<uint8_t> pubKeyBytes = hexToBytes(pubKey);
    const std::vector<uint8_t> derived_key = derive_symmetric_key(this->private_key_, pubKeyBytes);

    T ciphertext;
    // Ciphertext should always be the length of plaintext plus tag
    ciphertext.resize(plaintext.size() + 16);

    auto ciphertext_ptr = reinterpret_cast<unsigned char*>(&ciphertext[0]);

    unsigned long long ciphertext_len;

    const auto plaintext_ptr = reinterpret_cast<const unsigned char*>(&plaintext[0]);

    unsigned char nonce[crypto_aead_aes256gcm_NPUBBYTES];
    randombytes_buf(nonce, sizeof(nonce));

    crypto_aead_aes256gcm_encrypt(ciphertext_ptr, &ciphertext_len, plaintext_ptr,
                                  plaintext.size(), NULL, 0, NULL, nonce,
                                  derived_key.data());

    ciphertext.resize(ciphertext_len);

    ciphertext.insert(ciphertext.begin(), std::begin(nonce), std::end(nonce));

    // nonce (12 bytes) || ciphertext || tag (16 bytes)
    return ciphertext;
}

template <typename T>
T ChannelEncryption<T>::decrypt_gcm(const T& iv_ciphertext_tag,
                                    const std::string& pubKey) const {
    const std::vector<uint8_t> pubKeyBytes = hexToBytes(pubKey);
    const std::vector<uint8_t> derived_key = derive_symmetric_key(this->private_key_, pubKeyBytes);

    T output;

    // Plaintext should be (16 + 12) bytes shorter
    output.resize(iv_ciphertext_tag.size() - 28);

    auto outPtr = reinterpret_cast<unsigned char*>(&output[0]);

    unsigned long long decrypted_len;

    constexpr auto NONCE_SIZE = 12;
    const auto ciphertext =
        reinterpret_cast<const unsigned char*>(&iv_ciphertext_tag[0] + NONCE_SIZE);

    const auto nonce =
        reinterpret_cast<const unsigned char*>(&iv_ciphertext_tag[0]);

    unsigned long long clen = iv_ciphertext_tag.size() - NONCE_SIZE;

    if (crypto_aead_aes256gcm_decrypt(
            outPtr, &decrypted_len, NULL /* must be null */, ciphertext, clen,
            NULL, 0, nonce, derived_key.data()) != 0) {
        throw std::runtime_error("Could not decrypt (AES-GCM)");
    }

    assert(output.size() == decrypted_len);

    return output;
}

template <typename T>
T ChannelEncryption<T>::decrypt_cbc(const T& ciphertextAndIV,
                                    const std::string& pubKey) const {
    const std::vector<uint8_t> pubKeyBytes = hexToBytes(pubKey);
    const std::vector<uint8_t> sharedKey = calculate_shared_secret(this->private_key_, pubKeyBytes);

    // Initialise cipher
    const EVP_CIPHER* cipher = EVP_aes_256_cbc();
    const int ivLength = EVP_CIPHER_iv_length(cipher);

    auto inPtr = reinterpret_cast<const unsigned char*>(&ciphertextAndIV[0]);

    // Initialise cipher context
    EVP_CIPHER_CTX* ctx = EVP_CIPHER_CTX_new();
    if (EVP_DecryptInit_ex(ctx, cipher, NULL, sharedKey.data(), inPtr) <= 0) {
        throw std::runtime_error("Could not initialise decryption context");
    }

    int len;
    size_t plaintextLength = 0;
    const size_t ciphertextLength = ciphertextAndIV.size() - ivLength;

    // Add some padding of 'blockSize' as upper limit
    const int blockSize = EVP_CIPHER_CTX_block_size(ctx);
    T output;
    output.resize(ciphertextLength + blockSize);

    auto outPtr = reinterpret_cast<unsigned char*>(&output[0]);

    // Decrypt every full blocks
    if (EVP_DecryptUpdate(ctx, outPtr, &len, inPtr + ivLength,
                          ciphertextLength) <= 0) {
        throw std::runtime_error("Could not decrypt block");
    }
    plaintextLength += len;

    // Decrypt any remaining partial blocks
    if (EVP_DecryptFinal_ex(ctx, outPtr + len, &len) <= 0) {
        throw std::runtime_error("Could not finalise decryption");
    }
    plaintextLength += len;

    // Remove excess bytes
    output.resize(plaintextLength);

    // Don't we need to call free even when we throw??
    EVP_CIPHER_CTX_free(ctx);
    return output;
}

// explicit template specialization
template class ChannelEncryption<std::string>;

template class ChannelEncryption<std::vector<uint8_t>>;


// This file generate the score file .pss and produce a skeleton file
#include <cstdlib>
#include <ctime>
#include <math.h>
#include <stdexcept>

#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include <boost/thread.hpp>
#include <boost/algorithm/string.hpp>

#include "urlearning/base/record_file.h"
#include "urlearning/base/bayesian_network.h"
#include "urlearning/base/variable.h"

#include "urlearning/ad_tree/ad_node.h"
#include "urlearning/ad_tree/ad_tree.h"

#include "urlearning/scoring_function/scoring_function.h"
#include "urlearning/scoring_function/score_calculator.h"

#include "urlearning/scoring_function/constraints.h"
#include "urlearning/base/skeleton.hpp"

#include "urlearning/scoring_function/bdeu_scoring_function.h"
#include "urlearning/scoring_function/bic_scoring_function.h"
#include "urlearning/scoring_function/fnml_scoring_function.h"
#include "urlearning/scoring_function/lasso_entropy_scoring_function.h"
#include "urlearning/scoring_function/adaptive_lasso_entropy.h"
#include "urlearning/scoring_function/BIC_OLS.h"
namespace po = boost::program_options;
/**
 * The file containing the data.
 */
std::string inputFile;

/**
 * The delimiter in each line.
 */
char delimiter = ',';

/**
 * The file to write the scores.
 */
std::string outputFile;

/**
 * File specifying constraints on the scores.
 */
std::string constraintsFile;

/**
 * Skeleton File specifies the skeleton superstructure
 */
std::string skeletonFile;

datastructures::Skeleton skeleton;

/**
 * The minimum number of records in the AD-tree.
 */
int rMin = 5;

/**
 * The scoring function to use.
 */
std::string sf = "lasso";

/**
 * which score to use, 0 for lasso, 1 for entropy1, 2 for entropy2
 */
int which = 1;
/**
 * A reference to the scoring function object.
 */
scoring::ScoringFunction *scoringFunction;

/**
 * The ess to use for BDeu.
 */

float equivalentSampleSize = 1.0f;

/**
 * A hard limit on the size of parent sets.
 */
int maxParents = -1;

/**
 * The number of threads to use.
 */
int threadCount = 1;

/**
 * The maximum amount of time to use for each variable.
 */
int runningTime = -1;

/**
 * Whether the data file has variable names in the first row.
 */
bool hasHeader = false;

/**
 * Whether to prune the scores before printing
 */
bool prune = true;

/**
 * Whether to use deCampos-style pruning.
 */
bool enableDeCamposPruning = false;

/**
 * The network information.
 */
datastructures::BayesianNetwork network;

/**
 * Constraints on the allowed scores.
 */
scoring::Constraints *constraints;

inline std::string getTime() {
    time_t now = time(0);
    tm *gmtm = gmtime(&now);
    std::string dt(asctime(gmtm));
    boost::trim(dt);
    return dt;
}

void scoringThread(int thread) {
    scoring::ScoreCalculator scoreCalculator(scoringFunction, maxParents, network.size(), runningTime, constraints);
    const int num = network.size();

    for (int variable = 0; variable < network.size(); variable++) {
        if (variable % threadCount != thread) {
            continue;
        }

        printf("Thread: %d, Variable: %d, Time: %s\n", thread, variable, getTime().c_str());

        FloatMap sc;
        init_map(sc);
	// also include neighbors' neighbors
	varset orig_neighbor_bits = skeleton.get_neighbors(variable);
	varset neighbor_bits = orig_neighbor_bits;
	const int card1 = cardinality(orig_neighbor_bits);
	for(int j=0; j < num; j++)
	{
	  if(VARSET_GET(orig_neighbor_bits, j) and j != variable)
	    neighbor_bits = VARSET_OR( neighbor_bits, skeleton.get_neighbors(j) );
	}
	const int card2 = cardinality(neighbor_bits);
        scoreCalculator.calculateScores( variable, sc, neighbor_bits );

        //#ifdef DEBUG
        int size = sc.size();
        printf("Thread: %d, Variable: %d, Size before pruning: %d, Time: %s, neighbor cardinality %d/%d: %s / %s\n"
	      , thread, variable, size, getTime().c_str(), card1, card2
	      , varsetToString(orig_neighbor_bits).c_str(), varsetToString(neighbor_bits).c_str() );
        //#endif

        //Ni added, print out score
        //printf("Independent and dependent scores, variable # %d, ind_score %f, depend_score %f\n", variable, sc[0], sc[optimal_parents[variable]]);
//    Ni added, commented out on June 10, 2017, prune might not work with continuous case
//        if (prune) {
//            scoreCalculator.prune(sc);
//            int prunedSize = sc.size();
//            printf("Thread: %d, Variable: %d, Size after pruning: %d, Time: %s\n", thread, variable, prunedSize, getTime().c_str());
//        }

        std::string varFilename = outputFile + "." + TO_STRING(variable);
        FILE *varOut = fopen(varFilename.c_str(), "w");

        datastructures::Variable *var = network.get(variable);
        fprintf(varOut, "VAR %s\n", var->getName().c_str());
        fprintf(varOut, "META arity=%d\n", var->getCardinality());

        //fprintf(varOut, "META values=");
        //for (int i = 0; i < var->getCardinality(); i++) {
        //    fprintf(varOut, "%s ", var->getValue(i).c_str());
        //}
        //fprintf(varOut, "\n");

        for (auto score = sc.begin(); score != sc.end(); score++) {
            varset parentSet = (*score).first;
            float s = (*score).second;

            fprintf(varOut, "%f ", s);

            for (int p = 0; p < network.size(); p++) {
                if (VARSET_GET(parentSet, p)) {
                    fprintf(varOut, "%s ", network.get(p)->getName().c_str());
                }
            }

            fprintf(varOut, "\n");
        }

        fprintf(varOut, "\n");
        fclose(varOut);

        sc.clear();
    }
}

int main(int argc, char** argv) {
    boost::timer::auto_cpu_timer t;

    std::string description = std::string("Compute the scores for a csv file.  Example usage: ") + argv[0] + " iris.csv iris.pss";
    po::options_description desc(description);
    double lambda = 0.5; // for lasso
    bool adaptive = false;
    desc.add_options()
            ("input", po::value<std::string > (&inputFile)->required(), "The input file. First positional argument.")
            ("output", po::value<std::string > (&outputFile)->required(), "The output file. Second positional argument.")
            ("delimiter,d", po::value<char> (&delimiter)->required()->default_value(','), "The delimiter of the input file.")
            ("lambda,l", po::value<double> (&lambda), "The lambda in Lasso.")
            ("adaptive,a", "Use adaptive Lasso")
            ("scoreType,w", po::value<int> (&which)->default_value(1), "which score, 0 for lasso, 1 for entropy1, 2 for entropy2")
            ("constraints,c", po::value<std::string > (&constraintsFile), "The file specifying constraints on the scores.")
            ("skeleton,k", po::value<std::string > (&skeletonFile), "The file specifying the skeleton superstructure")
            ("rMin,m", po::value<int> (&rMin)->default_value(5), "The minimum number of records in the AD-tree nodes.")
            ("function,f", po::value<std::string > (&sf)->default_value("BIC"), "The scoring function to use.")
            ("ess,e", po::value<float> (&equivalentSampleSize)->default_value(1.0f), "The equivalent sample size, if BDeu is used.")
            ("maxParents,p", po::value<int> (&maxParents)->default_value(0), "The maximum number of parents for any variable. A value less than 1 means no limit.")
            ("threads,t", po::value<int> (&threadCount)->default_value(1), "The number of separate threads to use for score calculations.")
            ("time,r", po::value<int> (&runningTime)->default_value(-1), "The maximum amount of time to use for each variable. A value less than 1 means no limit.")
            ("hasHeader,s", "Add this flag if the first line of the input file gives the variable names.")
            ("doNotPrune,o", "Add this flag if the scores should NOT be pruned at the end of the search.")
            ("enableDeCamposPruning", "Add this flag to ENABLE DeCampos & Ji (JMLR 2011) pruning for BDeu. This feature is experimental and appears to contain some bugs for sufficiently large parent limits for BDeu. This flag has no effect for BIC or fNML.")
            ("help,h", "Show this help message.")
            ;

    po::positional_options_description positionalOptions;
    positionalOptions.add("input", 1);
    positionalOptions.add("output", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc)
            .positional(positionalOptions).run(),
            vm);

    if (vm.count("help") || argc == 1) {
        std::cout << desc;
        return 0;
    }
    if(vm.count("adaptive"))
    {
        adaptive = true;
        std::cout << "Will use adaptive Lasso\n";
    }

    po::notify(vm);

    hasHeader = vm.count("hasHeader");
    prune = (vm.count("doNotPrune") == 0);
    enableDeCamposPruning = vm.count("enableDeCamposPruning");

    if (threadCount < 1) {
        threadCount = 1;
    }

    printf("URLearning, Score Calculator\n");
    printf("Input file: '%s'\n", inputFile.c_str());
    printf("Output file: '%s'\n", outputFile.c_str());
    printf("Delimiter: '%c'\n", delimiter);
    printf("Constraints file: '%s'\n", constraintsFile.c_str());
    printf("r_min: '%d'\n", rMin);
    printf("Scoring function: '%s'\n", sf.c_str());
    printf("ESS: '%f'\n", equivalentSampleSize);
    printf("Maximum parents: '%d'\n", maxParents);
    printf("Threads: '%d'\n", threadCount);
    printf("Running time (per variable): '%d'\n", runningTime);
    printf("Has header: '%s'\n", (hasHeader ? "true" : "false"));
    printf("Enable end-of-scoring pruning: '%s'\n", (prune ? "true" : "False"));
    printf("Enable deCampos-style pruning (experimental for BDeu): '%s'\n", (enableDeCamposPruning ? "true" : "False"));

    printf("Parsing input file.\n");
    datastructures::RecordFile recordFile(inputFile, delimiter, hasHeader);
    recordFile.read();

    printf("Initializing data specifications.\n");
    network.initialize(recordFile);

    printf("Creating AD-tree.\n");
    scoring::ADTree *adTree = new scoring::ADTree(rMin);
    adTree->initialize(network, recordFile);
    adTree->createTree();

    boost::algorithm::to_lower(sf);

    if (maxParents > network.size() || maxParents < 1) {
        maxParents = network.size() - 1;
    }

    if (sf.compare("bic") == 0) { // bic discrete
        int maxParentCount = log(2 * recordFile.size() / log(recordFile.size()));
        if (maxParentCount < maxParents) {
            maxParents = maxParentCount;
        }
    } else if (sf.compare("cbic") == 0) { // bic continuous
    } else if (sf.compare("fnml") == 0) {
    } else if (sf.compare("bdeu") == 0) {
    } else if (sf.compare("lasso") == 0 or sf.compare("entropy") == 0) {  //Ni added
    } else if (sf.compare("adaptive_lasso") == 0 or sf.compare("adaptive") == 0) {  //Ni added
    } else {
        throw std::runtime_error("Invalid scoring function.  Options are: 'lasso','adaptive', 'BIC', 'fNML' or 'BDeu'.");
    }

    scoring::Constraints *constraints = NULL;
    if (constraintsFile.length() > 0) {
        constraints = scoring::parseConstraints(constraintsFile, network);
    }
    printf("Skeleton file %s\n", skeletonFile.c_str());
    if(skeletonFile.size() > 0) //Need parse the skeleton file
    {
    	if(skeletonFile.find(".arc") + 4 == skeletonFile.size())
    		skeleton.read_arc_list_file(skeletonFile, network.size());
    	else
    		skeleton.read_matrix_file(skeletonFile);
    }
    else
    {
        skeleton.set_variable_count(network.size());
    }

    scoringFunction = NULL;
    scoring::LogLikelihoodCalculator *llc = NULL;
    std::vector< std::vector< float >* >* regret = NULL;

    if(sf.compare("bic") == 0 or sf.compare("fnml") == 0)
    {
      std::vector<float> ilogi = scoring::LogLikelihoodCalculator::getLogCache(recordFile.size());
      llc = new scoring::LogLikelihoodCalculator(adTree, network, ilogi);
      regret = scoring::getRegretCache(recordFile.size(), network.getMaxCardinality());
    }

    if (sf.compare("bic") == 0) //bic for discrete variables
    {
        scoringFunction = new scoring::BICScoringFunction(network, recordFile, llc, constraints, enableDeCamposPruning);
    }
    else if(sf.compare("cbic") == 0) // bic for continuous variables
    {
	printf("Creating continuous BIC function\n");
	scoringFunction = new scoring::BIC_OLS_Function(network, inputFile, constraints, enableDeCamposPruning, lambda);
    }
    else if (sf.compare("fnml") == 0)
    {
        scoringFunction = new scoring::fNMLScoringFunction(network, llc, constraints, regret, enableDeCamposPruning);
    }
    else if (sf.compare("bdeu") == 0)
    {
        scoringFunction = new scoring::BDeuScoringFunction(equivalentSampleSize, network, adTree, constraints, enableDeCamposPruning);
    }
    else if (sf.compare("lasso") == 0 or sf.compare("entropy") == 0)
    {
        printf("Creating Entropy/LassoFunction with input file %s\n", inputFile.c_str());
        scoringFunction = new scoring::LassoEntropyScoringFunction(network, which, inputFile, lambda, constraints, enableDeCamposPruning);
    }
    else if (sf.compare("adaptive_lasso") == 0 or sf.compare("adaptive") == 0)
    {
        printf("Creating Adaptive Entropy/LassoFunction with input file %s\n", inputFile.c_str());
        scoringFunction = new scoring::AdaptiveLassoEntropyScoringFunction(network, which, inputFile, lambda, constraints, adaptive, enableDeCamposPruning);
    }

    std::vector<boost::thread*> threads;
    for (int thread = 0; thread < threadCount; thread++) {
        boost::thread *workerThread = new boost::thread(scoringThread, thread);
        threads.push_back(workerThread);
    }

    for (auto it = threads.begin(); it != threads.end(); it++) {
        (*it)->join();
    }

    // concatenate all of the files together
    std::ofstream out(outputFile, std::ios_base::out | std::ios_base::binary);

    // first, the header information
    std::string header = "META pss_version = 0.1\nMETA input_file=" + inputFile + "\nMETA num_records=" + TO_STRING(recordFile.size()) + "\n";
    header += "META parent_limit=" + TO_STRING(maxParents) + "\nMETA score_type=" + sf + "\nMETA ess=" + TO_STRING(equivalentSampleSize) + "\n\n";
    out.write(header.c_str(), header.size());

    for (int variable = 0; variable < network.size(); variable++) {
        std::string varFilename = outputFile + "." + TO_STRING(variable);
        std::ofstream varFile(varFilename, std::ios_base::in | std::ios_base::binary);

        out << varFile.rdbuf();
        varFile.close();

        // and remove the variable file
        remove(varFilename.c_str());
    }

    out.close();
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_UB_CONSTRAIN_HPP
#define STAN_MATH_PRIM_SCAL_FUN_UB_CONSTRAIN_HPP

#include <stan/math/prim/meta.hpp>
#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/scal/fun/identity_constrain.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <cmath>

namespace stan {
namespace math {

/**
 * Return the upper-bounded value for the specified unconstrained
 * scalar and upper bound.
 *
 *
The transform is
 *
 *
\f$f(x) = U - \exp(x)\f$
 *
 *
where \f$U\f$ is the upper bound.
 *
 * If the upper bound is positive infinity, this function
 * reduces to identity_constrain(x).
 *
 * @tparam T type of scalar
 * @tparam U type of upper bound
 * @param[in] x free scalar.
 * @param[in] ub upper bound
 * @return scalar constrained to have upper bound
 */
template <typename T, typename U>
inline return_type_t<T, U> ub_constrain(const T& x, const U& ub) {
  using std::exp;
  if (ub == INFTY) {
    return identity_constrain(x);
  }
  return ub - exp(x);
}

/**
 * Return the upper-bounded value for the specified unconstrained
 * scalar and upper bound and increment the specified log
 * probability reference with the log absolute Jacobian
 * determinant of the transform.
 *
 *
The transform is as specified for
 * ub_constrain(T, double).  The log absolute Jacobian
 * determinant is
 *
 *
\f$ \log | \frac{d}{dx} -\mbox{exp}(x) + U |
 *     = \log | -\mbox{exp}(x) + 0 | = x\f$.
 *
 * If the upper bound is positive infinity, this function
 * reduces to identity_constrain(x, lp).
 *
 * @tparam T type of scalar
 * @tparam U type of upper bound
 * @param[in] x free scalar.
 * @param[in] ub upper bound
 * @param[in,out] lp log density
 * @return scalar constrained to have upper bound
 */
template <typename T, typename U>
inline return_type_t<T, U> ub_constrain(const T& x, const U& ub, T& lp) {
  using std::exp;
  if (ub == INFTY) {
    return identity_constrain(x, lp);
  }
  lp += x;
  return ub - exp(x);
}

}  // namespace math

}  // namespace stan

#endif


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iostream>
#include <limits>
#include <list>
#include <numeric>
#include <queue>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) (a).begin(), (a).end()
#define AALL(a, n) (a), ((a) + (n))
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define MODNUM (static_cast<int>(1e9 + 7))
#define MOD(x) ((x) % MODNUM)

using namespace std;

using ll = long long;
using VI = vector<int>;
using VI2D = vector<vector<int>>;

const int INF = 2e9;
const double EPS = 1e-10;
const double PI = acos(-1.0);

const int dx[] = {-1, 0, 1, 0};
const int dy[] = {0, -1, 0, 1};

inline int toInt(string s) {
	int v;
	istringstream sin(s);
	sin >> v;
	return v;
}

int sign(ll n) {
	if(n > 0) {
		return 1;
	}
	if(n < 0) {
		return -1;
	}
	return 0;
}

int main() {
	ll t[2];
	ll a[2];
	ll b[2];
	scanf("%lld %lld", &t[0], &t[1]);
	scanf("%lld %lld", &a[0], &a[1]);
	scanf("%lld %lld", &b[0], &b[1]);
	ll ab[2];
	ab[0] = b[0] - a[0];
	ab[1] = b[1] - a[1];
	if(sign(ab[0]) == sign(ab[1])) {
		puts("0");
		return 0;
	}
	ll abt[2];
	abt[0] = ab[0] * t[0];
	abt[1] = ab[1] * t[1];
	if(abt[0] + abt[1] == 0) {
		puts("infinity");
		return 0;
	}
	if(abs(abt[0]) > abs(abt[1])) {
		puts("0");
		return 0;
	}
	ll abti = abs(abt[0] + abt[1]);
	abt[0] = abs(abt[0]);

	ll result = (abt[0] + (abti - 1)) / abti;
	printf("%lld\n", result * 2 - (abt[0] % abti ? 1 : 0));
	return 0;
}


// Copyright 2004 The Trustees of Indiana University.

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

//  Authors: Jeremiah Willcock
//           Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_GURSOY_ATUN_LAYOUT_HPP
#define BOOST_GRAPH_GURSOY_ATUN_LAYOUT_HPP

// Gursoy-Atun graph layout, based on:
// "Neighbourhood Preserving Load Balancing: A Self-Organizing Approach"
// in EuroPar 2000, p. 234 of LNCS 1900
// http://springerlink.metapress.com/link.asp?id=pcu07ew5rhexp9yt

#include <boost/config/no_tr1/cmath.hpp>
#include <boost/throw_exception.hpp>
#include <vector>
#include <exception>
#include <algorithm>

#include <boost/graph/visitors.hpp>
#include <boost/graph/properties.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/topology.hpp>

namespace boost {

namespace detail {

struct over_distance_limit : public std::exception {};

template <typename PositionMap, typename NodeDistanceMap,  typename Topology,
          typename Graph>
struct update_position_visitor {
  typedef typename Topology::point_type Point;
  PositionMap position_map;
  NodeDistanceMap node_distance;
  const Topology& space;
  Point input_vector;
  double distance_limit;
  double learning_constant;
  double falloff_ratio;

  typedef boost::on_examine_vertex event_filter;

  typedef typename graph_traits<Graph>::vertex_descriptor
    vertex_descriptor;

  update_position_visitor(PositionMap position_map,
                          NodeDistanceMap node_distance,
                          const Topology& space,
                          const Point& input_vector,
                          double distance_limit,
                          double learning_constant,
                          double falloff_ratio):
    position_map(position_map), node_distance(node_distance),
    space(space),
    input_vector(input_vector), distance_limit(distance_limit),
    learning_constant(learning_constant), falloff_ratio(falloff_ratio) {}

  void operator()(vertex_descriptor v, const Graph&) const
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::pow;
#endif

    if (get(node_distance, v) > distance_limit)
      BOOST_THROW_EXCEPTION(over_distance_limit());
    Point old_position = get(position_map, v);
    double distance = get(node_distance, v);
    double fraction =
      learning_constant * pow(falloff_ratio, distance * distance);
    put(position_map, v,
        space.move_position_toward(old_position, fraction, input_vector));
  }
};

template<typename EdgeWeightMap>
struct gursoy_shortest
{
  template<typename Graph, typename NodeDistanceMap, typename UpdatePosition>
  static inline void
  run(const Graph& g, typename graph_traits<Graph>::vertex_descriptor s,
      NodeDistanceMap node_distance,  UpdatePosition& update_position,
      EdgeWeightMap weight)
  {
    boost::dijkstra_shortest_paths(g, s, weight_map(weight).
      visitor(boost::make_dijkstra_visitor(std::make_pair(
       boost::record_distances(node_distance, boost::on_edge_relaxed()),
        update_position))));
  }
};

template<>
struct gursoy_shortest<dummy_property_map>
{
  template<typename Graph, typename NodeDistanceMap, typename UpdatePosition>
  static inline void
  run(const Graph& g, typename graph_traits<Graph>::vertex_descriptor s,
      NodeDistanceMap node_distance,  UpdatePosition& update_position,
      dummy_property_map)
  {
    boost::breadth_first_search(g, s,
      visitor(boost::make_bfs_visitor(std::make_pair(
        boost::record_distances(node_distance, boost::on_tree_edge()),
        update_position))));
  }
};

} // namespace detail

template <typename VertexListAndIncidenceGraph,  typename Topology,
          typename PositionMap, typename Diameter, typename VertexIndexMap,
          typename EdgeWeightMap>
void
gursoy_atun_step
  (const VertexListAndIncidenceGraph& graph,
   const Topology& space,
   PositionMap position,
   Diameter diameter,
   double learning_constant,
   VertexIndexMap vertex_index_map,
   EdgeWeightMap weight)
{
#ifndef BOOST_NO_STDC_NAMESPACE
  using std::pow;
  using std::exp;
#endif

  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_iterator
    vertex_iterator;
  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_descriptor
    vertex_descriptor;
  typedef typename Topology::point_type point_type;
  vertex_iterator i, iend;
  std::vector<double> distance_from_input_vector(num_vertices(graph));
  typedef boost::iterator_property_map<std::vector<double>::iterator,
                                       VertexIndexMap,
                                       double, double&>
    DistanceFromInputMap;
  DistanceFromInputMap distance_from_input(distance_from_input_vector.begin(),
                                           vertex_index_map);
  std::vector<double> node_distance_map_vector(num_vertices(graph));
  typedef boost::iterator_property_map<std::vector<double>::iterator,
                                       VertexIndexMap,
                                       double, double&>
    NodeDistanceMap;
  NodeDistanceMap node_distance(node_distance_map_vector.begin(),
                                vertex_index_map);
  point_type input_vector = space.random_point();
  vertex_descriptor min_distance_loc
    = graph_traits<VertexListAndIncidenceGraph>::null_vertex();
  double min_distance = 0.0;
  bool min_distance_unset = true;
  for (boost::tie(i, iend) = vertices(graph); i != iend; ++i) {
    double this_distance = space.distance(get(position, *i), input_vector);
    put(distance_from_input, *i, this_distance);
    if (min_distance_unset || this_distance < min_distance) {
      min_distance = this_distance;
      min_distance_loc = *i;
    }
    min_distance_unset = false;
  }
  assert (!min_distance_unset); // Graph must have at least one vertex
  boost::detail::update_position_visitor<
      PositionMap, NodeDistanceMap, Topology,
      VertexListAndIncidenceGraph>
    update_position(position, node_distance, space,
                    input_vector, diameter, learning_constant,
                    exp(-1. / (2 * diameter * diameter)));
  std::fill(node_distance_map_vector.begin(), node_distance_map_vector.end(), 0);
  try {
    typedef detail::gursoy_shortest<EdgeWeightMap> shortest;
    shortest::run(graph, min_distance_loc, node_distance, update_position,
                  weight);
  } catch (detail::over_distance_limit) {
    /* Thrown to break out of BFS or Dijkstra early */
  }
}

template <typename VertexListAndIncidenceGraph,  typename Topology,
          typename PositionMap, typename VertexIndexMap,
          typename EdgeWeightMap>
void gursoy_atun_refine(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps,
                        double diameter_initial,
                        double diameter_final,
                        double learning_constant_initial,
                        double learning_constant_final,
                        VertexIndexMap vertex_index_map,
                        EdgeWeightMap weight)
{
#ifndef BOOST_NO_STDC_NAMESPACE
  using std::pow;
  using std::exp;
#endif

  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_iterator
    vertex_iterator;
  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_descriptor
    vertex_descriptor;
  typedef typename Topology::point_type point_type;
  vertex_iterator i, iend;
  double diameter_ratio = (double)diameter_final / diameter_initial;
  double learning_constant_ratio =
    learning_constant_final / learning_constant_initial;
  std::vector<double> distance_from_input_vector(num_vertices(graph));
  typedef boost::iterator_property_map<std::vector<double>::iterator,
                                       VertexIndexMap,
                                       double, double&>
    DistanceFromInputMap;
  DistanceFromInputMap distance_from_input(distance_from_input_vector.begin(),
                                           vertex_index_map);
  std::vector<int> node_distance_map_vector(num_vertices(graph));
  typedef boost::iterator_property_map<std::vector<int>::iterator,
                                       VertexIndexMap, double, double&>
    NodeDistanceMap;
  NodeDistanceMap node_distance(node_distance_map_vector.begin(),
                                vertex_index_map);
  for (int round = 0; round < nsteps; ++round) {
    double part_done = (double)round / (nsteps - 1);
    int diameter = (int)(diameter_initial * pow(diameter_ratio, part_done));
    double learning_constant =
      learning_constant_initial * pow(learning_constant_ratio, part_done);
    gursoy_atun_step(graph, space, position, diameter, learning_constant,
                     vertex_index_map, weight);
  }
}

template <typename VertexListAndIncidenceGraph,  typename Topology,
          typename PositionMap, typename VertexIndexMap,
          typename EdgeWeightMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps,
                        double diameter_initial,
                        double diameter_final,
                        double learning_constant_initial,
                        double learning_constant_final,
                        VertexIndexMap vertex_index_map,
                        EdgeWeightMap weight)
{
  typedef typename graph_traits<VertexListAndIncidenceGraph>::vertex_iterator
    vertex_iterator;
  vertex_iterator i, iend;
  for (boost::tie(i, iend) = vertices(graph); i != iend; ++i) {
    put(position, *i, space.random_point());
  }
  gursoy_atun_refine(graph, space,
                     position, nsteps,
                     diameter_initial, diameter_final,
                     learning_constant_initial, learning_constant_final,
                     vertex_index_map, weight);
}

template <typename VertexListAndIncidenceGraph,  typename Topology,
          typename PositionMap, typename VertexIndexMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps,
                        double diameter_initial,
                        double diameter_final,
                        double learning_constant_initial,
                        double learning_constant_final,
                        VertexIndexMap vertex_index_map)
{
  gursoy_atun_layout(graph, space, position, nsteps,
                     diameter_initial, diameter_final,
                     learning_constant_initial, learning_constant_final,
                     vertex_index_map, dummy_property_map());
}

template <typename VertexListAndIncidenceGraph, typename Topology,
          typename PositionMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps,
                        double diameter_initial,
                        double diameter_final = 1.0,
                        double learning_constant_initial = 0.8,
                        double learning_constant_final = 0.2)
{
  gursoy_atun_layout(graph, space, position, nsteps, diameter_initial,
                     diameter_final, learning_constant_initial,
                     learning_constant_final, get(vertex_index, graph));
}

template <typename VertexListAndIncidenceGraph, typename Topology,
          typename PositionMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position,
                        int nsteps)
{
#ifndef BOOST_NO_STDC_NAMESPACE
  using std::sqrt;
#endif

  gursoy_atun_layout(graph, space, position, nsteps,
                     sqrt((double)num_vertices(graph)));
}

template <typename VertexListAndIncidenceGraph, typename Topology,
          typename PositionMap>
void gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                        const Topology& space,
                        PositionMap position)
{
  gursoy_atun_layout(graph, space, position, num_vertices(graph));
}

template<typename VertexListAndIncidenceGraph, typename Topology,
         typename PositionMap, typename P, typename T, typename R>
void
gursoy_atun_layout(const VertexListAndIncidenceGraph& graph,
                   const Topology& space,
                   PositionMap position,
                   const bgl_named_params<P,T,R>& params)
{
#ifndef BOOST_NO_STDC_NAMESPACE
  using std::sqrt;
#endif

  std::pair<double, double> diam(sqrt(double(num_vertices(graph))), 1.0);
  std::pair<double, double> learn(0.8, 0.2);
  gursoy_atun_layout(graph, space, position,
                     choose_param(get_param(params, iterations_t()),
                                  num_vertices(graph)),
                     choose_param(get_param(params, diameter_range_t()),
                                  diam).first,
                     choose_param(get_param(params, diameter_range_t()),
                                  diam).second,
                     choose_param(get_param(params, learning_constant_range_t()),
                                  learn).first,
                     choose_param(get_param(params, learning_constant_range_t()),
                                  learn).second,
                     choose_const_pmap(get_param(params, vertex_index), graph,
                                       vertex_index),
                     choose_param(get_param(params, edge_weight),
                                  dummy_property_map()));
}

} // namespace boost

#endif // BOOST_GRAPH_GURSOY_ATUN_LAYOUT_HPP


/*
 * DivRightHandSideAdjoint.cpp
 *
 *  Created on: 03.08.2017
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/types.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/fe/fe.h>
#include <deal.II/fe/fe_update_flags.h>
#include <deal.II/fe/fe_values.h>

#include <base/DiscretizedFunction.h>
#include <forward/DivRightHandSideAdjoint.h>

#include <functional>
#include <vector>

namespace wavepi {
namespace forward {
using namespace dealii;
using namespace wavepi::base;

template <int dim>
DivRightHandSideAdjoint<dim>::DivRightHandSideAdjoint(std::shared_ptr<Function<dim>> a,
                                                      std::shared_ptr<Function<dim>> u)
    : a(a), u(u) {}

template <int dim>
DivRightHandSideAdjoint<dim>::AssemblyScratchData::AssemblyScratchData(const FiniteElement<dim> &fe,
                                                                       const Quadrature<dim> &quad)
    : fe_values(fe, quad, update_values | update_gradients | update_quadrature_points | update_JxW_values) {}

template <int dim>
DivRightHandSideAdjoint<dim>::AssemblyScratchData::AssemblyScratchData(const AssemblyScratchData &scratch_data)
    : fe_values(scratch_data.fe_values.get_fe(), scratch_data.fe_values.get_quadrature(),
                update_values | update_gradients | update_quadrature_points | update_JxW_values) {}

template <int dim>
void DivRightHandSideAdjoint<dim>::copy_local_to_global(Vector<double> &result, const AssemblyCopyData &copy_data) {
  for (unsigned int i = 0; i < copy_data.local_dof_indices.size(); ++i)
    result(copy_data.local_dof_indices[i]) += copy_data.cell_rhs(i);
}

template <int dim>
void DivRightHandSideAdjoint<dim>::local_assemble_dd(const Vector<double> &a, const Vector<double> &u,
                                                     const typename DoFHandler<dim>::active_cell_iterator &cell,
                                                     AssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
  const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
  const unsigned int n_q_points    = scratch_data.fe_values.get_quadrature().size();

  copy_data.cell_rhs.reinit(dofs_per_cell);
  copy_data.local_dof_indices.resize(dofs_per_cell);
  scratch_data.fe_values.reinit(cell);

  cell->get_dof_indices(copy_data.local_dof_indices);

  for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
    for (unsigned int i = 0; i < dofs_per_cell; ++i)
      for (unsigned int ka = 0; ka < dofs_per_cell; ++ka)
        for (unsigned int ku = 0; ku < dofs_per_cell; ++ku)
          copy_data.cell_rhs(i) -= a[copy_data.local_dof_indices[ka]] * scratch_data.fe_values.shape_grad(ka, q_point) *
                                   u[copy_data.local_dof_indices[ku]] * scratch_data.fe_values.shape_grad(ku, q_point) *
                                   scratch_data.fe_values.shape_value(i, q_point) * scratch_data.fe_values.JxW(q_point);
}

template <int dim>
void DivRightHandSideAdjoint<dim>::local_assemble_cc(const Function<dim> *const a, const Function<dim> *const u,
                                                     const typename DoFHandler<dim>::active_cell_iterator &cell,
                                                     AssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
  const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
  const unsigned int n_q_points    = scratch_data.fe_values.get_quadrature().size();

  copy_data.cell_rhs.reinit(dofs_per_cell);
  copy_data.local_dof_indices.resize(dofs_per_cell);
  scratch_data.fe_values.reinit(cell);

  cell->get_dof_indices(copy_data.local_dof_indices);

  for (unsigned int q_point = 0; q_point < n_q_points; ++q_point) {
    auto grad_a = a->gradient(scratch_data.fe_values.quadrature_point(q_point));
    auto grad_u = u->gradient(scratch_data.fe_values.quadrature_point(q_point));

    for (unsigned int i = 0; i < dofs_per_cell; ++i)
      copy_data.cell_rhs(i) -=
          grad_a * grad_u * scratch_data.fe_values.shape_value(i, q_point) * scratch_data.fe_values.JxW(q_point);
  }
}

template <int dim>
void DivRightHandSideAdjoint<dim>::create_right_hand_side(const DoFHandler<dim> &dof, const Quadrature<dim> &quad,
                                                          Vector<double> &rhs) const {
  AssertThrow(a != nullptr, ExcZero());
  AssertThrow(u != nullptr, ExcZero());

  a->set_time(this->get_time());
  u->set_time(this->get_time());

  auto a_d = dynamic_cast<DiscretizedFunction<dim> *>(a.get());
  auto u_d = dynamic_cast<DiscretizedFunction<dim> *>(u.get());

  if (a_d != nullptr && u_d != nullptr) {
    Vector<double> ca = a_d->get_function_coefficients(a_d->get_time_index());
    Vector<double> cu = u_d->get_function_coefficients(u_d->get_time_index());

    Assert(ca.size() == dof.n_dofs(), ExcDimensionMismatch(ca.size(), dof.n_dofs()));
    Assert(cu.size() == dof.n_dofs(), ExcDimensionMismatch(cu.size(), dof.n_dofs()));

    WorkStream::run(
        dof.begin_active(), dof.end(),
        std::bind(&DivRightHandSideAdjoint<dim>::local_assemble_dd, *this, std::ref(ca), std::ref(cu),
                  std::placeholders::_1, std::placeholders::_2, std::placeholders::_3),
        std::bind(&DivRightHandSideAdjoint<dim>::copy_local_to_global, *this, std::ref(rhs), std::placeholders::_1),
        AssemblyScratchData(dof.get_fe(), quad), AssemblyCopyData());
  } else
    WorkStream::run(
        dof.begin_active(), dof.end(),
        std::bind(&DivRightHandSideAdjoint<dim>::local_assemble_cc, *this, a.get(), u.get(), std::placeholders::_1,
                  std::placeholders::_2, std::placeholders::_3),
        std::bind(&DivRightHandSideAdjoint<dim>::copy_local_to_global, *this, std::ref(rhs), std::placeholders::_1),
        AssemblyScratchData(dof.get_fe(), quad), AssemblyCopyData());
}

template <int dim>
DiscretizedFunction<dim> DivRightHandSideAdjoint<dim>::run_adjoint(std::shared_ptr<SpaceTimeMesh<dim>> mesh) {
  DiscretizedFunction<dim> target(mesh);

  for (size_t i = 0; i < mesh->length(); i++) {
    this->set_time(mesh->get_time(i));
    auto dof_handler = mesh->get_dof_handler(i);

    Vector<double> tmp(dof_handler->n_dofs());
    this->create_right_hand_side(*dof_handler, mesh->get_quadrature(), tmp);
    target[i] = tmp;
  }

  return target;
}

template class DivRightHandSideAdjoint<1>;
template class DivRightHandSideAdjoint<2>;
template class DivRightHandSideAdjoint<3>;

} /* namespace forward */
} /* namespace wavepi */


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


/*Copyright (c) 2021 James Gayvert

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <iostream>
#include <cmath>
#include <stdlib.h>
#include <vector>
//#include <boost/math/special_functions/gamma.hpp>  remove for now, keep in case we ever end up using boost
#include <vector>
#include "Shell.h"
#include "boxcap.h"
#include "../asa/asa239.h"

using namespace std;

const double PI = 3.14159265358979;
const double sqrt_pi = 1.77245385090552;
const double OOPI3 = 1.0 / (PI * PI * PI);
const double gauss_factor[] = {1.0, 0.5, 0.75, 1.875, 6.5625, 29.53125, 162.421875};

//
// box-CAP helper functions
//
double intmod_r2(double a, int k, double cl, double cu);
int xi (double z, int i);
double intgauss(double alpha, int n, int = 0, const char* = "message ");
vector<double> polmulti(vector<double>& g, int n, vector<double>& h, int m,
	       int = 0, const char* = "message");
void poltrans (vector<double>& f, double alpha, double beta, int n,
	       int = 0, const char* = "message ");
void gauss2(double ao, double alpha, double bo, double beta,
	    double& co, double& expo, double& factor, int = 0);
void print_vector(vector<double>& v, const char msg[]);

/*
     gaussian integral of the softbox potential
     V  = { (x+boxlength)^2  for x < -boxlength
          {        0         for -boxlength < x < boxlength
          { (x-boxlength)^2  for x > boxlength

This is a Fortran to Cpp translation of our old code from the late 1990ties.
Fortran by Uwe Riss, Robin Santra, and Thomas Sommerfeld
Cpp translation by Thomas Sommerfeld, Nov 2018 T. Sommerfeld

The integrals of the so-called box-CAP are computed analytically.
Do not even try to backwards-engineer this, dig out Robin's paper.

@Article{santra99,
author = "R. Santra and L. S. Cederbaum and H.-D. Meyer",
journal = "Chem. Phys. Lett.",
volume = 303,
year =   1999,
pages =  413,
title =  "Electronic decay of molecular clusters: Non-stationary states computed by standard quantum chemistry methods"}

gto1 and gto2 define two primitive Cartesian GTOs including their l value,
say, px, or dx^2.
boxlength[3] are the cutoff parameters defined above
normalized [0] is a flag 0=unnormalized GTOs, 1=normalized GTOs
verbose can be increased to create debug information

*/

double integrate_box_cap(Shell shell1, Shell shell2,
std::array<size_t,3> l1, std::array<size_t,3> l2, double boxlength[3])
{
  double sum = 0;
	for(size_t prim1 =0;prim1<shell1.num_prims;prim1++)
  {
    gto gto1 = {shell1.origin,shell1.exps[prim1],l1};
    for(size_t prim2=0;prim2<shell2.num_prims;prim2++)
    {
      gto gto2 = {shell2.origin,shell2.exps[prim2],l2};
      sum+= shell1.coeffs[prim1] * shell2.coeffs[prim2] * boxcap(gto1,gto2,boxlength,false,0);
    }
  }
  return sum;
}

double boxcap(gto gto1, gto gto2, double boxlength[3],
	      int normalized, int verbose) {

  int debug = verbose;

  double cen[3] = {0, 0, 0};
  double alpha = gto1.a;
  double beta  = gto2.a;
  double d0, delta, fakt;

  double ovl[3] = {0,0,0};
  double dip[3] = {0,0,0};
  for (int k = 0; k < 3; ++k) {
    int m = gto1.l[k];
    int n = gto2.l[k];
    int nm = n+m;
    double ao = gto1.r[k];
    double bo = gto2.r[k];
    gauss2(ao, alpha,  bo, beta, d0, delta, fakt, verbose);

    vector<double> g (m+1,0);
    vector<double> h (n+1,0);
    g[m] = 1;
    h[n] = 1;
    poltrans(g, ao, d0, m, debug, " g = ");
    poltrans(h, bo, d0, n, debug, " h = ");
    vector<double> f = polmulti(g, m, h, n, debug, " f = ");

    double cl = cen[k] - d0 - boxlength[k];
    double cu = cen[k] - d0 + boxlength[k];

    double temp1 = 0;
    double temp2 = 0;
    for (int i = 0; i <= nm; ++i)
    {
      temp1 += f[i] * intgauss( delta, i, debug, "  I = ");
      temp2 += f[i] * intmod_r2(delta, i, cl, cu);
    }
    ovl[k] = fakt * temp1;
    dip[k] = fakt * temp2;
  }
  if (verbose > 1) {
    cout << "Ovl: " << ovl[0] << ", " <<  ovl[1] << ", " <<  ovl[2] << endl;
    cout << "Dip: " << dip[0] << ", " <<  dip[1] << ", " <<  dip[2] << endl;
  }

  double cap = dip[0]*ovl[1]*ovl[2] + ovl[0]*dip[1]*ovl[2] + ovl[0]*ovl[1]*dip[2];
  if (normalized == 0)
    return cap;
  else {
    alpha *= 2;
    beta *= 2;
    double ovl[3] = {0, 0, 0};
    for (int k=0; k < 3; ++k) {
      int m = 2 * gto1.l[k];
      int n = 2 * gto2.l[k];
      ovl[k] = intgauss(alpha,m) * intgauss(beta,n);
    }
    if (verbose > 1)
      cout << "<0>: " << ovl[0] << ", " <<  ovl[1] << ", " <<  ovl[2] << endl;
    return cap / sqrt(ovl[0]*ovl[1]*ovl[2]);
  }
}

//
//
// integrate(x**k * f(x) * exp(-a*x**2), x, -oo, oo)
//
// where
//
//        (x + cl)**2 if x < cl
// f(x) =    0        if cl <= x <= cu
//        (x - cu)**2 if x > cu
//
// This integral can be expressed in terms of the incomplete gamma-function.
// see:
//  @Article{santra99,
//  author = "R. Santra and L. S. Cederbaum and H.-D. Meyer",
//  journal = "Chem. Phys. Lett.",
//  volume = 303,
//  year =   1999,
//  pages =  413,
//  title =  "Electronic decay of molecular clusters:
//            Non-stationary states computed by standard quantum chemistry methods"}
//
//
double intmod_r2(double a, int k, double cl, double cu) {
  int sg = -1;
  if ((k % 2) == 0)
    sg = 1;
  double ql = a*cl*cl;
  double qu = a*cu*cu;
  double result = 0;
  int ifault = 0;
  for (int i = 0; i < 3; ++i) {
    int j = k + i;
    double par = 0.5*(j + 1);
    int ex = 2 - i;
    double dl = pow(cl,ex);
    double du = pow(-cu,ex);
    double term = tgamma(par)*(sg*dl + du);
    double norm_factor = tgamma(par);
    term -= sg*dl*gammad(ql,par,&ifault)*norm_factor*xi(-cl,j);
    term -=    du*gammad(qu,par,&ifault)*norm_factor*xi( cu,j);
    //term -= sg*dl*boost::math::tgamma_lower(par,ql)*xi(-cl,j);  func is not normalized w/ boost
    //term -=    du*boost::math::tgamma_lower(par,qu)*xi( cu,j);  func is not normalized w/ boost
    term /= pow(a,par);
    if (i == 1)
      term *= 2;
    result += term;
  }
  return 0.5*result;
}

//
//  computes a sign needed in intmod_r2()
//
int xi (double z, int i) {
  if (z >= 0)
    return 1;
  else {
    if ((i%2) == 0)
      return -1;
    else
      return 1;
  }
}

//
//  returns int dx x^n exp(-alpha*x^2)
//
double intgauss(double alpha, int n, int debug, const char* msg) {
  double integral = 0.0;
  if (n%2 == 0) {
    int k = n/2;
    double a = pow(alpha,k);
    integral = gauss_factor[k] * sqrt(PI / alpha) / a;
  }
  if (debug > 3)
    cout << msg << integral << endl;
  return integral;
}

//
//   {sum} a_i x^i  *  {sum} b_i x^i  ->  {sum} c_i x^i
//
vector<double> polmulti(vector<double>& g, int n, vector<double>& h, int m,
			int verbose, const char msg[]) {
  vector<double> f (n+m+1,0);
  for (int i = 0; i <= n; ++i) {
    for (int j = 0; j <= m; ++j) {
      f[i+j] += g[i] * h[j];
    }
  }
  if (verbose > 2) {
    print_vector(f, msg);
  }
  return f;
}

//
// {sum} a_i ( x - a )^i   ->   {sum} b_i ( x - b )^i
//
void poltrans (vector<double> &f, double alpha, double beta, int n,
	       int verbose, const char msg[]) {
  double diff = beta - alpha;
  int k = 1;
  for (int j = 0; j <= n; ++j) {
    double w = 0;
    for (int i = n; i >= j; --i) {
      w = w*diff + f[i];
      f[i] *= (i-j);
    }
    f[j] = w / k;
    k = (j+1)*k;
  }
  if (verbose > 2)
    print_vector(f, msg);
}

void gauss2(double ao, double alpha, double bo, double beta,
	    double& co, double& expo, double& factor, int verbose) {
  expo = alpha + beta;
  co = (ao*alpha + bo*beta) / expo;
  factor = alpha * beta * (bo-ao) * (bo-ao) / expo;
  factor = exp(-factor);
  if (verbose > 2)
    cout << "gs2:" << co << "  " << expo << "  "  << factor << endl;
}

void print_vector(vector<double> &v, const char msg[]) {
  int n = static_cast<int>(v.size());
  cout << msg;
  for (int i = 0; i < n; ++i) {
    cout << v[i];
    if ((i+1)%5 == 0)
      cout << endl;
    else
      cout << ", ";
  }
  if (n%5 != 0)
    cout << endl;
}


#ifndef STAN_MATH_PRIM_PROB_SKEW_DOUBLE_EXPONENTIAL_RNG_HPP
#define STAN_MATH_PRIM_PROB_SKEW_DOUBLE_EXPONENTIAL_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/log1m.hpp>
#include <stan/math/prim/fun/max_size.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a skew double exponential random variate with the given location
 * scale and skewness using the specified random number generator.
 *
 * mu, sigma and tau can each be a scalar or a one-dimensional container. Any
 * non-scalar inputs must be the same size.
 *
 * @tparam T_loc Type of location parameter
 * @tparam T_scale Type of scale parameter
 * @tparam T_skewness Type of skewness parameter
 * @tparam RNG class of random number generator
 * @param mu (Sequence of) location parameter(s)
 * @param sigma (Sequence of) scale parameter(s)
 * @param tau (Sequence of) skewness parameter(s)
 * @param rng random number generator
 * @return (Sequence of) double exponential random variate(s)
 * @throw std::domain_error if mu is infinite or sigma is nonpositive or tau is
 *  not bound between 0.0 and 1.0
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_loc, typename T_scale, typename T_skewness, class RNG>
inline typename VectorBuilder<true, double, T_loc, T_scale, T_skewness>::type
skew_double_exponential_rng(const T_loc& mu, const T_scale& sigma,
                            const T_skewness& tau, RNG& rng) {
  using boost::variate_generator;
  using boost::random::uniform_real_distribution;
  using T_mu_ref = ref_type_t<T_loc>;
  using T_sigma_ref = ref_type_t<T_scale>;
  using T_tau_ref = ref_type_t<T_skewness>;
  static const char* function = "skew_double_exponential_rng";
  check_consistent_sizes(function, "Location parameter", mu, "Scale Parameter",
                         sigma, "Skewness Parameter", tau);
  T_mu_ref mu_ref = mu;
  T_sigma_ref sigma_ref = sigma;
  T_tau_ref tau_ref = tau;
  check_finite(function, "Location parameter", mu_ref);
  check_positive_finite(function, "Scale parameter", sigma_ref);
  check_bounded(function, "Skewness parameter", tau_ref, 0.0, 1.0);

  scalar_seq_view<T_mu_ref> mu_vec(mu_ref);
  scalar_seq_view<T_sigma_ref> sigma_vec(sigma_ref);
  scalar_seq_view<T_tau_ref> tau_vec(tau_ref);
  size_t N = max_size(mu, sigma, tau);
  VectorBuilder<true, double, T_loc, T_scale, T_skewness> output(N);

  variate_generator<RNG&, uniform_real_distribution<> > z_rng(
      rng, uniform_real_distribution<>(0.0, 1.0));
  for (size_t n = 0; n < N; ++n) {
    double z = z_rng();
    if (z < tau_vec[n]) {
      output[n]
          = log(z / tau_vec[n]) * sigma_vec[n] / (2.0 * (1.0 - tau_vec[n]))
            + mu_vec[n];
    } else {
      output[n] = log((1.0 - z) / (1.0 - tau_vec[n])) * (-sigma_vec[n])
                      / (2.0 * tau_vec[n])
                  + mu_vec[n];
    }
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


#include <Eigen/Eigen>
#include <Math/VECTOR.h>
#include <Physics/CONSTITUTIVE_MODEL.h>
#include <FEM/FEM_EXPORTER.h>
#include <Utils/STDCONTAINER.h>
#include <Utils/MESHIO.h>
#include <Utils/RANDOM.h>
#include <Utils/PROFILER.h>
#include <Utils/PARAMETER.h>
#include <Storage/prelude.hpp>
#include <Storage2/PRELUDE.hpp>
#include <pybind11/pybind11.h>

namespace py = pybind11;
namespace JGSL {

PYBIND11_MODULE(JGSL, m) {
    m.def("Kokkos_Initialize", &Kokkos_Initialize);
    m.def("Kokkos_Finalize", &Kokkos_Finalize);
    Export_Vector(m);
    Export_Random(m);
    py::module meshIO_m = m.def_submodule("MeshIO", "A submodule of JGSL for mesh IO");
    Export_MeshIO(meshIO_m);
    Export_StdContainer(m);
    Export_Constitutive_Model<double, 2>(m);
    Export_Constitutive_Model<double, 3>(m);
    py::module storage_m = m.def_submodule("Storage", "A submodule of JGSL for Storage");
    Export_Storage<Kokkos::Serial, Kokkos::HostSpace, 4>(storage_m);
    py::module storage_2_m = m.def_submodule("Storage2", "The newer version of Storage");
    Export_Storage_2(storage_2_m);
    Export_Profiler(m);
    py::module FEM_m = m.def_submodule("FEM", "A submodule of JGSL for FEM utils");
    Export_FEM(FEM_m);
    Export_SPARSE_MATRIX(m);
    PARAMETER::Export(m);
}

}


/*  _______________________________________________________________________

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

//- Class:	 NonDGPImpSampling
//- Description: Implementation code for NonDGPImpSampling class
//- Owner:       Laura Swiler and Keith Dalbey
//- Checked by:
//- Version:

#include "NonDGPImpSampling.hpp"
#include "dakota_system_defs.hpp"
#include "dakota_data_types.hpp"
#include "dakota_data_io.hpp"
#include "DakotaModel.hpp"
#include "DakotaResponse.hpp"
#include "NonDLHSSampling.hpp"
#include "ProblemDescDB.hpp"
#include "DataFitSurrModel.hpp"
#include "pecos_data_types.hpp"
#include "pecos_stat_util.hpp"
#include "DakotaApproximation.hpp"
#include <boost/lexical_cast.hpp>

static const char rcsId[]="@(#) $Id: NonDGPImpSampling.cpp 7035 2010-10-22 21:45:39Z mseldre $";

namespace Dakota {

/** This constructor is called for a standard letter-envelope iterator
    instantiation.  In this case, set_db_list_nodes has been called and
    probDescDB can be queried for settings from the method specification. */
NonDGPImpSampling::NonDGPImpSampling(ProblemDescDB& problem_db, Model& model):
  NonDSampling(problem_db, model)
{
  // sampleType default in DataMethod.cpp is SUBMETHOD_DEFAULT (0).
  // Enforce an LHS default for this method.
  if (!sampleType)
    sampleType = SUBMETHOD_LHS;

  samplingVarsMode = ACTIVE_UNIFORM;
  String sample_reuse, approx_type("global_kriging");/*("global_kriging");*/
  UShortArray approx_order; // not used by GP/kriging
  short corr_order = -1, data_order = 1, corr_type = NO_CORRECTION;
  if (probDescDB.get_bool("method.derivative_usage")) {
    if (iteratedModel.gradient_type() != "none") data_order |= 2;
    if (iteratedModel.hessian_type()  != "none") data_order |= 4;
  }
  unsigned short sample_type = SUBMETHOD_DEFAULT;
  statsFlag = true; //print computed probability levels at end
  bool vary_pattern = false; // for consistency across outer loop invocations
  // get point samples file
  const String& import_pts_file
    = probDescDB.get_string("method.import_build_points_file");
  // BMA: This was previously using numSamples = initial_samples from base class
  numSamples = probDescDB.get_int("method.build_samples");
  int samples = numSamples;
  if (!import_pts_file.empty())
    { samples = 0; sample_reuse = "all"; }

  gpBuild.assign_rep(new NonDLHSSampling(iteratedModel, sample_type,
     samples, randomSeed, rngName, varyPattern, ACTIVE_UNIFORM), false);
  //distribution 1 which is the distribution that the initial set of samples
  //used to build the initial GP are drawn from this should "ALWAYS" be
  //uniform in the input of the GP (even if the nominal distribution is not
  //uniform) because it is a set of samples to build a good GP and nothing
  //else.  Rho 0 is the nonminal distribution of the input variable

  ActiveSet gp_set = iteratedModel.current_response().active_set(); // copy
  gp_set.request_values(1); // no surr deriv evals, but GP may be grad-enhanced
  gpModel.assign_rep(new DataFitSurrModel(gpBuild, iteratedModel,
    gp_set, approx_type, approx_order, corr_type, corr_order, data_order,
    outputLevel, sample_reuse, import_pts_file,
    probDescDB.get_ushort("method.import_build_format"),
    probDescDB.get_bool("method.import_build_active_only"),
    probDescDB.get_string("method.export_approx_points_file"),
    probDescDB.get_ushort("method.export_approx_format")), false);
  vary_pattern = true; // allow seed to run among multiple approx sample sets
  // need to add to input spec
  numEmulEval = probDescDB.get_int("method.nond.samples_on_emulator");
  if (numEmulEval==0)
    numEmulEval = 10000;
  construct_lhs(gpEval, gpModel, sample_type, numEmulEval, randomSeed,
		rngName, vary_pattern);
  if (maxIterations < 0)
    numPtsAdd = 150;
  else
    numPtsAdd = maxIterations;

  //construct sampler to generate one draw from rhoOne distribution, with
  //seed varying between invocations
  construct_lhs(sampleRhoOne, iteratedModel, sample_type, 1, randomSeed,
		rngName, vary_pattern);
}

NonDGPImpSampling::~NonDGPImpSampling()
{ }

bool NonDGPImpSampling::resize()
{
  bool parent_reinit_comms = NonDSampling::resize();

  Cerr << "\nError: Resizing is not yet supported in method "
       << method_enum_to_string(methodName) << "." << std::endl;
  abort_handler(METHOD_ERROR);

  return parent_reinit_comms;
}

void NonDGPImpSampling::derived_init_communicators(ParLevLIter pl_iter)
{
  iteratedModel.init_communicators(pl_iter, maxEvalConcurrency);

  // gpBuild and gpEval use NoDBBaseConstructor, so no need to
  // manage DB list nodes at this level
  //gpBuild.init_communicators(pl_iter);
  gpEval.init_communicators(pl_iter);
}

void NonDGPImpSampling::derived_set_communicators(ParLevLIter pl_iter)
{
  NonD::derived_set_communicators(pl_iter);

  // gpBuild and gpEval use NoDBBaseConstructor, so no need to
  // manage DB list nodes at this level
  //gpBuild.set_communicators(pl_iter);
  gpEval.set_communicators(pl_iter);
}

void NonDGPImpSampling::derived_free_communicators(ParLevLIter pl_iter)
{
  gpEval.free_communicators(pl_iter);
  //gpBuild.free_communicators(pl_iter);

  iteratedModel.free_communicators(pl_iter, maxEvalConcurrency);
}

/** Calculate the failure probabilities for specified probability levels
    using Gaussian process based importance sampling. */
void NonDGPImpSampling::core_run()
{
  numPtsTotal = numSamples + numPtsAdd;

  // Build initial GP model.  This will be built over the initial LHS sample set
  // defined in the constructor.
  gpModel.build_approximation();

  gpCvars.resize(numEmulEval);
  RealVector temp_cvars;
  gpVar.resize(numEmulEval);
  gpMeans.resize(numEmulEval);
  indicator.resize(numPtsTotal);
  expIndicator.resize(numEmulEval);
  rhoDraw.resize(numEmulEval);
  normConst.resize(numPtsAdd);
  rhoMix.resize(numPtsTotal);
  //RealVector rhoEmul0(numEmulEval);
  //RealVector rhoEmul2(numEmulEval);
  int num_problem_vars=iteratedModel.acv();
  RealVector c_upper = iteratedModel.continuous_upper_bounds(),
             c_lower = iteratedModel.continuous_lower_bounds();

  int i,j,k;

// This piece of code prints out the build points, not the approximation points.
// const Pecos::SurrogateData& gp_data = gpModel.approximation_data(0);
//  for (j = 0; j < numSamples; j++) {
//    Cout << " Surrogate Vars " << gp_data.continuous_variables(j) << '\n';
//    Cout << " Surrogate Response " << gp_data.response_function(j);
//  }

// We have built the initial GP.  Now we need to go through, per response function
// and response level and calculate the failure probability.
// We will need to add error handling:  we will only be calculating
// results per response level, not probability level or reliability index.

  size_t resp_fn_count, level_count, iter;
  RealVector new_X;
  initialize_level_mappings();
  ParLevLIter pl_iter = methodPCIter->mi_parallel_level_iterator(miPLIndex);

  for (resp_fn_count=0; resp_fn_count<numFunctions; resp_fn_count++) {
    size_t num_levels = requestedRespLevels[resp_fn_count].length();
    const Pecos::SurrogateData& gp_data = gpModel.approximation_data(resp_fn_count);
    for (level_count=0; level_count<num_levels; level_count++) {
      Cout << "Starting calculations for response function " << resp_fn_count+1 << '\n';
      Cout << "Starting calculations for level  " << level_count+1 << '\n';
      Cout << "Threshold level is " << requestedRespLevels[resp_fn_count][level_count] << '\n';
      Real z = requestedRespLevels[resp_fn_count][level_count];
       // Calculate indicator over the true function evaluations
      double cdfMult = (cdfFlag?1.0:-1.0);
      for (j = 0; j < numSamples; j++) {
        indicator(j) = static_cast<double>((z-gp_data.response_function(j))*cdfMult>0.0);
        if (outputLevel > NORMAL_OUTPUT)
          Cout << "indicator(" << j << ")=" << indicator(j) << '\n';
      }

      // rho0 is the original PDF, rho1 is the compon importance pdf, rho2 is the
      // distribution used to generate candidate points for the emulator
      // For the initial implementation, we assume rho1 is uniform, and
      // rho0 and rho2 are uniform also with the same distribution.
      double rho0const, rho1const, rho2const;
      rho0const = 1.0;
      for (i = 0; i < num_problem_vars; i++)
        rho0const = rho0const/(c_upper[i]-c_lower[i]);
      rho1const = rho0const;
      rho2const = rho0const;

       // Here we loop over the number of points added,
       // where each time we calculate the expected indicator function
       // and add to the approximation.
      for (k = 0; k < numPtsAdd; k++) {
	// generate new set of emulator samples.
	// Note this will have a different seed each time.
        gpEval.run(pl_iter);
         // obtain results
        const RealMatrix&  all_samples = gpEval.all_samples();
        const IntResponseMap& all_resp = gpEval.all_responses();
        for (i = 0; i< numEmulEval; i++) {
          temp_cvars = Teuchos::getCol(Teuchos::View,
	    const_cast<RealMatrix&>(all_samples), i);
          gpCvars[i] = temp_cvars;
          //Cout << "input is " << gpCvars[i] << '\n';
	  // update gpModel currentVariables for use in approx_variances()
	  gpModel.continuous_variables(temp_cvars);
          gpVar[i]
	    = gpModel.approximation_variances(gpModel.current_variables());
          //Cout << "variance is " << gpVar[i];
        }

        IntRespMCIter resp_it = all_resp.begin();
        for (j=0, resp_it=all_resp.begin(); j<numEmulEval; ++j, ++resp_it) {
          RealVector temp_resp(numFunctions);
            for (i=0; i<numFunctions; i++)
              temp_resp(i) = resp_it->second.function_value(i);
            gpMeans[j]=temp_resp;
            //Cout << "output is " << gpMeans[j] << '\n';
        }

       // calculate expected indicator function;
        expIndicator = calcExpIndicator(resp_fn_count,z);
       // calculate distribution pdfs required to calculate the draw distribution
       // distribution_pdf(rhoEmul2);
       // distribution_pdf(rhoEmul0);
       // FOR NOW, we assume distribution 0 and 2 are the same,
       // but in general rhoDrawThis = expIndicator * rhoEmul0/rhoEmul2;
        for (i = 0; i< numEmulEval; i++)
          rhoDraw(i) = expIndicator(i)*rho0const/rho2const;

       // calculate the normalization constant
        Cout << "numEmulEval " << numEmulEval << '\n';
        Cout << "size exp indicator " << expIndicator.length() << '\n';
	double temp_sum_nc=0.0;
        normConst(k)=0.0;
        for (j = 0; j < numEmulEval; j++) {
	  temp_sum_nc+=rhoDraw(j);
        }
	temp_sum_nc/=numEmulEval;
        normConst(k)=temp_sum_nc;
        Real temp_norm_const = normConst(k);
        Cout << "norm const " << k << "=" << temp_sum_nc << "=" << normConst(k) <<  "\n";
       // calculate the draw distribution
        rhoDrawThis.resize(0);
        xDrawThis.resize(0);
        expIndThis.resize(0);
        calcRhoDraw();

        iter = 1;
        while ((iter<20) && (temp_norm_const*numEmulEval<25)) {
	  iter = iter+1;
          gpEval.run(pl_iter);
           // obtain results
          const RealMatrix&  this_samples = gpEval.all_samples();
          const IntResponseMap& this_resp = gpEval.all_responses();
          for (i = 0; i< numEmulEval; i++) {
	    temp_cvars = Teuchos::getCol(Teuchos::View,
	       const_cast<RealMatrix&>(this_samples), i);
            gpCvars[i] = temp_cvars;
            //Cout << "input is " << gpCvars[i] << '\n';
	    // update gpModel currentVariables for use in approx_variances()
	    gpModel.continuous_variables(temp_cvars);
            gpVar[i]
	      = gpModel.approximation_variances(gpModel.current_variables());
            //Cout << "variance is " << gpVar[i];
           }

          resp_it = this_resp.begin();
          for (j=0, resp_it=this_resp.begin(); j<numEmulEval; ++j, ++resp_it) {
            RealVector temp_resp(numFunctions);
            for (i=0; i<numFunctions; i++)
              temp_resp(i) = resp_it->second.function_value(i);
            gpMeans[j]=temp_resp;
            //Cout << "output is " << gpMeans[j] << '\n';
          }

       // calculate expected indicator function;
          expIndicator = calcExpIndicator(resp_fn_count,z);
          for (j = 0; j < numEmulEval; j++)
            rhoDraw(j)=expIndicator(j)*rho0const/rho2const;
          Real temp_norm_this=0.0;
          for (j = 0; j < numEmulEval; j++)
	    temp_norm_this+=rhoDraw(j);
          temp_norm_this/=numEmulEval;
          temp_norm_const = temp_norm_const+temp_norm_this;
          calcRhoDraw();
        }

       // xDrawThis, rhoDrawThis, and expIndThis should be populated now
        normConst(k)=temp_norm_const/iter;
        if (outputLevel > NORMAL_OUTPUT)
          Cout << "NormConst " << k << " =  " << normConst(k) << '\n';

        int num_eval_kept = xDrawThis.size();
        Real est_prob_hit_failregion;

        if (num_eval_kept==0) {
	  normConst(k)=0.0;
          sampleRhoOne.run(pl_iter);
         // obtain results
          const RealMatrix&  rho1_samples = sampleRhoOne.all_samples();
          // For now, we always only draw one sample
            new_X = Teuchos::getCol(Teuchos::View,
	      const_cast<RealMatrix&>(rho1_samples), 0);
            Cout << "Draw from Rho One is " << new_X << '\n';
        }
        else
          new_X = drawNewX(k);

         // add new_X to the build points and append approximation
        iteratedModel.continuous_variables(new_X);
        iteratedModel.evaluate();
        IntResponsePair resp_truth(iteratedModel.evaluation_id(),
                                   iteratedModel.current_response());
        gpModel.append_approximation(iteratedModel.current_variables(), resp_truth, true);
	indicator(numSamples+k) = static_cast<double>((z-gp_data.response_function(numSamples+k))*cdfMult>0.0);
        //if (gp_data.response_function(numSamples+k-1)<z)
	//indicator(numSamples+k-1)=1;
        //else indicator(numSamples+k-1)=0;
        Cout << "Done with iteration k ";
      }
      RealVectorArray gp_final_data(numPtsTotal);
      for (j = 0; j < numPtsTotal; j++)
        gp_final_data[j]=gp_data.continuous_variables(j);
      Cout << "GP final data size " <<  gp_final_data.size() << '\n';
//
//This is where we need some re-architecting.  I want to evaluate the GPmodel at a
//set of pre-defined points.  We will need to use a parameter list study.
//something like the following
//Iterator listStudy;
//The list of points will be the full X data, consisting of the original plus added points
//We have this stored in the Surrogate data
//VariablesArray list_points;
//list_points.resize(numPtsTotal);
//for (j = 0; j < numSamples; j++) {
//    list_points[j]=gp_data.continuous_variables(j);
//}
//
//listStudy.assign_rep(new ParamStudy(gpModel,"LIST", list_points));
//
      rhoOne.resize(numPtsTotal);
//not sure if I have this correct
//for now, since we are assuming rho0=rho1=rho2=all uniform, just set rho1 to 1.
      for (j = 0; j < numPtsTotal; j++)
        rhoOne(j)=rho1const; //for uniform this should be 1.0/prod(xmax-xmin) across all dimensions... ok now I see this isn't rhoOne it's rhoOne/rhoZero... if rho0=rho1=rho2 this should be ok

      for (j = 0; j < numPtsTotal; j++)
        rhoMix(j)=rhoOne(j)*numSamples;

      for (j = numPtsAdd-1; j>=0; j--){
        if (normConst(j)==0.0) {
          for (k = 0; k < numPtsTotal; k++)
            rhoMix(k)=rhoMix(k)+rhoOne(k);
        }
        else {
          RealVector this_mean;
          RealVector this_var;
          RealVector exp_ind_this(numPtsTotal);
          for (k = 0; k < numPtsTotal; k++){
            gpModel.continuous_variables(gp_final_data[k]);
            gpModel.evaluate();
            this_mean = gpModel.current_response().function_values();
            this_var
	      = gpModel.approximation_variances(gpModel.current_variables());
            exp_ind_this(k) = calcExpIndPoint(resp_fn_count,z,this_mean,this_var);
            if (outputLevel > NORMAL_OUTPUT)
              Cout << "exp_ind_final " << k << " " <<  exp_ind_this(k) << '\n';
          }
          for (k = 0; k < numPtsTotal; k++)
            rhoMix(k)=rhoMix(k)+exp_ind_this(k)*rho0const/normConst(j);
	  //the 1.0 here is reall rhoZero/rhoZero (ok for rho0=rho1=rho2)
          gpModel.pop_approximation(false, true);
          //gpModel.update_approximation(true);
          Cout << "Size of build data set " << gp_data.points() << '\n';
        }
//Since we need to evaluate the SUCCESSIVE SEQUENCES of GPs built using numSamples-->numPtsTotal
//it might be most efficient to "pop" the data and go backward:
//listStudy.run(pl_iter);
//obtain results and expected indicator functions
//gpModel.pop_approximation();
//
      }
      if (outputLevel > NORMAL_OUTPUT) {
        Cout << "rhoMix " << rhoMix << '\n';
        Cout << "indicator " << indicator << '\n';
      }
      for (j = 0; j < numPtsTotal; j++) {
	Real yada=rhoMix(j);
        rhoMix(j)=yada/numPtsTotal;
      }
      Real prob_mix=0.0;
      for (j = 0; j < numPtsTotal; j++)
        prob_mix+=rho0const*indicator(j)/rhoMix(j);
      //the 1.0 here is reall rhoZero/rhoZero (ok for rho0=rho1=rho2)
      prob_mix/=numPtsTotal;
      Cout << "Prob Mix IS " << prob_mix << '\n';

      Real fract_fail_mix = 0.0;
      for (j = 0; j < numPtsTotal; j++)
        fract_fail_mix+=indicator(j);
      fract_fail_mix/=numPtsTotal;
      Cout << "Fraction Fail IS " << fract_fail_mix << '\n';
      finalProb = prob_mix;
      computedProbLevels[resp_fn_count][level_count]=finalProb;
    }
  }
}

RealVector NonDGPImpSampling::drawNewX(int this_k)
{
  int i,j,templength;
  templength = xDrawThis.size();
  RealVector binEnds;
  binEnds.size(templength);
  Real cum_sum = 0.;
  Real est_prob_hit_failregion = 0;
  Real yada2;
  //Cout << "templength " << templength << '\n';
  for (i=0; i<templength; i++) {
    Real yada=rhoDrawThis(i);
    rhoDrawThis(i)=yada/normConst(this_k);
    if (i==0)
      binEnds(i)=rhoDrawThis(i);
    else{
      yada=rhoDrawThis(i);
      yada2=binEnds(i-1);
      binEnds(i)=yada+yada2;
    }
  }
  cum_sum = binEnds(templength-1);
  //Cout << "Cum Sum"  << cum_sum << '\n';
  //Cout << "BinEnds " << binEnds << '\n';
  //Cout << "RhoDrawThis " << rhoDrawThis << '\n';
  for (i=0; i<templength; i++) {
    yada2=binEnds(i);
    binEnds(i)=yada2/cum_sum;
  }
  //std::srand(randomSeed);
  double rand_cdf = (double)std::rand()/RAND_MAX;
  //Cout << "randcdf " << rand_cdf << '\n';
  bool found_cdf=false;
  i=0;
  while ((i<templength) && !found_cdf) {
    if (binEnds(i) > rand_cdf)
      found_cdf = true;
    else
      i = i+1;
  }
  for (j=0; j<templength; j++) {
    est_prob_hit_failregion+=expIndThis(j)*rhoDrawThis(j)/cum_sum;
  }
  Cout << "Estimated prob of hitting failure region " << est_prob_hit_failregion << '\n';
  return xDrawThis[i];
}

void NonDGPImpSampling::calcRhoDraw()
{
  int i, templength;
  templength = xDrawThis.size();

  for (i = 0; i<numEmulEval; i++) {
    if (expIndicator(i)!=0.0) {
      xDrawThis.resize(templength+1);
      expIndThis.resize(templength+1);
      rhoDrawThis.resize(templength+1);
      xDrawThis[templength]=gpCvars[i];
      expIndThis(templength)=expIndicator(i);
      // for now this is OK because rho0const = rho2const, will need to change this
      rhoDrawThis(templength)=expIndicator(i);
      templength=templength+1;
    }
  }

  //for (i = 0; i< templength; i++) {
  //  Cout << "xDrawThis  " << i << xDrawThis[i] << '\n';
  //  Cout << "rhoDrawThis  "  << rhoDrawThis[i] << '\n';
  //}
}

RealVector NonDGPImpSampling::calcExpIndicator(const int resp_fn_count, const Real respThresh)
{
  int i, j;
  RealVector ei(numEmulEval);

  Real cdf,snv,stdv;
  for (i = 0; i< numEmulEval; i++) {
    //Cout << "GPmean  " << gpMeans[i][resp_fn_count];
    //Cout << "GPvar  " << gpVar[i][resp_fn_count];
    snv = (respThresh-gpMeans[i][resp_fn_count])*(cdfFlag?1.0:-1.0);
    //this conditional sign maps the problem to the case where the mean being
    //"below" the threshold (i.e. snv > 0) indicates "mostly failure" and the
    //mean being "above" the threshold (i.e. snv < 0) indicates "mostly not
    //failure" this allows the mapped problem to ALWAYS use the cdf (instead
    //of complimentary cdf)

    stdv = std::sqrt(gpVar[i][resp_fn_count]);
    if(std::fabs(snv)>=std::fabs(stdv)*50.0) {
      //this will trap the denominator=0.0 case even if numerator=0.0
      ei(i)=(snv>=0.0)?1.0:0.0;
      //the mean being exactly at the threshold when variance=0.0 is
      //considered to indicate failure
    }
    else{
      snv/=stdv;
      ei(i)= Pecos::NormalRandomVariable::std_cdf(snv);
      //the expected indicator is the fraction of the mapped problem's cdf
      //that fails, the simple mapping is at most a change in sign of the
      //snv and might be the identity mapping (not even a change in sign)
    }

    //Cout << "EI " << ei(i) << " respThresh= " << respThresh << " mu= " << gpMeans[i][resp_fn_count] << " stdv= " << stdv << '\n';
  }
  return ei;
}

Real NonDGPImpSampling::calcExpIndPoint(const int resp_fn_count, const Real respThresh, const RealVector this_mean, const RealVector this_var)
{
  int i, j;
  Real ei;

  Real cdf,snv,stdv;
  //  Cout << "GPmean  " << this_mean(resp_fn_count);
  //  Cout << "GPvar  " << this_var(resp_fn_count);
  snv = (respThresh-this_mean(resp_fn_count))*(cdfFlag?1.0:-1.0);
    //this conditional sign maps the problem to the case where the mean being
    //"below" the threshold (i.e. snv > 0) indicates "mostly failure" and the
    //mean being "above" the threshold (i.e. snv < 0) indicates "mostly not
    //failure" this allows the mapped problem to ALWAYS use the cdf (instead
    //of complimentary cdf)
  stdv = std::sqrt(this_var(resp_fn_count));
  if(std::fabs(snv)>=std::fabs(stdv)*50.0) {
    //this will trap the denominator=0.0 case even if numerator=0.0
    ei=(snv>=0.0)?1.0:0.0;
    //the mean being exactly at the threshold when variance=0.0 is
    //considered to indicate failure
  }
  else{
    snv/=stdv;
    ei= Pecos::NormalRandomVariable::std_cdf(snv);
    //the expected indicator is the fraction of the mapped problem's cdf
    //that fails, the simple mapping is at most a change in sign of the
    //snv and might be the identity mapping (not even a change in sign)
  }

  //Cout << "EI " << ei << " respThresh= " << respThresh << " mu= " << this_mean(resp_fn_count) << " stdv= " << stdv << '\n';

  return ei;
}

void NonDGPImpSampling::print_results(std::ostream& s, short results_state)
{
  if (statsFlag) {
    s << "\nStatistics based on the importance sampling calculations:\n";
    print_level_mappings(s);
  }
}

} // namespace Dakota


/******************************************************************************
 * Copyright (C) 2014 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include "aslam/calibration/car/error-terms/ErrorTermVelocities.h"

#include <Eigen/Dense>

using namespace aslam::backend;

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

    ErrorTermVelocities::ErrorTermVelocities(const EuclideanExpression& r_v_mr,
        const EuclideanExpression& r_om_mr, const Input& r_v_mr_m, const Input&
        r_om_mr_m, const Covariance& sigma2_v, const Covariance& sigma2_om) :
        _r_v_mr(r_v_mr),
        _r_om_mr(r_om_mr),
        _r_v_mr_m(r_v_mr_m),
        _r_om_mr_m(r_om_mr_m),
        _sigma2_v(sigma2_v),
        _sigma2_om(sigma2_om) {
      Eigen::Matrix<double, 6, 6> sigma2 = Eigen::Matrix<double, 6, 6>::Zero();
      sigma2.topLeftCorner<3, 3>() = sigma2_v;
      sigma2.bottomRightCorner<3, 3>() = sigma2_om;
      setInvR(sigma2.inverse());
      DesignVariable::set_t dv;
      _r_v_mr.getDesignVariables(dv);
      _r_om_mr.getDesignVariables(dv);
      setDesignVariablesIterator(dv.begin(), dv.end());
    }

    ErrorTermVelocities::ErrorTermVelocities(const ErrorTermVelocities& other) :
        ErrorTermFs<6>(other),
        _r_v_mr(other._r_v_mr),
        _r_om_mr(other._r_om_mr),
        _r_v_mr_m(other._r_v_mr_m),
        _r_om_mr_m(other._r_om_mr_m),
        _sigma2_v(other._sigma2_v),
        _sigma2_om(other._sigma2_om) {
    }

    ErrorTermVelocities& ErrorTermVelocities::operator =
        (const ErrorTermVelocities& other) {
      if (this != &other) {
        ErrorTermFs<6>::operator=(other);
       _r_v_mr = other._r_v_mr;
       _r_om_mr = other._r_om_mr;
       _r_v_mr_m = other._r_v_mr_m;
       _r_om_mr_m = other._r_om_mr_m;
       _sigma2_v = other._sigma2_v;
       _sigma2_om = other._sigma2_om;
      }
      return *this;
    }

    ErrorTermVelocities::~ErrorTermVelocities() {
    }

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    double ErrorTermVelocities::evaluateErrorImplementation() {
      error_t error;
      error.head<3>() = _r_v_mr_m - _r_v_mr.toValue();
      error.tail<3>() = _r_om_mr_m - _r_om_mr.toValue();
      setError(error);
      return evaluateChiSquaredError();
    }

    void ErrorTermVelocities::evaluateJacobiansImplementation(JacobianContainer&
        jacobians) {
      Eigen::Matrix<double, 6, 3> Jv = Eigen::Matrix<double, 6, 3>::Zero();
      Jv.topLeftCorner<3, 3>() = Eigen::Matrix3d::Identity();
      _r_v_mr.evaluateJacobians(jacobians, -Jv);
      Eigen::Matrix<double, 6, 3> Jom = Eigen::Matrix<double, 6, 3>::Zero();
      Jom.bottomRightCorner<3, 3>() = Eigen::Matrix3d::Identity();
      _r_om_mr.evaluateJacobians(jacobians, -Jom);
    }

  }
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_GALLERY_ORTHOG_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_GALLERY_ORTHOG_HPP_INCLUDED
#include <nt2/linalg/functions/orthog.hpp>
#include <nt2/include/functions/whereij.hpp>
#include <nt2/include/functions/sin.hpp>
#include <nt2/include/functions/cos.hpp>
#include <nt2/include/functions/sinpi.hpp>
#include <nt2/include/functions/cospi.hpp>
#include <nt2/include/functions/ric.hpp>
#include <nt2/include/functions/cic.hpp>
#include <nt2/include/functions/cif.hpp>
#include <nt2/include/functions/rif.hpp>
#include <nt2/include/functions/tril.hpp>
#include <nt2/include/functions/from_diag.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/rec.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/twopi.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <nt2/include/functions/mul_i.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/divides.hpp>
#include <nt2/include/functions/is_eqz.hpp>
#include <nt2/include/functions/is_nez.hpp>
#include <nt2/include/functions/oneminus.hpp>
#include <nt2/include/functions/sqrt.hpp>
#include <nt2/include/functions/horzcat.hpp>
#include <nt2/include/functions/mtimes.hpp>
#include <nt2/include/functions/logical_and.hpp>
#include <boost/mpl/int.hpp>
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace nt2
{
  namespace details
  {
    struct isij0
    {
      template < class I, class J> inline
      typename meta::as_logical<I>::type operator()(const I & i , const J& j) const
      {
        return nt2::is_eqz(i*j);
      }
    };
    struct isi0
    {
      template < class I, class J> inline
      typename meta::as_logical<I>::type operator()(const I & i , const J& ) const
      {
        return nt2::is_eqz(i);
      }
    };
    struct isdiagpos
    {
      template < class I, class J> inline
      typename meta::as_logical<I>::type operator()(const I & i , const J& j ) const
      {
        return nt2::logical_and(nt2::is_nez(i), nt2::eq(i, j));
      }
    };
    template < class A0, class K, class T > struct orthog_return{};
  }
#define NT2_ORTHOG(V, K, Body)                                      \
  namespace ext                                                     \
  {                                                                 \
    NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::orthog##K##_, tag::cpu_,  \
                                (A0)(T),                            \
                                (scalar_<integer_<A0> >)            \
                                (target_<scalar_<floating_<T> > > ) \
                              )                                     \
    {                                                               \
      typedef typename T::type t_t;                                 \
      BOOST_DISPATCH_RETURNS(2, (A0 const& n,  T const& t),         \
                             Body                                   \
                            )                                       \
        };                                                          \
  }                                                                 \
  namespace details                                                 \
  {                                                                 \
    template < class A0, class T >                                  \
    struct orthog_return < A0,  boost::mpl::int_<V>, T>             \
      {                                                             \
        typedef typename nt2::meta::as_<T> Target;                  \
        typedef typename nt2::meta::call<nt2::tag::orthog##K##_(A0, \
                                               Target)>::type type; \
      };                                                            \
   }                                                                \
  template<class T>                                                 \
  typename details::orthog_return<ptrdiff_t,                        \
                                  boost::mpl::int_<V>, T>::type     \
                    orthog(ptrdiff_t n, const boost::mpl::int_<V>&) \
  {                                                                 \
    return nt2::orthog##K(n, meta::as_<T>());                       \
  }                                                                 \
/**/

  NT2_ORTHOG(1, 1, (nt2::sinpi(nt2::rif(n,T())*nt2::cif(n,T())/t_t(n+1))*nt2::sqrt(Two<t_t>()/t_t(n+1))))
  NT2_ORTHOG(2, 2, (nt2::sinpi(nt2::rif(n,T())*nt2::cif(n,T())*nt2::Two<t_t>()/t_t(2*n+1))*(Two<t_t>()/nt2::sqrt(t_t(2*n+1)))))
  NT2_ORTHOG(3, 3, (nt2::exp(nt2::mul_i(nt2::ric(n,T())*nt2::cic(n,T())*nt2::Twopi<t_t>()/t_t(n)))/nt2::sqrt(t_t(n))))
  NT2_ORTHOG(4, 4, (nt2::mtimes(nt2::from_diag(nt2::rec(nt2::sqrt(nt2::cath(t_t(n), _(t_t(1), t_t(n-1)))*_(t_t(1), t_t(n))))),
                                nt2::whereij(details::isdiagpos(),
                                             nt2::from_diag(nt2::oneminus(nt2::_(t_t(1), t_t(n)))),
                                             nt2::whereij(details::isi0(),
                                                          nt2::ones(n, T()),
                                                          nt2::tril(nt2::ones(n, T()))
                                                         )
                                            )
                               )
                   )
            )
  NT2_ORTHOG(5, 5,((nt2::cospi(nt2::ric(n,T())*nt2::cic(n,T())*nt2::Two<t_t>()/t_t(n))+
                    nt2::sinpi(nt2::ric(n,T())*nt2::cic(n,T())*nt2::Two<t_t>()/t_t(n)))/nt2::sqrt(t_t(n))))
  NT2_ORTHOG(6, 6, (nt2::cospi((nt2::rif(n, T())-Half<t_t>())*(nt2::cif(n, T())-Half<t_t>())/t_t(n))*nt2::sqrt(nt2::Two<t_t>()/t_t(n))))
  NT2_ORTHOG(7, 7, (nt2::whereij(details::isij0(), nt2::ones(n,T())/nt2::sqrt(t_t(n)),nt2::eye(n,T())-ones(n,T())*(1+rec(sqrt(t_t(n))))/t_t(n-1))))
  NT2_ORTHOG(-1, m1, (nt2::cospi(nt2::ric(n,T())*nt2::cic(n,T())*nt2::rec(nt2::max(t_t(1), t_t(n-1))))))
  NT2_ORTHOG(-2, m2, (nt2::cospi(nt2::ric(n,T())*(nt2::cic(n,T())+nt2::Half<t_t>())*nt2::rec(t_t(n)))))

#undef ORTHOG

  template<int K, class T>
  typename details::orthog_return<ptrdiff_t,  boost::mpl::int_<K>, T>::type
  orthog(ptrdiff_t n)
  {
    return nt2::orthog<T>(n, boost::mpl::int_<K>());
  }

}

#endif