/**
 * @file Solver.cpp
 * @author Giulio Romualdi
 * @copyright Released under the terms of the MIT License.
 * @date 2021
 */

#include <functional>
#include <numeric>

// clang-format off
// scs.h should be included before linsys/amatrix.h, since amatrix.h uses types
// scs_float, scs_int, etc, defined in scs.h
#include <scs.h>
#include <cones.h>
#include <linalg.h>
#include <util.h>
#include <amatrix.h>
// clang-format on

#include <ScsEigen/Logger.h>
#include <ScsEigen/Math.h>
#include <ScsEigen/MathematicalProgram.h>
#include <ScsEigen/Settings.h>
#include <ScsEigen/Solution.h>
#include <ScsEigen/Solver.h>
#include <ScsEigen/impl/SettingsImpl.h>

#include <Eigen/Sparse>

using namespace ScsEigen;

struct Solver::Impl
{
    Settings settings; /**< The settings of the ScsSolver */
    MathematicalProgram mathematicalProgram; /**< The mathematical problem that should be solved */
    Solution solution; /**< Solution of the conic problem */

    double constant{0}; /**< Constant term in the cost function. */
    std::vector<scs_float> gradient; /**< Gradient term in the cost function. */

    int optimizationVariables{0}; /**< Number of optimization variable */
    std::vector<scs_int> secondOrderConesSize; /**< Sizes of the second order cone. */

    Eigen::SparseMatrix<scs_float, 0, scs_int> constraintMatrix;
    std::vector<Eigen::Triplet<scs_float, scs_int>> constraintMatrixTriplets; /**< Triplets associated to the
                                                                     constraint matrix. */
    unsigned int constraintMatrixRows{0}; /**< Number of the rows of the constraint matrix */
    std::vector<scs_float> constraintVector; /**< vector containing the equality constraints */

    std::unique_ptr<ScsCone, std::function<void(ScsCone*)>> cone;
    std::unique_ptr<ScsData, std::function<void(ScsData*)>> data;
    std::unique_ptr<ScsSolution, std::function<void(ScsSolution*)>> scsSolution;

    Impl()
        : cone(static_cast<ScsCone*>(scs_calloc(1, sizeof(ScsCone))),
               [](ScsCone* ptr) {
                   // scs_free_data takes ScsData and ScsCone
                   // https://github.com/cvxgrp/scs/blob/48dfbe81caad2162c3ce5757faccdb3f3d31e142/src/util.c#L155
                   if (ptr != nullptr)
                       scs_free_data(nullptr, ptr);
               })
        , data(static_cast<ScsData*>(scs_calloc(1, sizeof(ScsData))),
               [](ScsData* ptr) {
                   // scs_free_data takes ScsData and ScsCone
                   // https://github.com/cvxgrp/scs/blob/48dfbe81caad2162c3ce5757faccdb3f3d31e142/src/util.c#L155
                   if (ptr != nullptr)
                       scs_free_data(ptr, nullptr);
               })
        , scsSolution(static_cast<ScsSolution*>(scs_calloc(1, sizeof(ScsSolution))),
                      [](ScsSolution* ptr) {
                          if (ptr != nullptr)
                              scs_free_sol(ptr);
                      })
    {
    }

    void rotateSecondOrderConeEmbedding(const std::vector<Eigen::Triplet<double>>& AConeTriplets,
                                        const Eigen::Ref<const Eigen::VectorXd>& bCone,
                                        const std::vector<int>& AiVarInded)
    {
        for (const auto& triplet : AConeTriplets)
        {
            const int xIndex = AiVarInded[triplet.col()];
            if (triplet.row() == 0)
            {
                this->constraintMatrixTriplets.emplace_back(this->constraintMatrixRows,
                                                            xIndex,
                                                            -0.5 * triplet.value());
                this->constraintMatrixTriplets.emplace_back(this->constraintMatrixRows + 1,
                                                            xIndex,
                                                            -0.5 * triplet.value());
            } else if (triplet.row() == 1)
            {
                this->constraintMatrixTriplets.emplace_back(this->constraintMatrixRows,
                                                            xIndex,
                                                            -0.5 * triplet.value());
                this->constraintMatrixTriplets.emplace_back(this->constraintMatrixRows + 1,
                                                            xIndex,
                                                            0.5 * triplet.value());
            } else
            {
                this->constraintMatrixTriplets.emplace_back(this->constraintMatrixRows
                                                                + triplet.row(),
                                                            xIndex,
                                                            -triplet.value());
            }
        }

        assert(bCone.size() >= 2
               && "Solver::Impl::rotateSecondOrderConeEmbedding] The size of the cone should be at "
                  "least 2.");

        this->constraintVector.push_back(0.5 * (bCone(0) + bCone(1)));
        this->constraintVector.push_back(0.5 * (bCone(0) - bCone(1)));
        for (int i = 2; i < bCone.rows(); ++i)
        {
            this->constraintVector.push_back(bCone(i));
        }
        this->constraintMatrixRows += bCone.rows();
        this->secondOrderConesSize.push_back(bCone.rows());
    }

    bool linearCostEmbedding()
    {
        for (const auto& [name, cost] : this->mathematicalProgram.getLinearCosts())
        {
            assert(cost->getNumberOfVariables() >= this->gradient.size());

            // the first cost->getNumberOfVariables() must be greater than zero
            Eigen::Map<Eigen::VectorXd>(this->gradient.data(),
                                        this->mathematicalProgram.numberOfVariables())
                += cost->getA();
            this->constant += cost->getB();
        }

        return true;
    }

    // A QuadraticCost encodes cost of the form
    //   0.5 xᵀQx + pᵀx + r
    // Since SCS only supports linear cost, we transform the cost using the epigraph form.
    // In details we introduce a new slack variable y as the upper bound of the cost, with the
    // rotated Lorentz cone constraint 2(y - r - pᵀx) ≥ xᵀQx.
    //    minimize_{x}  0.5 xᵀQx + pᵀx + r
    // becomes
    //    minimize_{x, y}  y
    //    subject to  2(y - r - pᵀx) ≥ xᵀQx.
    bool quadraticCostEmbedding()
    {
        std::vector<int> AiVarIndex(this->mathematicalProgram.numberOfVariables());
        std::iota(std::begin(AiVarIndex), std::end(AiVarIndex), 0);

        for (const auto& [name, cost] : this->mathematicalProgram.getQuadraticCosts())
        {
            const auto numberOfVariables = cost->getNumberOfVariables();

            // we create a vector containing the triplets of the matrix A_cone
            std::vector<Eigen::Triplet<double>> AConeTriplets;

            // Here we add the element in the first column latest row of the matrix A
            AConeTriplets.emplace_back(0, numberOfVariables, 1);

            // Set the constraint
            for (int i = 0; i < cost->getB().rows(); ++i)
            {
                AConeTriplets.emplace_back(0, i, -cost->getB()(i));
            }

            // Decompose Q in CᵀC
            // Q is always SDP. Please check QuadraticCost::setQ()

            const auto [outcome, C] = choleskyDecomposition(cost->getQ());
            if (!outcome)
            {
                log()->error("[Solver::Impl::quadraticCostEmbedding] Unable to compute the "
                             "Cholesky decomposition for the hessian matrix associated to the "
                             "quadratic cost named: "
                             + std::string(name) + ".");
                return false;
            }

            // get the element different from zero and store it in the triplets
            for (int i = 0; i < C.rows(); ++i)
                for (int j = 0; j < C.cols(); ++j)
                    if (C(i, j) != 0)
                        AConeTriplets.emplace_back(2 + i, j, C(i, j));

            // add the slack variables as optimization variable
            AiVarIndex.push_back(this->optimizationVariables);
            this->optimizationVariables++;

            Eigen::VectorXd bCone = Eigen::VectorXd::Zero(2 + C.cols());
            bCone(0) = -cost->getC();
            bCone(1) = 2;

            // Add the secondOrderCone to the constraints
            this->rotateSecondOrderConeEmbedding(AConeTriplets, bCone, AiVarIndex);

            // increase the gradient with the slack variable
            this->gradient.push_back(1);
        }

        return true;
    }

    bool linearConstraintEmbedding()
    {
        // The linear constraint lb ≤ Ax ≤ ub is converted to
        //
        //  Ax + s1 = ub,
        // -Ax + s2 = lb
        // s1, s2  are two vectors that belongs to the positive orthant.
        // If one of the element of lb or ub is equal to plus or minus infinity it is not added in
        // the constraint.
        unsigned int linearConstraintsRowIndex{0};
        for (const auto& [name, constraint] : this->mathematicalProgram.getLinearConstraints())
        {
            const Eigen::Ref<const Eigen::VectorXd> lowerBound = constraint->getLowerBound();
            const Eigen::Ref<const Eigen::VectorXd> upperBound = constraint->getUpperBound();

            const Eigen::Ref<const Eigen::MatrixXd> A = constraint->getA();
            const Eigen::Ref<const LinearConstraint::SparsityPattern> sparsityPattern = constraint->getASparsityPattern();

            // analyze each row of the matrix
            for (int i = 0; i < constraint->getNumberOfConstraints(); ++i)
            {
                const bool isLowerBoundFinite{!std::isinf(lowerBound(i))};
                const bool isUpperBoundFinite{!std::isinf(upperBound(i))};

                // if both of the elements are infinite the row is skipped
                if (isLowerBoundFinite || isUpperBoundFinite)
                {
                    const int lowerBoundRowIndex
                        = this->constraintMatrixRows + linearConstraintsRowIndex;
                    const int upperBoundRowIndex
                        = lowerBoundRowIndex + (isLowerBoundFinite ? 1 : 0);
                    for (int j = 0; j < this->mathematicalProgram.numberOfVariables(); ++j)
                    {
                        if (sparsityPattern(i, j))
                        {
                            if (isLowerBoundFinite)
                            {
                                this->constraintMatrixTriplets.emplace_back(lowerBoundRowIndex,
                                                                            j,
                                                                            -A(i, j));
                            }

                            if (isUpperBoundFinite)
                            {
                                this->constraintMatrixTriplets.emplace_back(upperBoundRowIndex,
                                                                            j,
                                                                            A(i, j));
                            }
                        }
                    }
                    if (isLowerBoundFinite)
                    {
                        this->constraintVector.push_back(-lowerBound(i));
                        linearConstraintsRowIndex++;
                    }
                    if (isUpperBoundFinite)
                    {
                        this->constraintVector.push_back(upperBound(i));
                        linearConstraintsRowIndex++;
                    }
                }
            }
        }

        this->constraintMatrixRows += linearConstraintsRowIndex;
        this->cone->l += linearConstraintsRowIndex;
        return true;
    }

    // A QuadraticConstraint encodes constraint of the form
    //   0.5 xᵀQx + pᵀx < u
    // We transform the constraint in a rotated Lorentz cone 2(u - pᵀx) ≥ xᵀQx.
    bool quadraticConstraintEmbedding()
    {
        std::vector<int> AiVarIndex(this->mathematicalProgram.numberOfVariables());
        std::iota(std::begin(AiVarIndex), std::end(AiVarIndex), 0);

        for (const auto& [name, constraint] : this->mathematicalProgram.getQuadraticConstraints())
        {

            if (!constraint->getLowerBound().array().isInf().all())
            {
                log()->error("[Solver::Impl::quadraticConstraintEmbedding] The only admissible "
                             "lower bound is -inf. Constraint name: "
                             + std::string(name) + ".");
                return false;
            }

            const auto numberOfVariables = constraint->getNumberOfVariables();

            // we create a vector containing the triplets of the matrix A_cone
            std::vector<Eigen::Triplet<double>> AConeTriplets;

            // Set the constraint
            for (int i = 0; i < constraint->getB().rows(); ++i)
            {
                AConeTriplets.emplace_back(0, i, -constraint->getB()(i));
            }

            // Decompose Q in CᵀC
            // Q is always SDP. Please check QuadraticConstraint::setQ()
            const auto [outcome, C] = choleskyDecomposition(constraint->getQ());
            if (!outcome)
            {
                log()->error("[Solver::Impl::quadraticConstraintEmbedding] Unable to compute the "
                             "Cholesky decomposition for the hessian matrix associated to the "
                             "quadratic constraint named: "
                             + std::string(name) + ".");
                return false;
            }

            // get the element different from zero and store it in the triplets
            for (int i = 0; i < C.rows(); ++i)
                for (int j = 0; j < C.cols(); ++j)
                    if (C(i, j) != 0)
                        AConeTriplets.emplace_back(2 + i, j, C(i, j));

            // add the slack variables as optimization variable
            AiVarIndex.push_back(this->optimizationVariables);

            Eigen::VectorXd bCone = Eigen::VectorXd::Zero(2 + C.cols());
            bCone(0) = constraint->getUpperBound()[0];
            bCone(1) = 2;

            // Add the secondOrderCone to the constraints
            this->rotateSecondOrderConeEmbedding(AConeTriplets, bCone, AiVarIndex);
        }

        return true;
    }

    void prepareScsData()
    {
        this->data->n = this->optimizationVariables;
        this->data->m = this->constraintMatrixRows;

        this->data->A = static_cast<ScsMatrix*>(malloc(sizeof(ScsMatrix)));
        this->data->A->x = static_cast<scs_float*>(
            scs_calloc(this->constraintMatrix.nonZeros(), sizeof(scs_float)));

        this->data->A->i = static_cast<scs_int*>(scs_calloc(this->constraintMatrix.nonZeros(), //
                                                            sizeof(scs_int)));
        this->data->A->p = static_cast<scs_int*>(scs_calloc(this->data->n + 1, sizeof(scs_int)));

        this->data->A->m = this->data->m;
        this->data->A->n = this->data->n;

        std::memcpy(this->data->A->x,
                    this->constraintMatrix.valuePtr(),
                    this->constraintMatrix.nonZeros() * sizeof(scs_float));

        std::memcpy(this->data->A->i,
                    this->constraintMatrix.innerIndexPtr(),
                    this->constraintMatrix.nonZeros() * sizeof(scs_int));

        std::memcpy(this->data->A->p,
                    this->constraintMatrix.outerIndexPtr(),
                    (this->data->n + 1) * sizeof(scs_int));

        this->data->b = static_cast<scs_float*>(scs_calloc(this->constraintVector.size(), //
                                                           sizeof(scs_float)));

        std::memcpy(this->data->b,
                    this->constraintVector.data(),
                    this->constraintVector.size() * sizeof(scs_float));

        this->data->c = static_cast<scs_float*>(scs_calloc(this->optimizationVariables, //
                                                           sizeof(scs_float)));
        std::memcpy(this->data->c,
                    this->gradient.data(),
                    this->optimizationVariables * sizeof(scs_float));

        // Set the parameters to default values.
        this->data->stgs = static_cast<ScsSettings*>(scs_calloc(1, sizeof(ScsSettings)));

        // we should change this
        scs_set_default_settings(this->data.get());
    }
};

Solver::Solver()
    : m_pimpl(std::make_unique<Impl>())
{
}

Solver::~Solver() = default;

Settings& Solver::settings()
{
    return m_pimpl->settings;
}

const Settings& Solver::settings() const
{
    return m_pimpl->settings;
}

MathematicalProgram& Solver::mathematicalProgram()
{
    return m_pimpl->mathematicalProgram;
}

const MathematicalProgram& Solver::mathematicalProgram() const
{
    return m_pimpl->mathematicalProgram;
}

bool Solver::solve()
{
    if (m_pimpl->mathematicalProgram.numberOfVariables() == 0)
    {
        log()->error("[Solver::solve] The number of variables is equal to zero. Please set the "
                     "number of variables.");
        return false;
    }

    // The number of optimization variables may changes accordingly to the constraints and costs
    m_pimpl->optimizationVariables = m_pimpl->mathematicalProgram.numberOfVariables();

    // reset all the gradient vector equal to zero
    m_pimpl->gradient.resize(m_pimpl->optimizationVariables, 0.0);

    // perform the embedding
    if (!m_pimpl->linearCostEmbedding())
    {
        log()->error("[Solver::solve] Unable to perform linear cost embedding.");
        return false;
    }

    if (!m_pimpl->linearConstraintEmbedding())
    {
        log()->error("[Solver::solve] Unable to perform linear constraint embedding.");
        return false;
    }

    if (!m_pimpl->quadraticConstraintEmbedding())
    {
        log()->error("[Solver::solve] Unable to perform quadratic constraint embedding.");
        return false;
    }

    if (!m_pimpl->quadraticCostEmbedding())
    {
        log()->error("[Solver::solve] Unable to perform quadratic cost embedding.");
        return false;
    }

    // set the second-order cone length in the SCS cone struct
    // Here dynamic allocation is performed.
    // The moeory will be automatically deallocated when Impl::cone goes out of scope
    m_pimpl->cone->qsize = m_pimpl->secondOrderConesSize.size();
    m_pimpl->cone->q = static_cast<scs_int*>(scs_calloc(m_pimpl->cone->qsize, sizeof(scs_int)));

    for (unsigned int i = 0; i < m_pimpl->cone->qsize; i++)
    {
        m_pimpl->cone->q[i] = m_pimpl->secondOrderConesSize[i];
    }

    // prepare the constraint matrix
    m_pimpl->constraintMatrix.resize(m_pimpl->constraintMatrixRows, m_pimpl->optimizationVariables);
    m_pimpl->constraintMatrix.setFromTriplets(m_pimpl->constraintMatrixTriplets.begin(),
                                              m_pimpl->constraintMatrixTriplets.end());

    m_pimpl->constraintMatrix.makeCompressed();

    m_pimpl->prepareScsData();

    // TODO (GR) handle the settings
    ScsInfo info{0};

    // solve the problem
    m_pimpl->solution.status = Solution::getStatus(scs(m_pimpl->data.get(), //
                                                       m_pimpl->cone.get(),
                                                       m_pimpl->scsSolution.get(),
                                                       &info));

    // copy the content of scs info.
    m_pimpl->solution.iteration = info.iter;
    m_pimpl->solution.primalObjective = info.pobj;
    m_pimpl->solution.dualObjective = info.dobj;
    m_pimpl->solution.primalResidue = info.res_pri;
    m_pimpl->solution.residueInfeasibility = info.res_infeas;
    m_pimpl->solution.residueUnbounded = info.res_unbdd;
    m_pimpl->solution.relativeDualityGap = info.rel_gap;
    m_pimpl->solution.setupTime = info.setup_time;
    m_pimpl->solution.solveTime = info.solve_time;

    m_pimpl->solution.dualVariable = Eigen::Map<Eigen::Matrix<scs_float, Eigen::Dynamic, 1>>( //
        m_pimpl->scsSolution->y,
        m_pimpl->constraintMatrixRows);

    m_pimpl->solution.primalEqualitySlack
        = Eigen::Map<Eigen::Matrix<scs_float, //
                                   Eigen::Dynamic,
                                   1>>(m_pimpl->scsSolution->s, m_pimpl->constraintMatrixRows);

    if (m_pimpl->solution.status == Solution::Status::solved
        || m_pimpl->solution.status == Solution::Status::solved_inaccurate)
    {
        m_pimpl->solution.completeSolution
            = Eigen::Map<Eigen::Matrix<scs_float, //
                                       Eigen::Dynamic,
                                       1>>(m_pimpl->scsSolution->x, m_pimpl->optimizationVariables);

        m_pimpl->solution.solution = m_pimpl->solution.completeSolution.head(
            m_pimpl->mathematicalProgram.numberOfVariables());

        return true;
    }

    log()->error("[Solver::Solve] Unable to find a feasible solution. Please inspect the "
                 "Solution struct.");
    return false;
}

const Solution& Solver::solution() const
{
    return m_pimpl->solution;
}


/*
 * Copyright 2017-2019 Janek Bevendorff, Webis Group
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "ComputeCostH.hpp"
#include "State.hpp"
#include "util/dekker.hpp"

#include <cassert>
#include <numeric>
#include <boost/optional.hpp>

/**
 * Compute h(n) heuristic function based on the Jensen-Shannon divergence
 * between two n-gram distributions.
 *
 * @param node node to calculate h(n) for
 * @param context search context
 * @param whether to allow approximate cost updates (faster) or only exact recalculations
 * @return computed cost
 */
double ComputeCostH::operator()(search::generic::Node<State> const& node, Context const& context, bool allowUpdate) const
{
    auto const& state = node.state();

    auto const sourceProfile = state.ngramProfile();
    auto const targetProfile = context.targetNgramProfile;

    double jsd;
//    if (!allowUpdate || !state.mutableMetaData()->jsd || node.depth() % 5 == 0 || state.ngramProfile()->lastUpdates().empty()) {
        jsd = calculateJsd(sourceProfile, targetProfile);

//    } else {
//        jsd = calculateJsdUpdate(state.mutableMetaData()->jsd.get(), state.ngramProfile()->lastUpdates(), sourceProfile, targetProfile);
//    }

    if (jsd > 1.0) {
        std::cerr << "ERROR: Numerical underflow, jsd = " << jsd << std::endl;
    }

    assert(jsd <= 1.0);
    state.mutableMetaData()->jsd = jsd;

    double origJsd;
    if (!context.mutableMetaData->originalJsd) {
        origJsd = std::max(0.0, jsd - 1.0e-10);
        context.mutableMetaData->originalJsd = origJsd;
    } else {
        origJsd = context.mutableMetaData->originalJsd.get();
    }
    double const jsDistance = std::sqrt(2.0 * jsd);
    double const goal = context.mutableMetaData->goalJSDist.get();

    double p = node.costG() / std::max(1.0e-6, jsDistance - std::sqrt(2.0 * origJsd));
    double r = std::max(0.0, goal - jsDistance);
    double h = r * p;

    return h;
}

namespace {
inline double logAdd(double s1, double s2)
{
    return s1 + std::log(1.0 + std::exp(s2 - s1));
}
}

/**
 * Calculate the Jensen-Shannon divergence between two n-gram profiles.
 */
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
double ComputeCostH::calculateJsd(Context::ConstNgramPtr const& sourceProfile, Context::ConstNgramPtr const& targetProfile) const
{
    auto pNorm = static_cast<double>(targetProfile->n());
    auto qNorm = static_cast<double>(sourceProfile->n());
    auto pIt = targetProfile->cbegin();
    auto qIt = sourceProfile->cbegin();
    auto const pEnd = targetProfile->cend();
    auto const qEnd = sourceProfile->cend();

    dekker::Double<double> jsdP = 0.0;
    dekker::Double<double> jsdQ = 0.0;

    double const logHalf = std::log(0.5);

    boost::optional<NgramProfile::NgramPair> pDeref;
    boost::optional<NgramProfile::NgramPair> qDeref;
    while (pIt != pEnd || qIt != qEnd) {
        double p = 1.0;
        double q = 1.0;

        pDeref = boost::none;
        if (pIt != pEnd) {
            pDeref = *pIt;
        }

        qDeref = boost::none;
        if (qIt != qEnd) {
            qDeref = *qIt;
        }

        if ((pDeref && !qDeref) || (pDeref && pDeref->first < qDeref->first)) {
            p = pDeref->second != 0 ? std::log(pDeref->second) - std::log(pNorm) : 1.0;
           ++pIt;
        } else if ((qDeref && !pDeref) || (qDeref && pDeref->first > qDeref->first)) {
            q = qDeref->second != 0 ? std::log(qDeref->second) - std::log(qNorm) : 1.0;
            ++qIt;
        } else if (pDeref && qDeref && pDeref->first == qDeref->first) {
            p = pDeref->second != 0 ? std::log(pDeref->second) - std::log(pNorm) : 1.0;
            q = qDeref->second != 0 ? std::log(qDeref->second) - std::log(qNorm) : 1.0;

            ++pIt;
            ++qIt;
        } else {
            assert(false);
        }

        double m;
        if (p <= 0.0 && q <= 0.0) {
            m = logHalf + logAdd(p, q);
        } else {
            m = logHalf + std::min(p, q);
        }

        if (p <= 0.0) {
            jsdP += std::exp(p) * std::log2(std::exp(p - m));
        }

        if (q <= 0.0) {
            jsdQ += std::exp(q) * std::log2(std::exp(q - m));
        }
    }

    return 0.5 * static_cast<double>(jsdP + jsdQ);
}

/**
 * Update the previous Jensen-Shannon divergence from a difference vector.
 * The updated value is only approximate and needs to be corrected after a few iterations.
 *
 * @param previousJsd previous JSD value
 * @param updates vector of n-gram count updates
 * @param sourceProfile new source profile
 * @param targetProfile target profile
 * @return approximate new JSD
 */
double ComputeCostH::calculateJsdUpdate(double previousJsd, std::vector<NgramProfile::NgramUpdate> const& updates,
        Context::ConstNgramPtr const& sourceProfile, Context::ConstNgramPtr const& targetProfile) const
{
    std::size_t const oldQN = sourceProfile->n();
    int newQN = 0;
    std::unordered_map<NgramProfile::Ngram, int> updatesMap;
    for (auto& update: updates) {
        newQN += update.second;
        updatesMap[update.first] += update.second;
    }
    newQN = oldQN + newQN;
    assert(newQN > 0);

    double const logHalf = std::log(0.5);
    double const newQNLog = std::log(newQN);
    double const oldQNLog = std::log(oldQN);

    dekker::Double<double> oldJsdDiff = 0.0;
    dekker::Double<double> newJsdDiff = 0.0;
    for (auto const& update: updatesMap) {
        double pNorm = targetProfile->normFreq(update.first);
        double oldQ = sourceProfile->freq(update.first);
        double newQ = oldQ + update.second;
        assert(newQ >= 0);

        pNorm = pNorm != 0.0 ? std::log(pNorm) : 1.0;
        double newQNorm = newQ > 0 ? std::log(newQ) - newQNLog : 1.0;
        double oldQNorm = oldQ > 0 ? std::log(oldQ) - oldQNLog : 1.0;

        double newMNorm = 1.0;
        if (pNorm <= 0.0 && newQNorm <= 0.0) {
            newMNorm = logHalf + logAdd(pNorm, newQNorm);
        } else if (pNorm <= 0.0 || newQNorm <= 0.0) {
            newMNorm = logHalf + std::min(pNorm, newQNorm);
        }

        double oldMNorm = 1.0;
        if (pNorm <= 0.0 && oldQNorm <= 0.0) {
            oldMNorm = logHalf + logAdd(pNorm, oldQNorm);
        } else if (pNorm <= 0.0 || oldQNorm <= 0.0) {
            oldMNorm = logHalf + std::min(pNorm, oldQNorm);
        }

        // new diff
        if (newMNorm <= 0.0) {
            if (pNorm <= 0.0)
                newJsdDiff += std::exp(pNorm) * std::log2(std::exp(pNorm - newMNorm));

            if (newQNorm <= 0.0)
                newJsdDiff += std::exp(newQNorm) * std::log2(std::exp(newQNorm - newMNorm));
        }

        // old diff
        if (oldMNorm <= 0.0) {
            if (pNorm <= 0.0)
                oldJsdDiff -= std::exp(pNorm) * std::log2(std::exp(pNorm - oldMNorm));

            if (oldQNorm <= 0.0)
                oldJsdDiff -= std::exp(oldQNorm) * std::log2(std::exp(oldQNorm - oldMNorm));
        }
    }

    return previousJsd + 0.5 * static_cast<double>(oldJsdDiff + newJsdDiff);
}


#include <iostream>
#include <ros/ros.h>

#include <libgp/include/gp.h>
#include <Eigen/Dense>

#include <math.h>

using namespace libgp;

int main(int argc, char **argv) {
  //Initiate the ROS system and become a node.
  ros::init(argc, argv, "gp_node");
  ros::NodeHandle nh;

  ROS_INFO("gp_ros started");

  //initial gp
  GaussianProcess gp(2, "CovSum ( CovSEiso, CovNoise)");
  Eigen::VectorXd params(gp.covf().get_param_dim());
  params << 0.0, 0.0, -2.0;
  // set parameters of covariance function
  gp.covf().set_loghyper(params);

  ROS_INFO("gp initilaized ");

  ros::spin();

  return 0;
}


/*
*
*	Author: Philipp Zschoche
*
*/
#ifndef __MEMORY_LOGGER_HPP__
#define __MEMORY_LOGGER_HPP__

#include "logger.hpp"
#include <atomic>
#include <stdexcept>
#include <boost/detail/lightweight_mutex.hpp>
#include <cmath>

namespace mlog {

struct memory_entry {
	log_metadata metadata;
	std::string text;
};

#ifdef _MSC_VER
unsigned long log2(unsigned long x, unsigned long count = 0)
#else
    constexpr unsigned long log2(unsigned long x, unsigned long count = 0)
#endif
{
	return x < 2 ? count : log2((x >> 1), count + 1);
}

// writing is already thread-safe.
template <unsigned long N> class memory_logger : public logger {
      public:

	memory_logger() : logger(), m_size(N), m_offset(0), m_use_mutex(false)  {
#ifdef _MSC_VER
		unsigned long bits = mlog::log2(N - 1) + 1;
		unsigned long mask = ((1 << bits) - 1);
		if ((N - 1) != mask)
			throw std::invalid_argument("N is not valid. Try 4, 8, "
						    "16, 32 ... 1024, 2048, "
						    "4096... and so on.");
#else
		constexpr unsigned long bits = mlog::log2(N - 1) + 1;
		constexpr unsigned long mask = ((1 << bits) - 1);
		static_assert((N - 1) == mask, "N is not valid. Try 4, 8, 16, "
					       "32 ... 1024, 2048, 4096... and "
					       "so on.");
#endif
	}

	inline bool is_lock_free() {
		return m_use_mutex.is_lock_free() && m_offset.is_lock_free();
	}

	virtual ~memory_logger() {}

	void write_to_log(log_metadata &&metadata,
			  std::string &&log_text) {

		std::size_t offset = get_offset();
		memory_entry& entry = m_log_entrys[offset];
		//entry.metadata = std::move(metadata); //odd errors on clang
		//entry.text = std::move(log_text); //odd errors on clang
		entry.metadata = metadata;
		entry.text = log_text;
	}

	void write_to_log(const log_metadata& metadata,
				  const std::string& log_text) {

		std::size_t offset = get_offset();
		memory_entry& entry = m_log_entrys[offset];
		entry.metadata = metadata;
		entry.text = log_text;
	}

	std::ostream &output(std::ostream &stream) {
		m_offset_temp = 0;
		boost::detail::lightweight_mutex::scoped_lock lock(m_mutex);
		m_use_mutex = false;
		std::size_t offset = m_offset & (N - 1);

		for (unsigned long index = offset; index < N; index++)
			m_log_entrys[index].metadata.output(stream)
			    << m_log_entrys[index].text << std::endl;

		for (unsigned long index = 0; index < offset; index++)
			m_log_entrys[index].metadata.output(stream)
			    << m_log_entrys[index].text << std::endl;

		return stream;
	}

	const memory_entry &operator[](std::size_t const &index) const {
		return m_log_entrys[(m_offset + index) & N_1];
	}

	inline unsigned long size() const {
		return m_size;
	}

      private:

	inline unsigned long get_offset() {
		if(m_use_mutex) {
			auto r = m_offset + m_offset_temp.fetch_add(1) & N_1;
			boost::detail::lightweight_mutex::scoped_lock lock(m_mutex);
			return r;
		} else
			return  m_offset.fetch_add(1) & N_1;
	}

	const unsigned long m_size;

	static const unsigned long N_1 = N - 1;

	memory_entry m_log_entrys[N];
	std::atomic<unsigned long> m_offset;
	std::atomic<unsigned long> m_offset_temp;
	std::atomic<bool> m_use_mutex;
	boost::detail::lightweight_mutex m_mutex;
};

typedef memory_logger<4096> memory_logger_normal;
typedef memory_logger<static_cast<unsigned short>(-1)> memory_logger_big;

typedef memory_logger_normal memory_logger_normal_thread_safe; // memory_logger
							       // is already
							       // thread-safe.
typedef memory_logger_big memory_logger_big_thread_safe; // memory_logger is
							 // already thread-safe.

#ifdef _MSC_VER
template <unsigned long N>
std::ostream &operator<<(std::ostream &lhs, memory_logger<N> &rhs)
#else
    template <unsigned long N>
constexpr std::ostream &operator<<(std::ostream &lhs, memory_logger<N> &rhs)
#endif
{
	return rhs.output(lhs);
}

} /* mlog */

#endif /* __MEMORY_LOGGER_HPP__ */


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

#include <iostream>

#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/transposed_view.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/glas_tag.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/maybe.hpp>
#include <boost/numeric/mtl/utility/complexity.hpp>
#include <boost/numeric/mtl/operation/print_matrix.hpp>
#include <boost/numeric/mtl/io/test_ostream.hpp>

using namespace std;

template <typename Matrix>
void symmetry_test(const Matrix& A)
{
    namespace traits = mtl::traits;

    typename traits::row<Matrix>::type                                 row(A);
    typename traits::col<Matrix>::type                                 col(A);
    typename traits::const_value<Matrix>::type                         value(A);
    typedef typename traits::range_generator<mtl::tag::major, Matrix>::type        cursor_type;
    for (cursor_type cursor = mtl::begin<mtl::tag::major>(A), cend = mtl::end<mtl::tag::major>(A);
	 cursor != cend; ++cursor) {
	typedef mtl::tag::nz     inner_tag;
	mtl::io::tout << "---\n";
	typedef typename traits::range_generator<inner_tag, cursor_type>::type icursor_type;
	for (icursor_type icursor = mtl::begin<inner_tag>(cursor), icend = mtl::end<inner_tag>(cursor); icursor != icend; ++icursor) {
	    mtl::io::tout << "A[" << row(*icursor) << ", " << col(*icursor) << "] = " << value(*icursor) << '\n';
	    MTL_THROW_IF(!A.indexer(A, col(*icursor), row(*icursor)), mtl::runtime_error("Symmetric counter-part not found"));
	}
    }
}

template <typename Matrix>
void matrix_init(Matrix& matrix)
{
    typedef typename Matrix::parameters   parameters;
    typedef typename Matrix::value_type   value_type;

    mtl::mat::compressed2D_inserter<value_type, parameters> inserter(matrix);
    inserter(1, 1) << 0;
    inserter(2, 2) << 7; inserter(1, 4) << 3; inserter(3, 2) << 9; inserter(5, 1) << 5;
}

template <typename Orientation>
void test_compressed2D(char const* name)
{
    mtl::io::tout << "\n====================\n" << name << "\n====================\n";
    typedef mtl::mat::parameters<Orientation>   parameters;
    typedef mtl::compressed2D<int, parameters>     matrix_type;
    matrix_type                                    A(6, 6);

    matrix_init(A);
    mtl::io::tout << "A is\n" << A;
    mtl::io::tout << "Has " << A.nnz() << " non-zeros.\n";

    A.make_symmetric_pattern();
    mtl::io::tout << "A after symmetrizing is\n" << A;
    mtl::io::tout << "Has " << A.nnz() << " non-zeros.\n";
    symmetry_test(A);
}

int main(int, char**)
{
    test_compressed2D<mtl::row_major>("CRS");
    test_compressed2D<mtl::col_major>("CCS");

    return 0;
}


//  (C) Copyright Jeremy Siek 2002.
//  Permission to copy, use, modify, sell and distribute this software
//  is granted provided this copyright notice appears in all
//  copies. This software is provided "as is" without express or
//  implied warranty, and with no claim as to its suitability for any
//  purpose.

#include <boost/pending/disjoint_sets.hpp>
#include <boost/test/test_tools.hpp>
#include <boost/cstdlib.hpp>

template <typename DisjointSet>
struct test_disjoint_set {
  static void do_test()
  {
    // The following tests are pretty lame, just a basic sanity check.
    // Industrial strength tests still need to be written.

#if !defined(__MWERKS__) || __MWERKS__ > 0x3003
    std::size_t elts[]
#else
    std::size_t elts[4]
#endif
        = { 0, 1, 2, 3 };

    const int N = sizeof(elts)/sizeof(*elts);

    DisjointSet ds(N);

    ds.make_set(elts[0]);
    ds.make_set(elts[1]);
    ds.make_set(elts[2]);
    ds.make_set(elts[3]);

    BOOST_CHECK(ds.find_set(0) != ds.find_set(1));
    BOOST_CHECK(ds.find_set(0) != ds.find_set(2));
    BOOST_CHECK(ds.find_set(0) != ds.find_set(3));
    BOOST_CHECK(ds.find_set(1) != ds.find_set(2));
    BOOST_CHECK(ds.find_set(1) != ds.find_set(3));
    BOOST_CHECK(ds.find_set(2) != ds.find_set(3));

    ds.union_set(0, 1);
    ds.union_set(2, 3);
    BOOST_CHECK(ds.find_set(0) != ds.find_set(3));
    int a = ds.find_set(0);
    BOOST_CHECK(a == ds.find_set(1));
    int b = ds.find_set(2);
    BOOST_CHECK(b == ds.find_set(3));

    ds.link(a, b);
    BOOST_CHECK(ds.find_set(a) == ds.find_set(b));
    BOOST_CHECK(1 == ds.count_sets(elts, elts + N));

    ds.normalize_sets(elts, elts + N);
    ds.compress_sets(elts, elts + N);
    BOOST_CHECK(1 == ds.count_sets(elts, elts + N));
  }
};

int
test_main(int, char*[])
{
  using namespace boost;
  {
    typedef
      disjoint_sets_with_storage<identity_property_map, identity_property_map,
      find_with_path_halving> ds_type;
    test_disjoint_set<ds_type>::do_test();
  }
  {
    typedef
      disjoint_sets_with_storage<identity_property_map, identity_property_map,
      find_with_full_path_compression> ds_type;
    test_disjoint_set<ds_type>::do_test();
  }
  return boost::exit_success;
}


//==============================================================================
//         Copyright 2014          LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2014          NumScale SAS
//
//          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
//==============================================================================
#define BOOST_TEST_MODULE odeint_nt2_algebra_dispatcher

#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/numeric/odeint/external/nt2/nt2_algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/mpl/list.hpp>

#include <boost/preprocessor/repetition.hpp>
#include <boost/preprocessor/arithmetic/mul.hpp>

#include <nt2/table.hpp>
#include <nt2/sdk/meta/as.hpp>
#include <nt2/include/functions/ones.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

typedef boost::mpl::list< float , double > fp_types;

#define TABLE(z,n,text) nt2::table<T> y ## n =                                   \
                        nt2::ones(1,2,nt2::meta::as_<T>() )*T(BOOST_PP_ADD(n,1));

#define PARAMS(z,n,text) T(BOOST_PP_ADD(n,1)),

#define SUM(z,n,text) +BOOST_PP_MUL(BOOST_PP_ADD(n,3),BOOST_PP_ADD(n,2))

#define TEST(z,n,text) BOOST_CHECK_SMALL( y0(BOOST_PP_ADD(n,1))                  \
                       -T( 2 BOOST_PP_REPEAT(text, SUM, text) ), T(1e-10) );

#define TEST_CASE(z,n,text) BOOST_AUTO_TEST_CASE_TEMPLATE (                      \
                            BOOST_PP_CAT(odeint_foreach, n), T, fp_types )       \
{                                                                                \
  vector_space_algebra algebra;                                                  \
  BOOST_PP_REPEAT(BOOST_PP_ADD(n,2),TABLE,tt)                                    \
  BOOST_PP_CAT(algebra.for_each,BOOST_PP_ADD(n,2))(                              \
               BOOST_PP_ENUM_PARAMS(BOOST_PP_ADD(n,2),y), default_operations::   \
  BOOST_PP_CAT(scale_sum,BOOST_PP_ADD(n,1)) <T>(                                 \
               BOOST_PP_REPEAT(n, PARAMS, text ) T(BOOST_PP_ADD(n,1))));         \
  BOOST_PP_REPEAT(2,TEST,n)                                                      \
}

BOOST_AUTO_TEST_SUITE( nt2_algebra )

BOOST_PP_REPEAT(7,TEST_CASE,dummy)

BOOST_AUTO_TEST_SUITE_END()


#include <opencv2/highgui/highgui.hpp>

#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>
#include <vector>
#ifndef WIN64
    #define EIGEN_DONT_ALIGN_STATICALLY
#endif
#include <Eigen/Dense>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

bool loadIntrinsics(const std::string &intrinsicsFile, Eigen::Matrix3f &K)
{
    if (intrinsicsFile.empty())
        return false;

    std::ifstream intrIn(intrinsicsFile.c_str());
    if (!intrIn.is_open())
        return false;

    //camera intrinsics
    K = Eigen::Matrix3f::Identity();
    float fVal = 0;
    for (size_t i = 0; i < 3; ++i) {
        for (size_t j = 0; j < 3; ++j) {
            intrIn >> fVal;
            K(i, j) = fVal;
        }
    }
    intrIn.close();

    return true;
}

bool loadFrame(const std::string &folder, size_t index, cv::Mat &color, cv::Mat &depth, cv::Mat &mask)
{
    // build postfix
    std::stringstream ss;
    ss << std::setfill('0') << std::setw(6) << index;
    std::string id = ss.str() + ".png";

    // load color
    std::string colorFile = folder + "/color_" + id;
    color = cv::imread(colorFile);
    if (color.empty())
        return false;

    // load depth
    std::string depthFile = folder + "/depth_" + id;
    //fill/read 16 bit depth image
    cv::Mat depthIn = cv::imread(depthFile, CV_LOAD_IMAGE_ANYDEPTH | CV_LOAD_IMAGE_ANYCOLOR);
    if (depthIn.empty())
        return false;
    depthIn.convertTo(depth, CV_32FC1, (1.0 / 1000.0));

    // load mask
    std::string maskFile = folder + "/omask_" + id;
    mask = cv::imread(maskFile);

    return true;
}

void filterDepth(const cv::Mat &mask, cv::Mat &depth)
{
    for (int y = 0; y < depth.rows; ++y)
    {
        for (int x = 0; x < depth.cols; ++x)
        {
            cv::Vec3b maskVal = mask.at<cv::Vec3b>(y, x);
            float val = depth.at<float>(y, x);
            if (!maskVal[0] > 0 || val == 0.0f || std::isnan(val))
            {
                //depth.at<float>(y,x) = std::numeric_limits<float>::quiet_NaN();
                depth.at<float>(y, x) = 0.0f;
            }
        }
    }
}


/**
 * @file test-jump.cpp
 *
 * @brief test jump function.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <iostream>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <string>
#include <inttypes.h>
#include <stdint.h>
#include <time.h>
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/ZZ.h>
#include "SFMT-calc-jump.hpp"
#include "SFMT-jump.h"

using namespace NTL;
using namespace std;
using namespace sfmt;

static void read_file(GF2X& characteristic, int line_no, const string& file);
static void test(sfmt_t * sfmt, GF2X& poly);
static int check(sfmt_t *a, sfmt_t *b);
static void print_state(sfmt_t *a, sfmt_t * b);
static void print_sequence(sfmt_t *a, sfmt_t * b);
static void speed(sfmt_t * sfmt, GF2X& characteristic);

int main(int argc, char * argv[]) {
    if (argc <= 1) {
	printf("%s -s|-c\n", argv[0]);
	return -1;
    }
    stringstream ss_file;
    GF2X characteristic;
    ss_file << "characteristic." << SFMT_MEXP << ".txt";
    read_file(characteristic, 0, ss_file.str());
#if 0
    string step_string = argv[1];
    ZZ step;
    stringstream ss(step_string);
    ss >> step;
    string jump_str;
    calc_jump(jump_str, step, characteristic);

    string tmp;
    polytostring(tmp, characteristic);
    cout << "tmp :" << endl;
    cout << tmp << endl;
    cout << jump_str << endl;
#endif
    sfmt_t sfmt;
    if (argv[1][1] == 's') {
	speed(&sfmt, characteristic);
    } else {
	test(&sfmt, characteristic);
    }
    return 0;
}

static void speed(sfmt_t * sfmt, GF2X& characteristic)
{
    uint32_t seed = 1234;
    long step = 10000;
    int exp = 4;
    ZZ test_count;
    string jump_string;
    clock_t start;
    double elapsed1;
    double elapsed2;

    sfmt_init_gen_rand(sfmt, seed);
    test_count = step;
    for (int i = 0; i < 10; i++) {
	start = clock();
	calc_jump(jump_string, test_count, characteristic);
	elapsed1 = clock() - start;
	elapsed1 = elapsed1 * 1000 / CLOCKS_PER_SEC;
	cout << "mexp "
	     << setw(5)
	     << SFMT_MEXP
	     << " jump 10^"
	     << setfill('0') << setw(2)
	     << exp
	     << " steps calc_jump:"
	     << setfill(' ') << setiosflags(ios::fixed)
	     << setw(6) << setprecision(3)
	     << elapsed1
	     << "ms"
	     << endl;
	start = clock();

	for (int j = 0; j < 10; j++) {
	    SFMT_jump(sfmt, jump_string.c_str());
	}
	elapsed2 = clock() - start;
	elapsed2 = elapsed2 * 1000 / 10 / CLOCKS_PER_SEC;
	cout << "mexp "
	     << setw(5)
	     << SFMT_MEXP
	     << " jump 10^"
	     << setfill('0') << setw(2)
	     << exp
	     << " steps SFMT_jump:"
	     << setfill(' ') << setiosflags(ios::fixed)
	     << setw(6) << setprecision(3)
	     << elapsed2
	     << "ms"
	     << endl;
	test_count *= 100;
	exp += 2;
#if 0
	if (elapsed1 > 200.0) {
	    break;
	}
#endif
    }
}

static void read_file(GF2X& characteristic, int line_no, const string& file)
{
    ifstream ifs(file.c_str());
    string line;
    for (int i = 0; i < line_no; i++) {
	ifs >> line;
	ifs >> line;
    }
    if (ifs) {
	ifs >> line;
	line = "";
	ifs >> line;
    }
    stringtopoly(characteristic, line);
#if defined(DEBUG)
    cout << "line = " << line << endl;
    cout << "deg = " << deg(characteristic) << endl;
    cout << "cha = " << characteristic << endl;
    string x;
    polytostring(x, characteristic);
    cout << "x = " << x << endl;
#endif
}

static int check(sfmt_t *a, sfmt_t *b)
{
    int check = 0;
    for (int i = 0; i < 100; i++) {
	uint32_t x = sfmt_genrand_uint32(a);
	uint32_t y = sfmt_genrand_uint32(b);
	if (x != y) {
	    print_state(a, b);
	    print_sequence(a, b);
	    check = 1;
	    break;
	}
    }
    if (check == 0) {
	printf("OK!\n");
    } else {
	printf("NG!\n");
    }
    return check;
}

static void print_state(sfmt_t *a, sfmt_t * b)
{
    printf("idx = %d                             idx = %d\n",
	   a->idx, b->idx);
    for (int i = 0; (i < 10) && (i < SFMT_N); i++) {
	printf("[");
	for (int j = 0; j < 4; j++) {
	    printf("%08"PRIx32, a->state[i].u[j]);
	    if (j < 3) {
		printf(" ");
	    } else {
		printf("]");
	    }
	}
	printf("[");
	for (int j = 0; j < 4; j++) {
	    printf("%08"PRIx32, b->state[i].u[j]);
	    if (j < 3) {
		printf(" ");
	    } else {
		printf("]");
	    }
	}
	printf("\n");
    }
}

static void print_sequence(sfmt_t *a, sfmt_t * b)
{
    for (int i = 0; i < 25; i++) {
	uint32_t c, d;
	c = sfmt_genrand_uint32(a);
	d = sfmt_genrand_uint32(b);
	printf("[%08"PRIx32" %08"PRIx32"]\n", c, d);
    }
}

static void test(sfmt_t * sfmt, GF2X& characteristic)
{
    sfmt_t new_sfmt_z;
    sfmt_t * new_sfmt = &new_sfmt_z;
//    uint32_t seed[] = {1, 998102, 1234, 0, 5};
    uint32_t seed[20] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
    long steps[] = {1, 2, SFMT_N + 1,
		    SFMT_N * 128 - 1,
		    SFMT_N * 128 + 1,
		    3003,
		    200004,
		    10000005};
    int steps_size = sizeof(steps) / sizeof(long);
    ZZ test_count;
    string jump_string;

    sfmt_init_gen_rand(sfmt, seed[0]);
    sfmt_genrand_uint32(sfmt);
    sfmt_genrand_uint32(sfmt);
    sfmt_genrand_uint32(sfmt);
    sfmt_genrand_uint32(sfmt);
    /* plus jump */
    for (int index = 0; index < steps_size; index++) {
//	sfmt_init_gen_rand(sfmt, seed[index]);
	test_count = steps[index];
	cout << "mexp " << dec << SFMT_MEXP << " jump "
	     << test_count << " steps" << endl;
	*new_sfmt = *sfmt;
	for (long i = 0; i < steps[index] * 4; i++) {
	    sfmt_genrand_uint32(sfmt);
	}
	calc_jump(jump_string, test_count, characteristic);
#if defined(DEBUG)
	cout << "jump string:" << jump_string << endl;
	cout << "before jump:" << endl;
	print_state(new_sfmt, sfmt);
#endif
	SFMT_jump(new_sfmt, jump_string.c_str());
#if defined(DEBUG)
	cout << "after jump:" << endl;
	print_state(new_sfmt, sfmt);
#endif
	if (check(new_sfmt, sfmt)) {
	    return;
	}
    }
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/is_finite.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/quarter.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/logical.hpp>

STF_CASE_TPL (" is_finite real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_finite;
  using r_t = decltype(is_finite(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(is_finite(bs::Inf<T>()), r_t(false));
  STF_EQUAL(is_finite(bs::Minf<T>()), r_t(false));
  STF_EQUAL(is_finite(bs::Nan<T>()), r_t(false));
#endif
  STF_EQUAL(is_finite(-bs::Zero<T>()), r_t(true));
  STF_EQUAL(is_finite(bs::Half<T>()), r_t(true));
  STF_EQUAL(is_finite(bs::Mone<T>()), r_t(true));
  STF_EQUAL(is_finite(bs::One<T>()), r_t(true));
  STF_EQUAL(is_finite(bs::Quarter<T>()), r_t(true));
  STF_EQUAL(is_finite(bs::Two<T>()), r_t(true));
  STF_EQUAL(is_finite(bs::Zero<T>()), r_t(true));
}

STF_CASE_TPL (" is_finite signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_finite;
  using r_t = decltype(is_finite(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_finite(bs::Mone<T>()), r_t(true));
  STF_EQUAL(is_finite(bs::One<T>()), r_t(true));
  STF_EQUAL(is_finite(bs::Two<T>()), r_t(true));
  STF_EQUAL(is_finite(bs::Zero<T>()), r_t(true));
}

STF_CASE_TPL (" is_finite _unsigned",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_finite;
  using r_t = decltype(is_finite(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, bs::logical<T>);

  // specific values tests
  STF_EQUAL(is_finite(bs::One<T>()), r_t(true));
  STF_EQUAL(is_finite(bs::Two<T>()), r_t(true));
  STF_EQUAL(is_finite(bs::Zero<T>()), r_t(true));
}

STF_CASE ( "is_finite bool")
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_finite;
  using r_t = decltype(is_finite(bool()));

  // return type conformity test
  STF_TYPE_IS(r_t, bool);

  // specific values tests
  STF_EQUAL(is_finite(true), true);
  STF_EQUAL(is_finite(false), true);
}