//
//  parameters.hpp
//  yamcmc++
//
//  Created by Brandon Kelly on 4/15/13.
//  Copyright (c) 2013 Brandon Kelly. All rights reserved.
//

#ifndef yamcmc___parameters_hpp
#define yamcmc___parameters_hpp

// Standard includes
#include <iostream>
#include <string>
// Boost includes
#include <boost/ptr_container/ptr_vector.hpp>
// Local includes
#include "random.hpp"

// Global random number generator object, instantiated in random.cpp
extern boost::random::mt19937 rng;

// Global object for generating random variables from various distributions,
// instantiated in steps.cpp
extern RandomGenerator RandGen;

// This is the base Parameter class. It is abstract, so it should
// never be instantiated directly. Users should subclass the Parameter class,
// not BaseParameter class as the parameter value type is not specified in the
// BaseParameter class. Because of this, the MCMC step classes take a reference
// to the Parameter class.
class BaseParameter {
public:
    // Constructor
    BaseParameter() {}
    BaseParameter(bool track, std::string label, double temperature=1.0) :
    track_(track), label_(label), temperature_(temperature) {}

	// Return the current value of the log-posterior. Useful for
	// Metropolis steps so we don't have to compute the log-posterior
	// for the current value of the parameter more than once
	double GetLogDensity() {
		return log_posterior_;
	}

    // Method to directly set the log-posterior of the parameter. Useful for certain steps
    // when we do not need to recalculate the posterior. Used in the exchange step.
    void SetLogDensity(double logpost) {
        log_posterior_ = logpost;
    }

    // Return the value of the temperature used. Primarily used in tempered transitions.
    double GetTemperature() {
        return temperature_;
    }

	// Return a string representation of the parameter value.
	virtual std::string StringValue() {
		return " ";
	}

	// Parameter is tracked / saved. Return True if parameter is tracked.
	bool Track() {
		return track_;
	}

    // Set whether a parameter is tracked or not.
    void SetTracking(bool track) {
        track_ = track;
    }

	// Return the a string containing the parameter label. This is used to identify the parameter.
	std::string Label() {
		return label_;
	}

    // Set the size of the vector containing the MCMC samples. This will be overidden by the Parameter class.
    virtual void SetSampleSize(int sample_size) = 0;

    // Add a value to the set of MCMC samples. This will be overidden by the Parameter class.
    virtual void AddToSample(int current_iter) = 0;

protected:
	/// Should this variable be tracked?
	bool track_;
	/// Name of variable for tracking purposes.
	std::string label_;
    // Temperature value, used when doing tempered steps. By default this is one.
    double temperature_;
    double log_posterior_; // The log of the posterior distribution
};

// Templated abstract parameter class. Users should subclass the Parameter class because the
// Step classes need to know the type of the parameter values.
template<class ParValueType>
class Parameter : public BaseParameter {
public:
    // Constructor
    Parameter() {}
    Parameter(bool track, std::string label, double temperature=1.0) :
    BaseParameter(track, label, temperature) {}

	// Method to return the starting value for the parameter.
	virtual ParValueType StartingValue() = 0;

   // Method to set the starting value of the parameter
   virtual ParValueType SetStartingValue(ParValueType init) = 0;

	// Method to return the log of the probability density (plus constant).
	// value: Value of parameter to evaluate density at.
	virtual double LogDensity(ParValueType value) {
		return 0.0;
    }

	// Return a random draw from the posterior.
	// Random draw from posterior is called by GibbsStep.
	virtual ParValueType RandomPosterior() {
        ParValueType p;
		return p;
	}

	// Return the current value of the parameter.
	ParValueType Value() {
        return value_;
    }

	// Save a new value of the parameter.
	// new_value: New value to save.
	virtual void Save(ParValueType new_value) {
        value_ = new_value;
        log_posterior_ = LogDensity(new_value);
    }

    // Set the size of the vector containing the MCMC samples
    void SetSampleSize(int sample_size) {
        samples_.resize(sample_size);
        logposts_.resize(sample_size);
    }

    // Add a value to the set of MCMC samples
    void AddToSample(int current_iter) {
        // TODO: Should be able to replace this with an iterator for efficiency, since we probably
        // will always add values sequentially for MCMC samplers
        samples_[current_iter] = value_;
        logposts_[current_iter] = log_posterior_;
    }

    // Add a value and its log-posterior to the MCMC samples
    void AddToSample(int current_iter, ParValueType value, double logpost) {
        samples_[current_iter] = value;
        logposts_[current_iter] = logpost;
    }

    // Return a copy of the MCMC samples
    std::vector<ParValueType> GetSamples() {
        return samples_;
    }

    // Return a copy of the MCMC samples
    std::vector<double> GetLogLikes() {
        return logposts_;
    }

protected:
    ParValueType value_; // The current value of the parameter
    std::vector<ParValueType> samples_; // Vector containing the MCMC samples
    std::vector<double> logposts_; // Vector containing the posterior likelihoods
};

// This is the Ensemble class. It is basically a class
// containing a pointer container (boost::ptr_vector) that allows
// one to collect an ensemble of objects. This is the basis for
// the Ensemble MCMC samplers, which require both an ensemble of
// parameter and proposal objects.

template <class EnsembleType>
class Ensemble {
public:
    // Empty constructor
    Ensemble() {}

    // Add parameter object to the parameter ensemble
    void AddObject(EnsembleType* pObject) {
        the_objects_.push_back(pObject);
    }

    // Return the number of parameters in the ensemble
    int size() {
        return the_objects_.size();
    }

    // Access elements of the parameter ensemble. This done by overloading
    // the [] operator so that one can obtain a reference to a parameter
    // as parameter = ensemble[i].
    EnsembleType& operator [] (const int index)
    {
        return the_objects_[index];
    }

    EnsembleType const & operator [] (const int index) const
    {
        return the_objects_[index];
    }

    //private:
    boost::ptr_vector<EnsembleType> the_objects_;
};

#endif


#include <ctime>
#include <iostream>
#include <limits>
using namespace std;

#define BOOST_TEST_MAIN
// #define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE TestPlanManager
#include <boost/test/unit_test.hpp>

#include <boost/random.hpp>

#include <fftscarf.h>
using namespace fftscarf;

BOOST_AUTO_TEST_CASE( test_fftplanmanager )
{
    int p_min = 5;
    int p_max = 13; // TODO Going above 13 crashes with FFTS on Travis (!?!?!)

    #ifdef FFTSCARF_PRECISION_SINGLE
        FFTPlanManager<FFTPlanSingle> pm_single;
        pm_single.write_status(std::cout);
        pm_single.prepare_osf(5, 11);
        pm_single.prepare(8192);
        pm_single.prepare(4096);
        pm_single.write_status(std::cout);
    #endif
    #ifdef FFTSCARF_PRECISION_DOUBLE
        FFTPlanManager<FFTPlanDouble> pm_double;
        pm_double.write_status(std::cout);
        pm_double.prepare_osf(p_min, p_max);
        pm_double.write_status(std::cout);
    #endif
    #ifdef FFTSCARF_PRECISION_LONGDOUBLE
        FFTPlanManager<FFTPlanLongDouble> pm_long_double;
        pm_long_double.write_status(std::cout);
        pm_long_double.prepare_osf(p_min, p_max);
        pm_long_double.write_status(std::cout);
    #endif

    FFTPlanManager<FFTPlan> pm_default_fwd(true);
    FFTPlanManager<FFTPlan> pm_default_bck(false);
    pm_default_fwd.write_status(std::cout);
    pm_default_bck.write_status(std::cout);

    boost::mt19937 rnd_engine((uint32_t)std::time(0));
    boost::normal_distribution<typename FFTPlan::FloatType> rnd_normal_distrib;
    boost::variate_generator<boost::mt19937&,
        boost::normal_distribution<typename FFTPlan::FloatType> > generator(rnd_engine, rnd_normal_distrib);

    std::cout << "testing: " << std::flush;

    std::vector<typename FFTPlan::FloatType> inframe;
    for(int p=p_min; p<=p_max; ++p){
        int dftlen = std::pow(2, p);
        std::cout << dftlen << " " << std::flush;

        // Fill an input frame
        inframe.resize(dftlen);
        for(int n=0; n<dftlen; ++n)
            inframe[n] = generator();

        // Run the tested implementation
        std::vector<std::complex<typename FFTPlan::FloatType> > spec;
        pm_default_fwd.rfft(inframe, spec, dftlen);
        pm_default_bck.irfft(spec, inframe);

        // Test the global Plan Manager
        fftscarf::rfft(inframe, spec, dftlen);
        fftscarf::irfft(spec, inframe);
    }
    std::cout << std::endl;
    pm_default_fwd.write_status(std::cout);
    pm_default_bck.write_status(std::cout);

//    BOOST_CHECK(!fftscarf::isPow2(p)); // The reminder should be non-power of 2^a
}


#include "pybind11/pybind11.h"

#include "xtensor/xmath.hpp"
#include "xtensor/xarray.hpp"
#include <boost/pending/disjoint_sets.hpp>

#define FORCE_IMPORT_ARRAY
#include "xtensor-python/pytensor.hpp"

#include <iostream>
#include <numeric>
#include <cmath>

namespace py = pybind11;

xt::pytensor<uint64_t, 2> watershed(const xt::pytensor<float, 2> & weights,
                                    xt::pytensor<uint64_t, 2> & seeds)
{
    typedef typename xt::pytensor<float, 2>::shape_type IndexType;
    const auto & shape = weights.shape();
    const std::size_t n_nodes = shape[0] * shape[1];

    // make union find and map seeds to reperesentatives
    std::vector<uint64_t> ranks(n_nodes);
    std::vector<uint64_t> parents(n_nodes);
    boost::disjoint_sets<uint64_t*, uint64_t*> ufd(&ranks[0], &parents[0]);
    for(uint64_t node = 0; node < n_nodes; ++node) {
        ufd.make_set(node);
    }

    // argsort the edges by edge weight
    auto flat_weights = xt::flatten(weights);
    auto flat_seeds = xt::flatten(seeds);

    std::vector<std::size_t> argsorted(n_nodes);
    std::iota(argsorted.begin(), argsorted.end(), 0);
    std::sort(argsorted.begin(), argsorted.end(), [&](const std::size_t a,
                                                      const std::size_t b){
        return flat_weights[a] < flat_weights[b];}
    );

    const int n_ngbs = 4;
    std::vector<int> shifts_x = {-1, 1, 0, 0};
    std::vector<int> shifts_y = {0, 0, -1, 1};

    // run kruskal
    for(const uint64_t u : argsorted) {
        // get representative
        const uint64_t ru = ufd.find_set(u);

        // get seed
        uint64_t seed_u = flat_seeds[ru];
        if(seed_u == 0) {
            seed_u = seeds[u];
            flat_seeds[ru] = seed_u;
        } else {
            const uint64_t seed_uu = seeds[u];
            if(seed_uu != 0 && seed_uu != seed_u) {
                std::cout << u << ", " << ru << " : " << seed_uu << ", " << seed_u << std::endl;
                throw std::runtime_error("Seeds disagree!");
            }
        }

        const auto coordinate = xt::unravel_index(u, shape);
        std::vector<IndexType> neighbor_coords;

        // make the neighbors
        for(unsigned ngb = 0; ngb < n_ngbs; ++ngb) {
            const int sx = shifts_x[ngb];
            const int sy = shifts_y[ngb];
            const int64_t x = coordinate[0] + sx;
            const int64_t y = coordinate[1] + sy;

            // bounds check
            if(sx < 0 || sx >= shape[0]) {
                continue;
            }
            if(sy < 0 || sy >= shape[1]) {
                continue;
            }

            neighbor_coords.emplace_back(IndexType({x, y}));
        }

        const auto neighbors = xt::ravel_indices(neighbor_coords, shape);
        // iterate over the neighbors
        for(const uint64_t v: neighbors) {
            const uint64_t rv = ufd.find_set(v);
            if(ru == rv) {
                continue;
            }

            const uint64_t seed_v = flat_seeds[v];
            if(seed_u != 0 && seed_v != 0 && (seed_v != seed_u)) {
                continue;
            }

            ufd.link(ru, rv);
            if(seed_u != 0) {
                flat_seeds[ru] = seed_u;
                flat_seeds[rv] = seed_u;
            }
            else if(seed_v != 0) {
                flat_seeds[ru] = seed_v;
                flat_seeds[rv] = seed_v;
            }
        }
    }

    xt::pytensor<uint64_t, 2> seg = xt::zeros<uint64_t>(shape);
    return seg;
}

PYBIND11_MODULE(wsxt, m)
{
    xt::import_numpy();

    m.doc() = R"pbdoc(
        wsxt

        .. currentmodule:: wsxt

        .. autosummary::
           :toctree: _generate

           watershed
    )pbdoc";

    m.def("watershed", watershed, "compute watershed");
}


//==================================================================================================
/*
  Copyright 2017 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)
*/
//==================================================================================================
//! [all_of]
#include <boost/simd/algorithm/all_of.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <iostream>
#include <vector>

int main()
{
  namespace bs =  boost::simd;
  std::vector<float> d{1.f, 2.f, 3.f, 4.f, 5.f, 6.f, 7.f};

  auto rt = bs::all_of( d.data(), d.data()+7, bs::is_gtz );
  std::cout <<  "rt " << rt << "\n";
  d[4] = 0.0f;
  auto rf = bs::all_of( d.data(), d.data()+7, bs::is_gtz);
  std::cout << "rf " << rf << "\n";

  return 0;
}
//! [all_of]


#define BOOST_TEST_MODULE TEST_RNN

#include <dynet/dynet.h>
#include <dynet/expr.h>
#include <dynet/cfsm-builder.h>
#include <dynet/hsm-builder.h>
#include <dynet/dict.h>
#include <dynet/grad-check.h>
#include <boost/test/unit_test.hpp>
#include "test.h"
#include <stdexcept>
#include <fstream>
#include <sstream>

using namespace dynet;
using namespace std;

struct SoftmaxTest {
  SoftmaxTest() {
    // initialize if necessary
    if (default_device == nullptr) {
      for (auto x : {"SoftmaxTest", "--dynet-seed", "10", "--dynet-mem", "10"}) {
        av.push_back(strdup(x));
      }
      char **argv = &av[0];
      int argc = av.size();
      dynet::initialize(argc, argv);
    }
    vec_values = {1.f, 2.f, 3.f};
    batch_values = {1.f, 2.f, 3.f, 4.f, 5.f, 6.f};
    ofstream ofs("cluster_file.txt");
    for (unsigned i = 0; i < 5; ++i) {
      for (unsigned j = 0; j < 2; ++j) {
        stringstream ss;
        ss << (i * 2 + j);
        dic.convert(ss.str());
        ofs << i << " " << ss.str() << endl;
      }
    }
    ofs.close();
  }
  ~SoftmaxTest() {
    // This was causing double deallocation errors?
    // for (auto x : av) free(x);
  }

  template <class T>
  std::string print_vec(const std::vector<T> vec) {
    ostringstream oss;
    if (vec.size()) oss << vec[0];
    for (size_t i = 1; i < vec.size(); i++)
      oss << ' ' << vec[i];
    return oss.str();
  }

  std::vector<float> vec_values, batch_values;
  std::vector<char*> av;
  Dict dic;
};

// define the test suite
BOOST_FIXTURE_TEST_SUITE(softmax_test, SoftmaxTest);

BOOST_AUTO_TEST_CASE( standard_softmax_grad ) {
  dynet::ParameterCollection mod;
  StandardSoftmaxBuilder softmax(3, 10, mod, true);
  dynet::ComputationGraph cg;
  softmax.new_graph(cg);
  auto h = input(cg, {3}, vec_values);
  auto nll = softmax.neg_log_softmax(h, 5);
  BOOST_CHECK(check_grad(mod, nll, 0));
}

BOOST_AUTO_TEST_CASE( standard_softmax_batch_grad ) {
  dynet::ParameterCollection mod;
  StandardSoftmaxBuilder softmax(3, 10, mod, true);
  dynet::ComputationGraph cg;
  softmax.new_graph(cg);
  Expression h = input(cg, Dim({3}, 2), batch_values);
  vector<unsigned> batch_elems = {2, 5};
  Expression nll = sum_batches(softmax.neg_log_softmax(h, batch_elems));
  BOOST_CHECK(check_grad(mod, nll, 0));
}

BOOST_AUTO_TEST_CASE( cf_softmax_grad ) {
  dynet::ParameterCollection mod;
  ClassFactoredSoftmaxBuilder softmax(3, "cluster_file.txt", dic, mod, true);
  dynet::ComputationGraph cg;
  softmax.new_graph(cg);
  auto h = input(cg, {3}, vec_values);
  auto nll = softmax.neg_log_softmax(h, 5);
  BOOST_CHECK(check_grad(mod, nll, 0));
}

BOOST_AUTO_TEST_CASE( cf_softmax_batch_grad ) {
  dynet::ParameterCollection mod;
  ClassFactoredSoftmaxBuilder softmax(3, "cluster_file.txt", dic, mod, true);
  dynet::ComputationGraph cg;
  softmax.new_graph(cg);
  Expression h = input(cg, Dim({3}, 2), batch_values);
  vector<unsigned> batch_elems = {2, 5};
  Expression nll = sum_batches(softmax.neg_log_softmax(h, batch_elems));
  BOOST_CHECK(check_grad(mod, nll, 0));
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2014-2021, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_NUM_SEGMENTS_HPP
#define BOOST_GEOMETRY_ALGORITHMS_NUM_SEGMENTS_HPP

#include <cstddef>

#include <boost/range/size.hpp>
#include <boost/range/value_type.hpp>

#include <boost/geometry/algorithms/detail/counting.hpp>
#include <boost/geometry/algorithms/detail/visit.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>

#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/visit.hpp>

#include <boost/geometry/geometries/adapted/boost_variant.hpp> // For backward compatibility
#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/util/range.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace num_segments
{

struct range_count
{
    template <typename Range>
    static inline std::size_t apply(Range const& range)
    {
        std::size_t n = boost::size(range);
        if ( n <= 1 )
        {
            return 0;
        }

        return
            geometry::closure<Range>::value == open
            ?
            n
            :
            static_cast<std::size_t>(n - 1);
    }
};

}} // namespace detail::num_segments
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Geometry, typename Tag = typename tag<Geometry>::type>
struct num_segments
    : not_implemented<Tag>
{};

template <typename Geometry>
struct num_segments<Geometry, point_tag>
    : detail::counting::other_count<0>
{};

// the number of segments (1-dimensional faces) of the hypercube is
// given by the formula: d * 2^(d-1), where d is the dimension of the
// hypercube; see also:
//            http://en.wikipedia.org/wiki/Hypercube
template <typename Geometry>
struct num_segments<Geometry, box_tag>
    : detail::counting::other_count
        <
            geometry::dimension<Geometry>::value
            * (1 << (geometry::dimension<Geometry>::value - 1))
        >
{};

template <typename Geometry>
struct num_segments<Geometry, segment_tag>
    : detail::counting::other_count<1>
{};

template <typename Geometry>
struct num_segments<Geometry, linestring_tag>
    : detail::num_segments::range_count
{};

template <typename Geometry>
struct num_segments<Geometry, ring_tag>
    : detail::num_segments::range_count
{};

template <typename Geometry>
struct num_segments<Geometry, polygon_tag>
    : detail::counting::polygon_count<detail::num_segments::range_count>
{};

template <typename Geometry>
struct num_segments<Geometry, multi_point_tag>
    : detail::counting::other_count<0>
{};

template <typename Geometry>
struct num_segments<Geometry, multi_linestring_tag>
    : detail::counting::multi_count
        <
            num_segments< typename boost::range_value<Geometry>::type>
        >
{};

template <typename Geometry>
struct num_segments<Geometry, multi_polygon_tag>
    : detail::counting::multi_count
        <
            num_segments< typename boost::range_value<Geometry>::type>
        >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_dynamic
{

template <typename Geometry, typename Tag = typename tag<Geometry>::type>
struct num_segments
{
    static inline std::size_t apply(Geometry const& geometry)
    {
        concepts::check<Geometry const>();

        return dispatch::num_segments<Geometry>::apply(geometry);
    }
};

template <typename Geometry>
struct num_segments<Geometry, dynamic_geometry_tag>
{
    static inline std::size_t apply(Geometry const& geometry)
    {
        std::size_t result = 0;
        traits::visit<Geometry>::apply([&](auto const& g)
        {
            result = num_segments<util::remove_cref_t<decltype(g)>>::apply(g);
        }, geometry);
        return result;
    }
};

template <typename Geometry>
struct num_segments<Geometry, geometry_collection_tag>
{
    static inline std::size_t apply(Geometry const& geometry)
    {
        std::size_t result = 0;
        detail::visit_breadth_first([&](auto const& g)
        {
            result += num_segments<util::remove_cref_t<decltype(g)>>::apply(g);
            return true;
        }, geometry);
        return result;
    }
};

} // namespace resolve_dynamic

/*!
\brief \brief_calc{number of segments}
\ingroup num_segments
\details \details_calc{num_segments, number of segments}.
\tparam Geometry \tparam_geometry
\param geometry \param_geometry
\return \return_calc{number of segments}

\qbk{[include reference/algorithms/num_segments.qbk]}
*/
template <typename Geometry>
inline std::size_t num_segments(Geometry const& geometry)
{
    return resolve_dynamic::num_segments<Geometry>::apply(geometry);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_NUM_SEGMENTS_HPP


/**
 * @file test-jump64.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 <errno.h>
#include "mtgp64-calc-poly.hpp"
#include "mtgp-calc-jump.hpp"
#include "mtgp64-fast-jump.h"
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/ZZ.h>

using namespace NTL;
using namespace std;

static void test(mtgp64_fast_t * mtgp, GF2X& poly);
static int check(mtgp64_fast_t *a, mtgp64_fast_t *b);
static void print_state(mtgp64_fast_t * a, mtgp64_fast_t * b);
static void print_sequence(mtgp64_fast_t * a, mtgp64_fast_t * b);
static void speed(mtgp64_fast_t * mtgp, GF2X& characteristic);

/**
 * test main
 * @param[in] argc number of arguments.
 * @param[in] argv an array of arguments.
 * @return 0 if normal, others abnormal.
 */
int main(int argc, char * argv[]) {
    if (argc <= 3) {
	printf("%s -s|-c mexp no.\n", argv[0]);
	return -1;
    }
    errno = 0;
    GF2X characteristic;
    mtgp64_params_fast_t *params;
    mtgp64_fast_t mtgp;
    uint32_t seed = 0;
    //    int rc;
    int mexp = strtol(argv[2], NULL, 10);
    int no = strtol(argv[3], NULL, 10);
    if (no < 0 || no > 127) {
	cout << "error in no" << endl;
	return 1;
    }
    switch (mexp) {
    case 23209:
        params = mtgp64_params_fast_23209;
        break;
    case 44497:
        params = mtgp64_params_fast_44497;
        break;
    case 110503:
        params = mtgp64_params_fast_110503;
        break;
    default:
        printf("%s: mexp no.\n", argv[0]);
        printf("mexp shuould be 23209, 44497 or 110503\n");
        return 2;
    }
    params += no;
    mtgp64_init(&mtgp, params, seed);
    mtgp64_calc_characteristic(characteristic, &mtgp);
    if (argv[1][1] == 's') {
	speed(&mtgp, characteristic);
    } else {
	test(&mtgp, characteristic);
    }
    mtgp64_free(&mtgp);
    return 0;
}

/**
 * check speed
 * @param[in] mtgp generator
 * @param[in] characteristic characteristic polynomial
 */
static void speed(mtgp64_fast_t * mtgp, GF2X& characteristic)
{
    long step = 10000;
    int exp = 4;
    ZZ test_count;
    string jump_string;
    clock_t start;
    double elapsed1;
    double elapsed2;

    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)
	     << mtgp->params.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++) {
	    mtgp64_fast_jump(mtgp, jump_string.c_str());
	}
	elapsed2 = clock() - start;
	elapsed2 = elapsed2 * 1000 / 10 / CLOCKS_PER_SEC;
	cout << "mexp "
	     << setw(5)
	     << mtgp->params.mexp
	     << " jump 10^"
	     << setfill('0') << setw(2)
	     << exp
	     << " steps MTGP_jump:"
	     << setfill(' ') << setiosflags(ios::fixed)
	     << setw(6) << setprecision(3)
	     << elapsed2
	     << "ms"
	     << endl;
	test_count *= 100;
	exp += 2;
    }
}

/**
 * equality check
 * @param[in] a mtgp generator
 * @param[in] b mtgp generator
 * @return 0 if equal
 */
static int check(mtgp64_fast_t *a, mtgp64_fast_t *b)
{
    int check = 0;
    for (int i = 0; i < 100; i++) {
	uint64_t x = mtgp64_genrand_uint64(a);
	uint64_t y = mtgp64_genrand_uint64(b);
	if (x != y) {
	    print_state(a, b);
	    print_sequence(a, b);
	    check = 1;
	    break;
	}
    }
    if (check == 0) {
      cout << "OK!" << endl;
    } else {
      cout << "NG!" << endl;
    }
    return check;
}

/**
 * print internal state of two mtgp generators for checking by human eyes.
 * @param[in] a mtgp generator
 * @param[in] b mtgp generator
 */
static void print_state(mtgp64_fast_t *a, mtgp64_fast_t * b)
{
  int large_size = a->status->large_size;
  cout << "idx = " << dec << a->status->idx
       << "   " << dec << b->status->idx
       << endl;
    for (int i = 0; (i < 10) && (i < large_size); i++) {
      cout << setfill('0') << setw(16) << hex
	   << a->status->array[(i + a->status->idx) % large_size];
      cout << " ";
      cout << setfill('0') << setw(16) << hex
	   << b->status->array[(i + b->status->idx) % large_size];
      cout << endl;
    }
}

/**
 * print output sequences of two mtgp generators for checking by human eyes.
 * @param[in] a mtgp generator
 * @param[in] b mtgp generator
 */
static void print_sequence(mtgp64_fast_t *a, mtgp64_fast_t * b)
{
    for (int i = 0; i < 25; i++) {
	uint64_t c, d;
	c = mtgp64_genrand_uint64(a);
	d = mtgp64_genrand_uint64(b);
	cout << setfill('0') << setw(16) << hex << c;
	cout << " " << setfill('0') << setw(16) << hex << d;
	cout << endl;
    }
}

/**
 * sanity check
 * @param[in] mtgp generator
 * @param[in] characteristic characteristic polynomial
 */
static void test(mtgp64_fast_t * mtgp, GF2X& characteristic)
{
    mtgp64_fast_t new_mtgp_z;
    mtgp64_fast_t * new_mtgp = &new_mtgp_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, mtgp->status->size + 1,
		    mtgp->status->size * 32 - 1,
		    mtgp->status->size * 32 + 1,
		    3003,
		    200004,
		    10000005};
    int steps_size = sizeof(steps) / sizeof(long);
    ZZ test_count;
    string jump_string;
    mtgp64_params_fast_t params = mtgp->params;
    mtgp64_init(new_mtgp, &params, 0);
    mtgp64_genrand_uint64(mtgp);
    mtgp64_genrand_uint64(mtgp);
    mtgp64_genrand_uint64(mtgp);
    /* plus jump */
    for (int index = 0; index < steps_size; index++) {
//	mtgp_init_gen_rand(mtgp, seed[index]);
	test_count = steps[index];
	cout << "mexp " << dec << mtgp->params.mexp << " jump "
	     << test_count << " steps" << endl;
//	*new_mtgp = *mtgp;
	mtgp64_copy(new_mtgp, mtgp);
	for (long i = 0; i < steps[index]; i++) {
	    mtgp64_genrand_uint64(mtgp);
	}
	calc_jump(jump_string, test_count, characteristic);
#if defined(DEBUG)
	cout << "jump string:" << jump_string << endl;
	cout << "before jump:" << endl;
	print_state(new_mtgp, mtgp);
#endif
	mtgp64_fast_jump(new_mtgp, jump_string.c_str());
#if defined(DEBUG)
	cout << "after jump:" << endl;
	print_state(new_mtgp, mtgp);
#endif
	if (check(new_mtgp, mtgp)) {
	    return;
	}
    }
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2016 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <sstream>

#ifdef EIGEN_TEST_MAX_SIZE
#undef EIGEN_TEST_MAX_SIZE
#endif

#define EIGEN_TEST_MAX_SIZE 50

#ifdef EIGEN_TEST_PART_1
#include "cholesky.cpp"
#endif

#ifdef EIGEN_TEST_PART_2
#include "lu.cpp"
#endif

#ifdef EIGEN_TEST_PART_3
#include "qr.cpp"
#endif

#ifdef EIGEN_TEST_PART_4
#include "qr_colpivoting.cpp"
#endif

#ifdef EIGEN_TEST_PART_5
#include "qr_fullpivoting.cpp"
#endif

#ifdef EIGEN_TEST_PART_6
#include "eigensolver_selfadjoint.cpp"
#endif

#ifdef EIGEN_TEST_PART_7
#include "eigensolver_generic.cpp"
#endif

#ifdef EIGEN_TEST_PART_8
#include "eigensolver_generalized_real.cpp"
#endif

#ifdef EIGEN_TEST_PART_9
#include "jacobisvd.cpp"
#endif

#ifdef EIGEN_TEST_PART_10
#include "bdcsvd.cpp"
#endif

#ifdef EIGEN_TEST_PART_11
#include "simplicial_cholesky.cpp"
#endif

#include <Eigen/Dense>

#undef min
#undef max
#undef isnan
#undef isinf
#undef isfinite

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/number.hpp>
#include <boost/math/special_functions.hpp>
#include <boost/math/complex.hpp>

namespace mp = boost::multiprecision;
typedef mp::number<mp::cpp_dec_float<100>, mp::et_on> Real;

namespace Eigen {
  template<> struct NumTraits<Real> : GenericNumTraits<Real> {
    static inline Real dummy_precision() { return 1e-50; }
  };

  template<typename T1,typename T2,typename T3,typename T4,typename T5>
  struct NumTraits<boost::multiprecision::detail::expression<T1,T2,T3,T4,T5> > : NumTraits<Real> {};

  template<>
  Real test_precision<Real>() { return 1e-50; }

  // needed in C++93 mode where number does not support explicit cast.
  namespace internal {
    template<typename NewType>
    struct cast_impl<Real,NewType> {
      static inline NewType run(const Real& x) {
        return x.template convert_to<NewType>();
      }
    };

    template<>
    struct cast_impl<Real,std::complex<Real> > {
      static inline std::complex<Real>  run(const Real& x) {
        return std::complex<Real>(x);
      }
    };
  }
}

namespace boost {
namespace multiprecision {
  // to make ADL works as expected:
  using boost::math::isfinite;
  using boost::math::isnan;
  using boost::math::isinf;
  using boost::math::copysign;
  using boost::math::hypot;

  // The following is needed for std::complex<Real>:
  Real fabs(const Real& a) { return abs EIGEN_NOT_A_MACRO (a); }
  Real fmax(const Real& a, const Real& b) { using std::max; return max(a,b); }

  // some specialization for the unit tests:
  inline bool test_isMuchSmallerThan(const Real& a, const Real& b) {
    return internal::isMuchSmallerThan(a, b, test_precision<Real>());
  }

  inline bool test_isApprox(const Real& a, const Real& b) {
    return internal::isApprox(a, b, test_precision<Real>());
  }

  inline bool test_isApproxOrLessThan(const Real& a, const Real& b) {
    return internal::isApproxOrLessThan(a, b, test_precision<Real>());
  }

  Real get_test_precision(const Real&) {
    return test_precision<Real>();
  }

  Real test_relative_error(const Real &a, const Real &b) {
    using Eigen::numext::abs2;
    return sqrt(abs2<Real>(a-b)/Eigen::numext::mini<Real>(abs2(a),abs2(b)));
  }
}
}

namespace Eigen {

}

void test_boostmultiprec()
{
  typedef Matrix<Real,Dynamic,Dynamic> Mat;
  typedef Matrix<std::complex<Real>,Dynamic,Dynamic> MatC;

  std::cout << "NumTraits<Real>::epsilon()         = " << NumTraits<Real>::epsilon() << std::endl;
  std::cout << "NumTraits<Real>::dummy_precision() = " << NumTraits<Real>::dummy_precision() << std::endl;
  std::cout << "NumTraits<Real>::lowest()          = " << NumTraits<Real>::lowest() << std::endl;
  std::cout << "NumTraits<Real>::highest()         = " << NumTraits<Real>::highest() << std::endl;
  std::cout << "NumTraits<Real>::digits10()        = " << NumTraits<Real>::digits10() << std::endl;

  // chekc stream output
  {
    Mat A(10,10);
    A.setRandom();
    std::stringstream ss;
    ss << A;
  }
  {
    MatC A(10,10);
    A.setRandom();
    std::stringstream ss;
    ss << A;
  }

  for(int i = 0; i < g_repeat; i++) {
    int s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE);

    CALL_SUBTEST_1( cholesky(Mat(s,s)) );

    CALL_SUBTEST_2( lu_non_invertible<Mat>() );
    CALL_SUBTEST_2( lu_invertible<Mat>() );
    CALL_SUBTEST_2( lu_non_invertible<MatC>() );
    CALL_SUBTEST_2( lu_invertible<MatC>() );

    CALL_SUBTEST_3( qr(Mat(internal::random<int>(1,EIGEN_TEST_MAX_SIZE),internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
    CALL_SUBTEST_3( qr_invertible<Mat>() );

    CALL_SUBTEST_4( qr<Mat>() );
    CALL_SUBTEST_4( cod<Mat>() );
    CALL_SUBTEST_4( qr_invertible<Mat>() );

    CALL_SUBTEST_5( qr<Mat>() );
    CALL_SUBTEST_5( qr_invertible<Mat>() );

    CALL_SUBTEST_6( selfadjointeigensolver(Mat(s,s)) );

    CALL_SUBTEST_7( eigensolver(Mat(s,s)) );

    CALL_SUBTEST_8( generalized_eigensolver_real(Mat(s,s)) );

    TEST_SET_BUT_UNUSED_VARIABLE(s)
  }

  CALL_SUBTEST_9(( jacobisvd(Mat(internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE), internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2))) ));
  CALL_SUBTEST_10(( bdcsvd(Mat(internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE), internal::random<int>(EIGEN_TEST_MAX_SIZE/4, EIGEN_TEST_MAX_SIZE/2))) ));

  CALL_SUBTEST_11(( test_simplicial_cholesky_T<Real,int>() ));
}


//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// 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 "Mesh_CleanMesh.h"

#include <assert.h>

#pragma warning(push, 0)
#include <igl/remove_unreferenced.h>
#pragma warning(pop)

#include "ConversionUtilities.h"
#include "TriMesh.h"

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

using namespace Urho3D;

String Mesh_CleanMesh::iconTexture = "Textures/Icons/Mesh_CleanMesh.png";

Mesh_CleanMesh::Mesh_CleanMesh(Context* context) : IoComponentBase(context, 1, 2)
{
	SetName("CleanMeshVertices");
	SetFullName("Cull Unused Vertices");
	SetDescription("Cull unused vertices from trimesh");
	SetGroup(IoComponentGroup::MESH);
	SetSubgroup("Operators");

	inputSlots_[0]->SetName("Mesh");
	inputSlots_[0]->SetVariableName("M");
	inputSlots_[0]->SetDescription("Mesh with unused vertices");
	inputSlots_[0]->SetVariantType(VariantType::VAR_VARIANTMAP);
	inputSlots_[0]->SetDataAccess(DataAccess::ITEM);

	outputSlots_[0]->SetName("Mesh");
	outputSlots_[0]->SetVariableName("M");
	outputSlots_[0]->SetDescription("Mesh after removing unused vertices");
	outputSlots_[0]->SetVariantType(VariantType::VAR_VARIANTMAP);
	outputSlots_[0]->SetDataAccess(DataAccess::ITEM);

	outputSlots_[1]->SetName("NumRemoved");
	outputSlots_[1]->SetVariableName("N");
	outputSlots_[1]->SetDescription("Number of vertices removed");
	outputSlots_[1]->SetVariantType(VariantType::VAR_INT);
	outputSlots_[1]->SetDataAccess(DataAccess::ITEM);
}

void Mesh_CleanMesh::SolveInstance(
	const Vector<Variant>& inSolveInstance,
	Vector<Variant>& outSolveInstance
)
{
	assert(inSolveInstance.Size() == inputSlots_.Size());
	assert(outSolveInstance.Size() == outputSlots_.Size());

	///////////////////
	// VERIFY & EXTRACT

	// Verify input slot 0
	Variant inMesh = inSolveInstance[0];
	if (!TriMesh_Verify(inMesh)) {
		URHO3D_LOGWARNING("M must be a valid mesh.");
		outSolveInstance[0] = Variant();
		return;
	}

	///////////////////
	// COMPONENT'S WORK

	Eigen::MatrixXf V, NV;
	Eigen::MatrixXi F, NF;
	bool loadSuccess = IglMeshToMatrices(inMesh, V, F);
	if (!loadSuccess) {
		URHO3D_LOGWARNING("Failed to produce Eigen matrices V, F from trimesh M");
		SetAllOutputsNull(outSolveInstance);
		return;
	}

	Eigen::VectorXi _1;
	igl::remove_unreferenced(V, F, NV, NF, _1);
	Variant out_mesh = TriMesh_Make(NV, NF);
	int num_removed = V.rows() - NV.rows();

	/////////////////
	// ASSIGN OUTPUTS

	outSolveInstance[0] = out_mesh;
	outSolveInstance[1] = Variant(num_removed);
}


#pragma once

#include <cmath>
#include <iostream>
#include <boost/config.hpp>
#include <boost/operators.hpp>

// Taking strong inspiration from the answers on stackoverflow.
// https://stackoverflow.com/questions/23726038/how-can-i-create-a-new-primitive-type-using-c11-style-strong-typedefs
// as well as from Boost strong typedef
// http://www.boost.org/doc/libs/1_61_0/libs/serialization/doc/strong_typedef.html
// In contrast to the boost version I don't want automatic conversion from T to the new strong typedef'ed type.
// This is uselful in case I want to use float as base type. In this case I use c++11 custom literals that
// return the appropriate strong type. That has the benefit of avoiding inefficiencies due to unwanted
// double to float conversion if I forget to write 1.f instead of 1.
// Moreover I don't want the strong type to convert to the base type. Otherwise function applications like
// strong_type x{5.}; pow(x); could implicitly convert to the base type (??).
template <typename T, typename N>
struct very_strong_typedef : public \
  boost::field_operators1<very_strong_typedef<T,N>,
    boost::dividable2<very_strong_typedef<T,N>, long,
      boost::totally_ordered1<very_strong_typedef<T,N>,
        boost::totally_ordered2<very_strong_typedef<T,N>,T>>>>
{
    using strong_type = very_strong_typedef<T,N>;
    using type = T; // the wrapped type

private:
    T value_;

public:
    // Constexpr should make that construction with a literal argument
    // leads to conversion to the appropriate base type at compile time.
    constexpr explicit very_strong_typedef(T val): value_(val){};
    very_strong_typedef(){value_={};}; // default, zero-initialization

    // Conversion operators. But I don't want auto conversion.
    // operator T& () { return value_; }
    // operator const T& () const { return value_; }

    // These are address of operators.
    T* operator &() { return &value_; }
    const T* operator& () const { return &value_; }

    explicit operator T() const { return value_; }

    strong_type& operator+=(const strong_type& rhs)
    {
        value_+=rhs.value_;
        return *this;
    }

    strong_type& operator*=(const strong_type& rhs)
    {
      value_*=rhs.value_;
      return *this;
    }

    strong_type& operator/=(const strong_type& rhs)
    {
      value_/=rhs.value_;
      return *this;
    }

    strong_type& operator/=(long rhs)
    {
      value_/=rhs;
      return *this;
    }

    constexpr strong_type operator-() const
    {
      return strong_type{-value_};
    }

    strong_type& operator-=(const strong_type& rhs)
    {
      value_-=rhs.value_;
      return *this;
    }

    bool operator<(const strong_type& rhs) const
    {
      return value_ < rhs.value_;
    }

    bool operator<(T rhs) const
    {
      return value_ < rhs;
    }

    friend inline bool operator==(strong_type a, strong_type b)
    {
      return a.value_ == b.value_;
    }

    // Function definitions for use with argument dependent lookup
    // http://en.cppreference.com/w/cpp/language/adl
    friend inline type value(strong_type x)
    {
      return x.value_;
    }

    friend inline strong_type pow(strong_type x, strong_type e)
    {
      return strong_type{std::pow(value(x), value(e))};
    }

    friend inline strong_type abs(strong_type x)
    {
      using namespace std;
      return strong_type{abs(x.value_)};
    }

    friend inline auto isfinite(strong_type x)
    {
      using namespace std;
      return isfinite(value(x));
    }

    friend std::ostream& operator<<(std::ostream & lhs, const strong_type& rhs)
    {
        lhs << rhs.value_;
        return lhs;
    }
};


#include <big_types.h>
#include <big_return_codes.h>
#include <min_map/big_compute_index_sets.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(big_compute_index_sets);

BOOST_AUTO_TEST_CASE(test_case1)
{
  typedef double                    real_type;
  typedef ublas::vector<real_type>  vector_type;
  typedef ublas::vector<size_t>     idx_vector_type;

  vector_type x;
  vector_type y;

  x.resize(10,false);
  y.resize(10,false);

  x(0) =  0.0;   y(0) =  0.0;  // inactive
  x(1) = -1.0;   y(1) =  1.0;  // inactive
  x(2) =  1.0;   y(2) = -1.0;  // active
  x(3) = -2.0;   y(3) = -1.0;  // inactive
  x(4) =  2.0;   y(4) =  1.0;  // active
  x(5) = -3.0;   y(5) =  0.0;  // inactive
  x(6) =  3.0;   y(6) =  0.0;  // active
  x(7) = -0.5;   y(7) =  0.5;  // inactive
  x(8) =  0.5;   y(8) = -0.5;  // active
  x(9) =  0.25;  y(9) = 0.25;  // inactive

  idx_vector_type bitmask;
  size_t cnt_active;
  size_t cnt_inactive;
  big::compute_index_sets( y, x, bitmask, cnt_active, cnt_inactive );

  BOOST_CHECK( cnt_active == 4 );
  BOOST_CHECK( cnt_inactive == 6 );

  BOOST_CHECK( bitmask(0) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(1) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(2) == big::IN_ACTIVE );
  BOOST_CHECK( bitmask(3) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(4) == big::IN_ACTIVE );
  BOOST_CHECK( bitmask(5) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(6) == big::IN_ACTIVE );
  BOOST_CHECK( bitmask(7) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(8) == big::IN_ACTIVE );
  BOOST_CHECK( bitmask(9) == big::IN_NON_ACTIVE );
}

BOOST_AUTO_TEST_CASE(test_case2)
{
  typedef double                    real_type;
  typedef ublas::vector<real_type>  vector_type;
  typedef ublas::vector<size_t>     idx_vector_type;

  vector_type x;
  vector_type y;

  x.resize(10,false);
  y.resize(10,false);

  x(0) =  0.0;   y(0) =  0.0;  // inactive
  x(1) = -1.0;   y(1) =  1.0;  // inactive
  x(2) = -2.0;   y(2) =  2.0;  // inactive
  x(3) = -3.0;   y(3) =  3.0;  // inactive
  x(4) = -4.0;   y(4) =  4.0;  // inactive
  x(5) = -5.0;   y(5) =  5.0;  // inactive
  x(6) = -6.0;   y(6) =  6.0;  // inactive
  x(7) = -7.0;   y(7) =  7.0;  // inactive
  x(8) = -8.0;   y(8) =  8.0;  // inactive
  x(9) = -9.0;   y(9) =  9.0;  // inactive

  idx_vector_type bitmask;
  size_t cnt_active;
  size_t cnt_inactive;
  big::compute_index_sets( y, x, bitmask, cnt_active, cnt_inactive );

  BOOST_CHECK( cnt_active == 0 );
  BOOST_CHECK( cnt_inactive == 10 );

  BOOST_CHECK( bitmask(0) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(1) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(2) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(3) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(4) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(5) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(6) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(7) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(8) == big::IN_NON_ACTIVE );
  BOOST_CHECK( bitmask(9) == big::IN_NON_ACTIVE );
}

BOOST_AUTO_TEST_CASE(test_case3)
{
  typedef double                    real_type;
  typedef ublas::vector<real_type>  vector_type;
  typedef ublas::vector<size_t>     idx_vector_type;

  vector_type x;
  vector_type y;

  x.resize(10,false);
  y.resize(10,false);

  x(0) =  0.1;   y(0) =  0.0;  // active
  x(1) =  1.0;   y(1) = -1.0;  // active
  x(2) =  2.0;   y(2) = -2.0;  // active
  x(3) =  3.0;   y(3) = -3.0;  // active
  x(4) =  4.0;   y(4) = -4.0;  // active
  x(5) =  5.0;   y(5) = -5.0;  // active
  x(6) =  6.0;   y(6) = -6.0;  // active
  x(7) =  7.0;   y(7) = -7.0;  // active
  x(8) =  8.0;   y(8) = -8.0;  // active
  x(9) =  9.0;   y(9) = -9.0;  // active

  idx_vector_type bitmask;
  size_t cnt_active;
  size_t cnt_inactive;
  big::compute_index_sets( y, x, bitmask, cnt_active, cnt_inactive );

  BOOST_CHECK( cnt_active == 10 );
  BOOST_CHECK( cnt_inactive == 0 );

  BOOST_CHECK( bitmask(0) == big::IN_ACTIVE );
  BOOST_CHECK( bitmask(1) == big::IN_ACTIVE );
  BOOST_CHECK( bitmask(2) == big::IN_ACTIVE );
  BOOST_CHECK( bitmask(3) == big::IN_ACTIVE );
  BOOST_CHECK( bitmask(4) == big::IN_ACTIVE );
  BOOST_CHECK( bitmask(5) == big::IN_ACTIVE );
  BOOST_CHECK( bitmask(6) == big::IN_ACTIVE );
  BOOST_CHECK( bitmask(7) == big::IN_ACTIVE );
  BOOST_CHECK( bitmask(8) == big::IN_ACTIVE );
  BOOST_CHECK( bitmask(9) == big::IN_ACTIVE );
}

BOOST_AUTO_TEST_SUITE_END();


/*
 * RigidBody.hpp
 *
 *  Created on: July 18, 2019
 *      Author: Quincy Jones
 *
 * Copyright (c) <2019> <Quincy Jones - quincy@implementedrobotics.com/>
 * 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.
 */

#ifndef NOMAD_SYSTEMS_RIGIDBODY_H
#define NOMAD_SYSTEMS_RIGIDBODY_H

//
#include <Systems/DynamicalSystem.hpp>

// C System Files

// C++ System Files
#include <iostream>
#include <string>

// Third Party Includes
#include <Eigen/Dense>

// Project Include Files

class RigidBlock1D : public LinearDynamicalSystem
{
    public:

        // Constructor
        RigidBlock1D();
        RigidBlock1D(const double& mass, const Eigen::Vector3d& box_shape, const double& T_s = 1e-1);

        // Step System
        void Step(const Eigen::VectorXd& u);
        void Step(double u);

        // Update
        void Update();

        // Mass
        double GetMass() const {return mass_;}

    protected:
        double mass_;
        double width_;
        double height_;
        double length_;

};

#endif // NOMAD_SYSTEMS_RIGIDBODY_H


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 * Copyright 2012 The MITRE Corporation                                      *
 *                                                                           *
 * 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 <QFutureSynchronizer>
#include <QtConcurrentRun>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Core>
#include "openbr_internal.h"

#include "openbr/core/common.h"
#include "openbr/core/opencvutils.h"
#include "openbr/core/qtutils.h"

using namespace cv;

namespace br
{

/*!
 * \ingroup transforms
 * \brief Histogram equalization
 * \author Josh Klontz \cite jklontz
 */
class EqualizeHistTransform : public UntrainableTransform
{
    Q_OBJECT

    void project(const Template &src, Template &dst) const
    {
        equalizeHist(src, dst);
    }
};

BR_REGISTER(Transform, EqualizeHistTransform)

/*!
 * \ingroup transforms
 * \brief Normalize matrix to unit length
 * \author Josh Klontz \cite jklontz
 */
class NormalizeTransform : public UntrainableTransform
{
    Q_OBJECT
    Q_ENUMS(NormType)
    Q_PROPERTY(NormType normType READ get_normType WRITE set_normType RESET reset_normType STORED false)

    Q_PROPERTY(bool ByRow READ get_ByRow WRITE set_ByRow RESET reset_ByRow STORED false)
    BR_PROPERTY(bool, ByRow, false)
    Q_PROPERTY(int alpha READ get_alpha WRITE set_alpha RESET reset_alpha STORED false)
    BR_PROPERTY(int, alpha, 1)
    Q_PROPERTY(int beta READ get_beta WRITE set_beta RESET reset_beta STORED false)
    BR_PROPERTY(int, beta, 0)

public:
    /*!< */
    enum NormType { Inf = NORM_INF,
                    L1 = NORM_L1,
                    L2 = NORM_L2,
                    Range = NORM_MINMAX };

private:
    BR_PROPERTY(NormType, normType, L2)

    void project(const Template &src, Template &dst) const
    {
        if (!ByRow) normalize(src, dst, alpha, beta, normType, CV_32F);
        else {
            dst = src;
            for (int i=0; i<dst.m().rows; i++) {
                Mat temp;
                cv::normalize(dst.m().row(i), temp, alpha, beta, normType);
                temp.copyTo(dst.m().row(i));
            }
        }

    }
};

BR_REGISTER(Transform, NormalizeTransform)

/*!
 * \ingroup transforms
 * \brief Normalize each dimension based on training data.
 * \author Josh Klontz \cite jklontz
 */
class CenterTransform : public Transform
{
    Q_OBJECT
    Q_ENUMS(Method)
    Q_PROPERTY(Method method READ get_method WRITE set_method RESET reset_method STORED false)
    Q_PROPERTY(QString inputVariable READ get_inputVariable WRITE set_inputVariable RESET reset_inputVariable STORED false)

public:
    /*!< */
    enum Method { Mean,
                  Median,
                  Range,
                  Hellinger };

private:
    BR_PROPERTY(Method, method, Mean)
    BR_PROPERTY(QString, inputVariable, "Label")

    Mat a, b; // dst = (src - b) / a

    static void _train(Method method, const cv::Mat &m, const QList<int> &labels, double *ca, double *cb)
    {
        double A = 1, B = 0;
        if      (method == Mean)      mean(m, &A, &B);
        else if (method == Median)    median(m, &A, &B);
        else if (method == Range)     range(m, &A, &B);
        else if (method == Hellinger) hellinger(m, labels, &A, &B);
        else                          qFatal("Invalid method.");
        *ca = A;
        *cb = B;
    }

    void train(const TemplateList &data)
    {
        Mat m;
        OpenCVUtils::toMat(data.data()).convertTo(m, CV_64F);

        const QList<int> labels = data.indexProperty(inputVariable);
        const int dims = m.cols;

        std::vector<Mat> mv, av, bv;
        split(m, mv);
        for (size_t c = 0; c < mv.size(); c++) {
            av.push_back(Mat(1, dims, CV_64FC1));
            bv.push_back(Mat(1, dims, CV_64FC1));
        }

        QFutureSynchronizer<void> futures;
        for (size_t c = 0; c < mv.size(); c++) {
            for (int i=0; i<dims; i++)
                futures.addFuture(QtConcurrent::run(_train, method, mv[c].col(i), labels, &av[c].at<double>(0, i), &bv[c].at<double>(0, i)));
            av[c] = av[c].reshape(1, data.first().m().rows);
            bv[c] = bv[c].reshape(1, data.first().m().rows);
        }
        futures.waitForFinished();

        merge(av, a);
        merge(bv, b);
        a.convertTo(a, data.first().m().type());
        b.convertTo(b, data.first().m().type());
        OpenCVUtils::saveImage(a, Globals->property("CENTER_TRAIN_A").toString());
        OpenCVUtils::saveImage(b, Globals->property("CENTER_TRAIN_B").toString());
    }

    void project(const Template &src, Template &dst) const
    {
        subtract(src, b, dst);
        divide(dst, a, dst);
    }

    void store(QDataStream &stream) const
    {
        stream << a << b;
    }

    void load(QDataStream &stream)
    {
        stream >> a >> b;
    }

    static void mean(const Mat &src, double *a, double *b)
    {
        Scalar mean, stddev;
        meanStdDev(src, mean, stddev);
        *a = stddev[0];
        *b = mean[0];
    }

    static void median(const Mat &src, double *a, double *b)
    {
        QVector<double> vals; vals.reserve(src.rows);
        for (int i=0; i<src.rows; i++)
            vals.append(src.at<double>(i, 0));
        double q1, q3;
        *b = Common::Median(vals, &q1, &q3);
        *a = q3 - q1;
    }

    static void range(const Mat &src, double *a, double *b)
    {
        double min, max;
        minMaxLoc(src, &min, &max);
        *a = max - min;
        *b = min;
    }

    static void hellinger(const Mat &src, const QList<int> &labels, double *a, double *b)
    {
        const QList<float> vals = OpenCVUtils::matrixToVector<float>(src);
        if (vals.size() != labels.size())
            qFatal("Logic error.");

        QVector<float> genuineScores; genuineScores.reserve(vals.size());
        QVector<float> impostorScores; impostorScores.reserve(vals.size()*vals.size()/2);
        for (int i=0; i<vals.size(); i++)
            for (int j=i+1; j<vals.size(); j++)
                if (labels[i] == labels[j]) genuineScores.append(vals[i]-vals[j]);
                else                        impostorScores.append(vals[i]-vals[j]);

        float min, max;
        Common::MinMax(vals, &min, &max);

        double gm, gs, im, is;
        Common::MeanStdDev(genuineScores, &gm, &gs);
        Common::MeanStdDev(impostorScores, &im, &is);

        *a = (max-min)/sqrt(1-sqrt(2*gs*is/(gs*gs+is*is))*exp(-0.25*pow(gm-im,2.0)/(gs*gs+is*is)));
        *b = min;
    }
};

BR_REGISTER(Transform, CenterTransform)

/*!
 * \ingroup transforms
 * \brief Remove the row-wise training set average.
 * \author Josh Klontz \cite jklontz
 */
class RowWiseMeanCenterTransform : public Transform
{
    Q_OBJECT
    Mat mean;

    void train(const TemplateList &data)
    {
        Mat m = OpenCVUtils::toMatByRow(data.data());
        mean = Mat(1, m.cols, m.type());
        for (int i=0; i<m.cols; i++)
            mean.col(i) = cv::mean(m.col(i));
    }

    void project(const Template &src, Template &dst) const
    {
        Mat m = src.m().clone();
        for (int i=0; i<m.rows; i++)
            m.row(i) -= mean;
        dst = m;
    }

    void store(QDataStream &stream) const
    {
        stream << mean;
    }

    void load(QDataStream &stream)
    {
        stream >> mean;
    }
};

BR_REGISTER(Transform, RowWiseMeanCenterTransform)

/*!
 * \ingroup transforms
 * \brief dst=sqrt(norm_L1(src)) proposed as RootSIFT in \cite Arandjelovic12
 * \author Josh Klontz \cite jklontz
 */
class RootNormTransform : public UntrainableTransform
{
    Q_OBJECT

    void project(const Template &src, Template &dst) const
    {
        const Mat &m = src;
        dst.m() = Mat(m.rows, m.cols, m.type());
        for (int i=0; i<m.rows; i++) {
            Mat temp;
            cv::normalize(m.row(i), temp, 1, 0, NORM_L1);
            cv::sqrt(temp, temp);
            temp.copyTo(dst.m().row(i));
        }
    }
};

BR_REGISTER(Transform, RootNormTransform)

} // namespace br

#include "normalize.moc"


#include <cstdio>
#include <boost/mpl/vector.hpp>
#include <nt2/sdk/meta/any.hpp>
#include <boost/type_traits/is_same.hpp>

using namespace boost;
using namespace boost::mpl;

int main()
{
  typedef vector<int,bool,float> seq_t;

  typedef nt2::meta::any<is_same<_,bool>,seq_t> any_seq;
  typedef nt2::meta::any<is_same<_,bool>,int,bool,float> any_types;

  printf("is any type bool in vector<int,bool,float> : %d\n", any_seq::value);
  printf("is any type bool in (int,bool,float) : %d\n", any_types::value);
}


/*
Copyright 2012, 2013 Rogier van Dalen.

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

/** \file
Test log_float.hpp.
Text-search for "****" to find these sections:
- domain error
- overflow error
- underflow error
- basic functionality on corner cases
- binary operations
- functions that mimic the ones from <cmath>
- conversion and interaction

Particular care is taken (hopefully!) to check that everything works with
different policies.
The only error handling policies that are tested are throwing and ignoring.
The implementation uses Boost.Math functions to raise errors so this should
generalise to other forms of behaviour.
Testing the ignoring policies is important if specialisations for error policies
bypass explicit error handling in favour of leaving this to the hardware.
*/

#define BOOST_TEST_MODULE log_float_hash
#include "../boost_unit_test.hpp"

#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/tools/test.hpp>

#include "math/log-float.hpp"

#include "range/std/container.hpp"

#include "math/check/check_hash.hpp"

BOOST_AUTO_TEST_SUITE(test_suite_log_float_hash)

BOOST_AUTO_TEST_CASE (test_hash) {
    typedef math::log_float <double> log_float;
    typedef math::signed_log_float <double> signed_log_float;

    // Check hash on log_float.
    std::vector <log_float> examples;
    examples.push_back (0);
    examples.push_back (log_float (
        -std::numeric_limits <double>::max(), math::as_exponent()));
    examples.push_back (1.e-30);
    examples.push_back (1.e-10);
    examples.push_back (.1);
    examples.push_back (log_float (-0., math::as_exponent()));
    examples.push_back (log_float (0., math::as_exponent()));
    examples.push_back (.5);
    examples.push_back (1);
    examples.push_back (2);
    examples.push_back (10);
    examples.push_back (1.e+30);
    examples.push_back (log_float (
        std::numeric_limits <double>::max(), math::as_exponent()));
    examples.push_back (log_float (
        std::numeric_limits <double>::infinity(), math::as_exponent()));

    math::check_hash (examples);

    // Check hash on signed_log_float.
    // For positive values, the hash value should be the same.
    math::check_cast_hash <signed_log_float> (examples);

    std::vector <signed_log_float> signed_examples;
    for (log_float example : examples) {
        signed_examples.push_back (example);
        signed_examples.push_back (-example);
    }
    math::check_hash (signed_examples);
}

BOOST_AUTO_TEST_SUITE_END()


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author:       François Godi, Vincent Rouvreau
 *
 *    Copyright (C) 2018  INRIA
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <iostream>
#include <vector>
#include <gudhi/Lazy_toplex_map.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "lazy toplex map"
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(toplex_map) {
  using Vertex = Gudhi::Lazy_toplex_map::Vertex;

  Gudhi::Lazy_toplex_map tm;
  std::cout << "insert_simplex {1, 2, 3, 4}" << std::endl;
  std::vector<Vertex> sigma1 = {1, 2, 3, 4};
  tm.insert_simplex(sigma1);
  std::cout << "insert_simplex {5, 2, 3, 6}" << std::endl;
  std::vector<Vertex> sigma2 = {5, 2, 3, 6};
  tm.insert_simplex(sigma2);
  std::cout << "insert_simplex {5}" << std::endl;
  std::vector<Vertex> sigma3 = {5};
  tm.insert_simplex(sigma3);
  std::cout << "insert_simplex {4, 5, 3}" << std::endl;
  std::vector<Vertex> sigma6 = {4, 5, 3};
  tm.insert_simplex(sigma6);
  std::cout << "insert_simplex {4, 5, 9}" << std::endl;
  std::vector<Vertex> sigma7 = {4, 5, 9};
  tm.insert_simplex(sigma7);

  std::cout << "num_maximal_simplices = " << tm.num_maximal_simplices() << std::endl;
  BOOST_CHECK(tm.num_maximal_simplices() == 5);

  std::vector<Vertex> sigma4 = {5, 2, 3};
  std::vector<Vertex> sigma5 = {5, 2, 7};
  BOOST_CHECK(tm.membership(sigma4));
  BOOST_CHECK(!tm.membership(sigma5));
  std::cout << "insert_simplex {5, 2, 7}" << std::endl;
  tm.insert_simplex(sigma5);

  std::cout << "num_maximal_simplices = " << tm.num_maximal_simplices() << std::endl;
  BOOST_CHECK(tm.num_maximal_simplices() == 6);

  BOOST_CHECK(tm.membership(sigma5));

  std::cout << "contraction(4,5)" << std::endl;
  auto r = tm.contraction(4, 5);
  std::cout << "r=" << r << std::endl;
  BOOST_CHECK(r == 5);

  std::cout << "num_maximal_simplices = " << tm.num_maximal_simplices() << std::endl;
  BOOST_CHECK(tm.num_maximal_simplices() == 6);

  std::vector<Vertex> sigma8 = {1, 2, 3};
  std::vector<Vertex> sigma9 = {2, 7};

  sigma8.emplace_back(r);
  sigma9.emplace_back(r);
  BOOST_CHECK(!tm.membership(sigma6));
  BOOST_CHECK(tm.membership(sigma8));
  BOOST_CHECK(tm.membership(sigma9));

  std::cout << "remove_simplex({2, 7, r = 5})" << std::endl;
  tm.remove_simplex(sigma9);
  BOOST_CHECK(!tm.membership(sigma9));

  std::cout << "num_maximal_simplices = " << tm.num_maximal_simplices() << std::endl;
  BOOST_CHECK(tm.num_maximal_simplices() == 8);

  // {2, 7, 5} is removed, but verify its edges are still there
  std::vector<Vertex> edge = {2, 7};
  BOOST_CHECK(tm.membership(edge));
  edge = {2, 5};
  BOOST_CHECK(tm.membership(edge));
  edge = {7, 5};
  BOOST_CHECK(tm.membership(edge));
}

BOOST_AUTO_TEST_CASE(toplex_map_empty_toplex) {
  using Vertex = Gudhi::Lazy_toplex_map::Vertex;

  Gudhi::Lazy_toplex_map tm;
  std::cout << "num_maximal_simplices = " << tm.num_maximal_simplices() << std::endl;
  BOOST_CHECK(tm.num_maximal_simplices() == 0);
  std::cout << "num_vertices = " << tm.num_vertices() << std::endl;
  BOOST_CHECK(tm.num_vertices() == 0);

  std::cout << "Check an empty simplex is a member." << std::endl;
  std::vector<Vertex> empty_sigma = {};
  BOOST_CHECK(tm.membership(empty_sigma));

  std::cout << "Check the edge 2,7 is not a member." << std::endl;
  std::vector<Vertex> edge = {2, 7};
  BOOST_CHECK(!tm.membership(edge));

  std::cout << "Insert an empty simplex." << std::endl;
  tm.insert_simplex(empty_sigma);

  std::cout << "num_maximal_simplices = " << tm.num_maximal_simplices() << std::endl;
  BOOST_CHECK(tm.num_maximal_simplices() == 0);
  std::cout << "num_vertices = " << tm.num_vertices() << std::endl;
  BOOST_CHECK(tm.num_vertices() == 0);

  std::cout << "Check an empty simplex is a member." << std::endl;
  BOOST_CHECK(tm.membership(empty_sigma));
  std::cout << "Check the edge 2,7 is not a member." << std::endl;
  BOOST_CHECK(!tm.membership(edge));

  std::cout << "Insert edge 2,7." << std::endl;
  tm.insert_simplex(edge);

  std::cout << "num_maximal_simplices = " << tm.num_maximal_simplices() << std::endl;
  BOOST_CHECK(tm.num_maximal_simplices() == 1);
  std::cout << "num_vertices = " << tm.num_vertices() << std::endl;
  BOOST_CHECK(tm.num_vertices() == 2);

  std::cout << "Check an empty simplex is a member." << std::endl;
  BOOST_CHECK(tm.membership(empty_sigma));
  std::cout << "Check the edge 2,7 is a member." << std::endl;
  BOOST_CHECK(tm.membership(edge));

  std::cout << "contraction(2,7)" << std::endl;
  auto r = tm.contraction(2, 7);
  std::cout << "r=" << r << std::endl;
  BOOST_CHECK(r == 7);

  std::cout << "num_maximal_simplices = " << tm.num_maximal_simplices() << std::endl;
  BOOST_CHECK(tm.num_maximal_simplices() == 1);
  std::cout << "num_vertices = " << tm.num_vertices() << std::endl;
  BOOST_CHECK(tm.num_vertices() == 1);

  std::cout << "Check an empty simplex is a member." << std::endl;
  BOOST_CHECK(tm.membership(empty_sigma));
  std::cout << "Check the edge 2,7 is not a member." << std::endl;
  BOOST_CHECK(!tm.membership(edge));

  std::cout << "Remove the vertex 7." << std::endl;
  std::vector<Vertex> vertex = {7};
  tm.remove_simplex(vertex);

  std::cout << "num_maximal_simplices = " << tm.num_maximal_simplices() << std::endl;
  BOOST_CHECK(tm.num_maximal_simplices() == 0);
  std::cout << "num_vertices = " << tm.num_vertices() << std::endl;
  BOOST_CHECK(tm.num_vertices() == 0);

  std::cout << "Check an empty simplex is a member." << std::endl;
  BOOST_CHECK(tm.membership(empty_sigma));
  std::cout << "Check the edge 2,7 is not a member." << std::endl;
  BOOST_CHECK(!tm.membership(edge));
}


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     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.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE threecenter_dft_test

// Third party includes
#include <boost/test/unit_test.hpp>

// VOTCA includes
#include <votca/tools/eigenio_matrixmarket.h>
#include <votca/tools/tokenizer.h>

// Local VOTCA includes
#include <votca/xtp/aobasis.h>
#include <votca/xtp/qmmolecule.h>
#include <votca/xtp/threecenter.h>

using namespace votca::xtp;
using namespace votca;

BOOST_AUTO_TEST_SUITE(threecenter_dft_test)

Eigen::MatrixXd ReadMatrixFromString(const std::string& matrix) {
  votca::tools::Tokenizer lines(matrix, "\n");

  std::vector<double> entries;
  Index cols = 0;
  Index rows = 0;
  for (auto line : lines) {
    if (line[0] == '#') {
      continue;
    }
    votca::tools::Tokenizer entries_tok(line, " ");
    std::vector<std::string> temp = entries_tok.ToVector();
    cols = Index(temp.size());
    rows++;
    for (const auto& s : temp) {
      entries.push_back(std::stod(s));
    }
  }

  return Eigen::Map<Eigen::MatrixXd>(entries.data(), rows, cols);
}

BOOST_AUTO_TEST_CASE(small_basis) {

  QMMolecule mol(" ", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                   "/threecenter_dft/molecule.xyz");
  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) + "/threecenter_dft/3-21G.xml");
  AOBasis aobasis;
  aobasis.Fill(basis, mol);
  TCMatrix_dft threec;
  threec.Fill(aobasis, aobasis);

  Eigen::MatrixXd Ref0 = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/threecenter_dft/Ref0.mm");

  Eigen::MatrixXd Ref4 = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/threecenter_dft/Ref4.mm");

  bool check_three1 = Ref0.isApprox(threec[0].FullMatrix(), 0.00001);
  if (!check_three1) {
    std::cout << "Res0" << std::endl;
    std::cout << threec[0].FullMatrix() << std::endl;
    std::cout << "0_ref" << std::endl;
    std::cout << Ref0 << std::endl;
  }
  BOOST_CHECK_EQUAL(check_three1, true);
  bool check_three2 = Ref4.isApprox(threec[4].FullMatrix(), 0.00001);
  if (!check_three2) {
    std::cout << "Res4" << std::endl;
    std::cout << threec[4].FullMatrix() << std::endl;
    std::cout << "4_ref" << std::endl;
    std::cout << Ref4 << std::endl;
  }
  BOOST_CHECK_EQUAL(check_three2, true);
}

BOOST_AUTO_TEST_CASE(large_l_test) {

  QMMolecule mol("C", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                   "/threecenter_dft/C2.xyz");

  BasisSet basisset;
  basisset.Load(std::string(XTP_TEST_DATA_FOLDER) + "/threecenter_dft/G.xml");
  AOBasis dftbasis;
  dftbasis.Fill(basisset, mol);
  BasisSet auxbasisset;
  auxbasisset.Load(std::string(XTP_TEST_DATA_FOLDER) +
                   "/threecenter_dft/I.xml");
  AOBasis auxbasis;
  auxbasis.Fill(auxbasisset, mol);

  TCMatrix_dft threec;
  threec.Fill(auxbasis, dftbasis);
  // we only test half of it because it gets a bit big
  std::array<Eigen::MatrixXd, 4> ref;
  std::array<std::string, 4> ref_string;

  std::array<int, 4> indeces = {0, 1, 3, 12};
  for (Index i = 0; i < 4; i++) {
    ref[i] = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
        std::string(XTP_TEST_DATA_FOLDER) + "/threecenter_dft/RefList" +
        std::to_string(i) + ".mm");
  }

  for (Index i = 0; i < 4; i++) {
    bool check = ref[i].isApprox(threec[indeces[i]].FullMatrix(), 1e-5);
    BOOST_CHECK_EQUAL(check, true);
    if (!check) {
      std::cout << "ref " << indeces[i] << std::endl;
      std::cout << ref[i] << std::endl;
      std::cout << "result " << indeces[i] << std::endl;
      std::cout << threec[indeces[i]].FullMatrix() << std::endl;
    }
  }
}
BOOST_AUTO_TEST_SUITE_END()


//!
//! Contains the interface for interpolation.
//!
//! \file world/interpolatorspec.hpp
//! \author Alistair Reid
//! \author Darren Shen
//! \date 2014
//! \license Affero General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include <Eigen/Core>

#include "datatype/world.hpp"

namespace obsidian
{
  namespace world
  {
    //! Represents a boundary of the layer, which is specified by a mesh grid of
    //! control points that vary in depth according to a vector of parameters.
    //!
    class InterpolatorSpec
    {
      public:
        //! Initialise interpolation params.
        //!
        //! \param xBounds, yBounds, zBounds Pairs containing the min and max boundaries in each direction.
        //! \param boundarySpec The boundary specification.
        //!
        InterpolatorSpec(const std::pair<double, double>& xBounds, const std::pair<double, double>& yBounds,
                         const std::pair<double, double>& zBounds, const BoundarySpec& boundaryspec);

        //! Initialise an empty interpolator specification.
        //!
        InterpolatorSpec();

        //! Get the weights for the mean function. Multiply the transpose of this
        //! with the thetas will give the interpolated depths.
        //!
        Eigen::MatrixXd getWeights(const Eigen::MatrixXd &queryX) const;

        //! Get the number of control points.
        //!
        //! \return The total number of control points.
        //!
        int numControlPoints() const;

        //! Pair containing the x and y resolutions
        std::pair<uint, uint> resolution;

        //!
        Eigen::MatrixXd controlPointX;

        //! The type of boundary that this represents
        BoundaryClass boundaryClass;

        //!
        Eigen::MatrixXd offsetFunction;

        //!
        std::pair<double, double> offsetXBounds;

        //!
        std::pair<double, double> offsetYBounds;

        //!
        double floorHeight;

      private:
        // Some caching:
        Eigen::Vector2d lengthScale_;
        Eigen::LDLT<Eigen::MatrixXd> L_; // Cholesky decomposition factor
    };

  } // world namespace
} // gdf namespace


// This file is part of PolyMPC, a lightweight C++ template library
// for real-time nonlinear optimization and optimal control.
//
// Copyright (C) 2020 Listov Petr <petr.listov@epfl.ch>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef UTILS_HPP
#define UTILS_HPP

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

template <typename qp_t>
void print_qp(qp_t qp)
{
    Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", ",", "[", "]", "[", "]");
    std::cout << "P = " << qp.P.format(fmt) << std::endl;
    std::cout << "q = " << qp.q.transpose().format(fmt) << std::endl;
    std::cout << "A = " << qp.A.format(fmt) << std::endl;
    std::cout << "l = " << qp.l.transpose().format(fmt) << std::endl;
    std::cout << "u = " << qp.u.transpose().format(fmt) << std::endl;
}

template <typename Mat>
bool is_psd(Mat &h)
{
    Eigen::EigenSolver<Mat> eigensolver(h);
    for (int i = 0; i < eigensolver.eigenvalues().RowsAtCompileTime; i++) {
        double v = eigensolver.eigenvalues()(i).real();
        if (v < 0) {
            return false;
        }
    }
    return true;
}

#endif /* UTILS_HPP */


/* Copyright 2022 Zuru Tech HK Limited.
 *
 * 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 <cmath>
#include <filesystem>
#include <stdexcept>
#include <tuple>
#include <utility>
#include <vector>

#include <cnpy/cnpy.h>

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <fmt/core.h>

#include <petscmat.h>
#include <petscvec.h>

#include <solvers/SparseSystem.hpp>

namespace solvers {

SparseSystem::SparseSystem(
    const Eigen::SparseMatrix<double, Eigen::RowMajor>& upper_A,
    Eigen::VectorXd b,
    Eigen::VectorXd x)
    : _upper_A(upper_A), _b(std::move(b)), _x(std::move(x))
{
    _upper_A.makeCompressed();
}

SparseSystem SparseSystem::loadNpz(const std::filesystem::path& npz_path)
{
    cnpy::npz_t system = cnpy::npz_load(npz_path.string());
    cnpy::NpyArray A_indices_array = system["A_indices"];
    cnpy::NpyArray A_values_array = system["A_values"];
    cnpy::NpyArray b_array = system["b"];
    cnpy::NpyArray x_array = system["x"];

    const auto n = static_cast<int64_t>(b_array.shape[0]);
    const auto nnz = static_cast<int64_t>(A_values_array.shape[0]);

    Eigen::VectorXd b = Eigen::Map<Eigen::VectorXd>(b_array.data<double>(), n);
    Eigen::VectorXd x = Eigen::Map<Eigen::VectorXd>(x_array.data<double>(), n);
    Eigen::MatrixX2<int> indices =
        Eigen::Map<Eigen::MatrixX2<int>>(A_indices_array.data<int>(), nnz, 2);
    Eigen::VectorXd values =
        Eigen::Map<Eigen::VectorXd>(A_values_array.data<double>(), nnz);

    std::vector<Eigen::Triplet<double>> upper_A_entries;
    upper_A_entries.reserve(nnz / 2 + n);
    for (int k = 0; k < nnz; ++k) {
        if (indices(k, 0) <= indices(k, 1)) {
            upper_A_entries.emplace_back(Eigen::Triplet<double>(
                indices(k, 0), indices(k, 1), values(k)));
        }
    }
    Eigen::SparseMatrix<double, Eigen::RowMajor> upper_A(n, n);
    upper_A.setFromTriplets(upper_A_entries.begin(), upper_A_entries.end());
    upper_A.makeCompressed();
    return SparseSystem{upper_A, b, x};
}

void SparseSystem::saveNpz(const std::filesystem::path& npz_path) const
{
    Eigen::SparseMatrix<double, Eigen::RowMajor> A(
        _upper_A.selfadjointView<Eigen::Upper>());
    const size_t n = A.rows();
    const size_t nnz = A.nonZeros();

    using InnerIterator =
        Eigen::SparseMatrix<double, Eigen::RowMajor>::InnerIterator;
    std::vector<int> indices_vector;
    indices_vector.reserve(2 * nnz);
    for (Eigen::Index row = 0; row < static_cast<Eigen::Index>(n); ++row) {
        for (InnerIterator it(A, row); it; ++it) {
            indices_vector.emplace_back(it.row());
        }
    }
    for (size_t c = 0; c < nnz; ++c) {
        indices_vector.emplace_back(*(A.innerIndexPtr() + c));
    }

    cnpy::npz_save<int>(npz_path.string(), "A_indices", indices_vector.data(),
                        {2, nnz}, "a");
    cnpy::npz_save<double>(npz_path.string(), "A_values", A.valuePtr(), {nnz},
                           "a");
    cnpy::npz_save<double>(npz_path.string(), "b", _b.data(), {n}, "a");
    cnpy::npz_save<double>(npz_path.string(), "x", _x.data(), {n}, "a");
}

void SparseSystem::removeNullEntries(double tolerance)
{
    _upper_A.prune([tolerance](const Eigen::Index row, const Eigen::Index col,
                               const double value) {
        return std::abs(value) > tolerance;
    });
}

size_t SparseSystem::dim() const
{
    return static_cast<size_t>(_b.size());
}

size_t SparseSystem::nnz() const
{
    Eigen::SparseMatrix<double, Eigen::RowMajor> A(
        _upper_A.selfadjointView<Eigen::Upper>());
    return static_cast<size_t>(A.nonZeros());
}

Eigen::VectorXd SparseSystem::getEigenSolution() const
{
    return _x;
}

Vec SparseSystem::getPetscSolution() const
{
    auto n = static_cast<int>(_b.size());
    Vec x;
    VecCreate(PETSC_COMM_WORLD, &x);
    VecSetSizes(x, PETSC_DECIDE, n);
    VecSetFromOptions(x);
    std::vector<int> indices(n);
    std::iota(indices.begin(), indices.end(), 0);
    VecSetValues(x, n, indices.data(), _x.data(), INSERT_VALUES);
    VecAssemblyBegin(x);
    VecAssemblyEnd(x);
    return x;
}

std::tuple<Eigen::SparseMatrix<double, Eigen::ColMajor>, Eigen::VectorXd>
SparseSystem::toEigenCSC() const
{
    Eigen::SparseMatrix<double, Eigen::ColMajor> A(
        _upper_A.selfadjointView<Eigen::Upper>());
    return std::make_tuple(A, _b);
}

std::tuple<Eigen::SparseMatrix<double, Eigen::RowMajor>, Eigen::VectorXd>
SparseSystem::toEigenCSR() const
{
    Eigen::SparseMatrix<double, Eigen::RowMajor> A(
        _upper_A.selfadjointView<Eigen::Upper>());
    return std::make_tuple(A, _b);
}

std::tuple<Eigen::SparseMatrix<double, Eigen::RowMajor>, Eigen::VectorXd>
SparseSystem::toEigenUpperCSR() const
{
    return std::make_tuple(_upper_A, _b);
}

std::tuple<Mat, Vec> SparseSystem::toPetscCSR() const
{
    auto n = static_cast<int>(_b.size());

    Mat upper_A;
    Eigen::SparseMatrix<double, Eigen::RowMajor> eigen_upper_A = _upper_A;
    MatCreateSeqSBAIJWithArrays(
        PETSC_COMM_SELF, 1, n, n, eigen_upper_A.outerIndexPtr(),
        eigen_upper_A.innerIndexPtr(), eigen_upper_A.valuePtr(), &upper_A);
    Mat A;
    MatConvert(upper_A, MATSEQAIJ, MAT_INITIAL_MATRIX, &A);

    Vec b;
    VecCreate(PETSC_COMM_WORLD, &b);
    VecSetSizes(b, PETSC_DECIDE, n);
    VecSetFromOptions(b);
    std::vector<int> indices(n);
    std::iota(indices.begin(), indices.end(), 0);
    VecSetValues(b, n, indices.data(), _b.data(), INSERT_VALUES);
    VecAssemblyBegin(b);
    VecAssemblyEnd(b);

    return std::make_tuple(A, b);
}

std::tuple<std::vector<int>,
           std::vector<int>,
           std::vector<double>,
           std::vector<double>>
SparseSystem::toStdCSR() const
{
    const Eigen::SparseMatrix<double, Eigen::RowMajor> A(
        _upper_A.selfadjointView<Eigen::Upper>());
    const size_t n = A.rows();
    const size_t nnz = A.nonZeros();
    std::vector<int> crow_index(A.outerIndexPtr(), A.outerIndexPtr() + n + 1);
    std::vector<int> col_index(A.innerIndexPtr(), A.innerIndexPtr() + nnz);
    std::vector<double> values(A.valuePtr(), A.valuePtr() + nnz);
    std::vector<double> b(_b.data(), _b.data() + n);
    return std::make_tuple(crow_index, col_index, values, b);
}

}    // namespace solvers


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef LIMEX_HH
#define LIMEX_HH

#include <boost/timer/timer.hpp>

#include "timestepping/extrapolation.hh"
#include "fem/iterate_grid.hh"
#include "timestepping/semieuler.hh"

namespace Kaskade
{
  /**
   * This class implements the extrapolated linearly implicit Euler
   * method for integrating time-dependent evolution problems. The
   * implementation follows Deuflhard/Bornemann Chapter 6.4.3.
   *
   * \todo Currently the matrix \f$ B \f$ has to be constant.
   */
  template <class Eq>
  class Limex
  {
  public:
    typedef Eq                                                     EvolutionEquation;
    typedef typename EvolutionEquation::AnsatzVars::VariableSet State;

  private:
    typedef SemiLinearizationAt<SemiImplicitEulerStep<EvolutionEquation> > Linearization;
    typedef VariationalFunctionalAssembler<Linearization> GOp;

  public:
    /**
     * Constructs an ODE integrator. The arguments eq and ansatzVars
     * have to exist during the lifetime of the integrator.
     */
    Limex(GridManager<typename EvolutionEquation::AnsatzVars::Grid>& gridManager,
        EvolutionEquation& eq_, typename EvolutionEquation::AnsatzVars const& ansatzVars_,
        std::vector<std::pair<double,double> > const& tolX):
          ansatzVars(ansatzVars_), eq(&eq_,0), gop(gridManager.signals,ansatzVars.spaces), extrap(0),
          rhsAssemblyTime(0.0), matrixAssemblyTime(0.0), factorizationTime(0.0), solutionTime(0.0) {}

    /**
     * Computes a state increment that advances the given state in
     * time. The time in the given evolution equation is increased by
     * dt.
     *
     * \param x the initial state to be evolved
     * \param dt the time step
     * \param order the extrapolation order
     * \return the state increment (references an internal variable that will be invalidated by a subsequent call of step)
     *
     * \todo (i) check for B constant, do not reassemble matrix in this case (ii) implement fixed point
     * iteration instead of new factorization in case B is not constant
     */
    State const& step(State const& x, double dt, int order)
    {
      boost::timer::cpu_timer timer;

      std::vector<double> stepFractions(order+1);
      for (int i=0; i<=order; ++i) stepFractions[i] = 1.0/(i+1);
      extrap.clear();

      int const nvars = EvolutionEquation::AnsatzVars::noOfVariables;
      int const neq = EvolutionEquation::TestVars::noOfVariables;
      size_t nnz = gop.nnz(0,neq,0,nvars,false);
      size_t size = ansatzVars.degreesOfFreedom(0,nvars);

      std::vector<int> ridx(nnz), cidx(nnz);
      std::vector<double> data(nnz), rhs(size), sol(size);

      State dx(x), dxsum(x), tmp(x);
      double const t = eq.time();

      eq.temporalEvaluationRange(t,t+dt);

      for (int i=0; i<=order; ++i) {
        double const tau = stepFractions[i]*dt;
        eq.setTau(tau);
        eq.time(t);

        // Evaluate and factorize matrix B(t)-tau*J
        dx *= 0;
        timer.start();
        gop.assemble(Linearization(eq,x,x,dx));
        matrixAssemblyTime += (double)(timer.elapsed().user)/1e9;

        timer.start();
        gop.toTriplet(0,neq,0,nvars,ridx.begin(),cidx.begin(),data.begin(),false);
        UMFFactorization<double> matrix(size,0,ridx,cidx,data);
        factorizationTime += timer.elapsed();

        // First right hand side (j=0) has been assembled together with matrix.
        timer.start();
        gop.toSequence(0,neq,rhs.begin());
        for (int k=0; k<rhs.size(); ++k) assert(finite(rhs[k]));
        matrix.solve(rhs,sol);
        for (int k=0; k<sol.size(); ++k) assert(finite(sol[k]));
        dx.read(sol.begin());
        dxsum = dx;
        solutionTime += (double)(timer.elapsed().user)/1e9;

        // propagate by linearly implicit Euler
        for (int j=1; j<=i; ++j) {
          // Assemble new right hand side tau*f(x_j)+(B(t)-B(t+j*tau))*dx_(j-1)
          eq.time(eq.time()+tau);
          tmp = x; tmp += dxsum;
          timer.start();
          gop.assemble(Linearization(eq,tmp,x,dx),GOp::RHS);
          rhsAssemblyTime += timer.elapsed();
          timer.start();
          gop.toSequence(0,neq,rhs.begin());
          for (int k=0; k<rhs.size(); ++k) assert(finite(rhs[k]));
          matrix.solve(rhs,sol);
          for (int k=0; k<sol.size(); ++k) assert(finite(sol[k]));
          dx.read(sol.begin());
          dxsum += dx;
          solutionTime += (double)(timer.elapsed().user)/1e9;
        }

        // insert into extrapolation tableau
        extrap.push_back(dxsum,stepFractions[i]);

        // restore initial time
        eq.time(t);
      }

      return extrap.back();
    }

    double estimateError(State const& x,int i, int j) const
    {
      assert(extrap.size()>1);

      std::vector<std::pair<double,double> > e(ansatzVars.noOfVariables);

      relativeError(typename EvolutionEquation::AnsatzVars::Variables(),extrap[i].data,
                    extrap[j].data,x.data,ansatzVars.spaces,eq.scaling(),
                    e.begin());

      return e[0].first/(0.1+e[0].second);
    }

    template <class OutStream>
    void reportTime(OutStream& out) const {
      out << "Limex time: " << matrixAssemblyTime << "s matrix assembly\n"
          << "            " << rhsAssemblyTime << "s rhs assembly\n"
          << "            " << factorizationTime << "s factorization\n"
          << "            " << solutionTime << "s solution\n";
    }

    void advanceTime(double dt) { eq.time(eq.time()+dt); }

  private:
    typename EvolutionEquation::AnsatzVars const& ansatzVars;
    SemiImplicitEulerStep<EvolutionEquation>  eq;
    GOp                                       gop;

  public:
    ExtrapolationTableau<State>  extrap;
    double rhsAssemblyTime, matrixAssemblyTime, factorizationTime, solutionTime;
  };
} // namespace Kaskade
#endif


/*
 * MatcherPacker.cpp
 *
 *  Created on: Nov 2, 2011
 *      Author: dkoes
 */

#include "MatcherPacker.h"
#include <boost/unordered_set.hpp>
#include <lemon/full_graph.h>
#include <lemon/matching.h>
#include <lemon/list_graph.h>
#include <lemon/connectivity.h>
#include <lemon/graph_to_eps.h>
#include <lemon/lp.h>
#include <ANN/ANN.h>
#include "Timer.h"
using namespace lemon;

void MatcherPacker::pack(const DataViewer* dv, vector<Cluster>& clusters) const
{
	if (dv->size() == 0)
		return;

	vector<unsigned> indices(dv->size());
	for (unsigned i = 0, n = indices.size(); i < n; i++)
		indices[i] = i;

	//start with each object in its own cluster
	clusters.clear();
	clusters.resize(indices.size());
	for (unsigned i = 0, n = indices.size(); i < n; i++)
	{
		unsigned index = indices[i];
		clusters[i].setToSingleton(index, dv->getMIV(index), dv->getMSV(index));
	}

	//combine everything as much as possible
	unsigned curSz = 1;

	if (K == 0)
	{
		FullCache dcache(dv);

		while (curSz < packSize
				&& fullMergeClusters(dv, clusters, curSz, dcache))
			curSz *= 2;
	}
	else //use knn
	{
		OnDemandCache dcache(dv);

		while (curSz < packSize && knnMergeClusters(dv, clusters, curSz, dcache))
			curSz *= 2;

		//cout << "dcache " << dcache.size() << "\n";
	}

}

/* find the pseudo-optimal matching of every pair of clusters
 * use the knn graph instead of the full graph
 * maxSz is what the largest current cluster should be
 */
bool MatcherPacker::knnMergeClusters(const DataViewer *D,
		vector<Cluster>& clusters, unsigned maxSz, DCache& dcache) const
{
	unsigned N = clusters.size();
	SmartGraph SG;
	SmartGraph::EdgeMap<double> Sweights(SG);
	SmartGraph::NodeMap<unsigned> Sindex(SG);
	vector<SmartGraph::Node> Snodes;

	ClusterCache ccache(clusters.size());
	makeKNNGraph(D, clusters, maxSz, dcache, ccache, SG, Sweights, Sindex,
			Snodes);

	Timer t;
	MaxWeightedMatching<SmartGraph, SmartGraph::EdgeMap<double> > matcher(SG,
			Sweights);
	matcher.run();
	//merge clusters that were matched
	vector<Cluster> newclusters;
	newclusters.reserve(N / 2 + 1);
	vector<bool> packed(N, false);
	bool didmerge = false;
	for (unsigned i = 0; i < N; i++)
	{
		if (!packed[i])
		{
			SmartGraph::Node m = matcher.mate(Snodes[i]);
			if (!SG.valid(m))
			{
				newclusters.push_back(Cluster());
				newclusters.back().moveInto(clusters[i]);
			}
			else
			{
				unsigned j = Sindex[m];

				if (clusters[i].size() + clusters[j].size() <= packSize) //merge
				{
					newclusters.push_back(Cluster());
					newclusters.back().mergeInto(clusters[i], clusters[j]);
					didmerge = true;
					packed[i] = true;
					packed[j] = true;
				}
				else
				{
					//didn't merge, keep clusters as is
					newclusters.push_back(Cluster());
					newclusters.back().moveInto(clusters[i]);
					newclusters.push_back(Cluster());
					newclusters.back().moveInto(clusters[j]);
					packed[i] = true;
					packed[j] = true;
				}
			}
		}
	}

	swap(clusters, newclusters);
	return didmerge;
}

//write dot file from graph
void writeDot(SmartGraph& g, SmartGraph::EdgeMap<double>& length,
		SmartGraph::NodeMap<unsigned>& index,
		ostream& out)
{
	out << "graph name {" << endl;
	out << "  node [ shape=ellipse, fontname=Helvetica, fontsize=10 ];" << endl;
	for (SmartGraph::NodeIt n(g); n != INVALID; ++n)
	{
		out << "  n" << g.id(n) << " [ label=\"" << index[n] << "\" ]; " << endl;
	}
	out << "  edge [ shape=ellipse, fontname=Helvetica, fontsize=10 ];" << endl;
	for (SmartGraph::EdgeIt e(g); e != INVALID; ++e)
	{
		out << "  n" << g.id(g.u(e)) << " -- " << " n" << g.id(g.v(e))
				<< " [ label=\"" << length[e] << "\" ]; " << endl;
	}
	out << "}" << endl;

}

typedef pair<unsigned, unsigned> IndexPair;

void writeEGDot(SmartGraph& g, SmartGraph::EdgeMap<double>& length,
		ostream& out, SmartGraph::NodeMap<IndexPair>& edgePairs,
		boost::unordered_set<unsigned>& matched)
{
	out << "graph name {" << endl;
	out << "  node [ shape=ellipse, fontname=Helvetica, fontsize=10 ];" << endl;
	for (SmartGraph::NodeIt n(g); n != INVALID; ++n)
	{
		IndexPair p = edgePairs[n];
		if(p.first != p.second)
			out << "  n" << g.id(n) << " [ label=\"" << p.first << ":" << p.second << "\" ]; " << endl;
		else
			out << "  n" << g.id(n) << " [ shape=triangle, label=\"" << p.first << "\" ]; \n";
	}
	out << "  edge [ shape=ellipse, fontname=Helvetica, fontsize=10 ];" << endl;
	for (SmartGraph::EdgeIt e(g); e != INVALID; ++e)
	{
		string extra = "";
		if(matched.count(g.id(e)))
			extra = "style = bold, ";
		out << "  n" << g.id(g.u(e)) << " -- " << " n" << g.id(g.v(e))
				<< " [ " << extra << "label=\"" << length[e] << "\" ]; " << endl;
	}
	out << "}" << endl;
}

/* find the optimal matching of every pair of clusters
 * maxSz is what the largest current cluster should be
 */
bool MatcherPacker::fullMergeClusters(const DataViewer *D,
		vector<Cluster>& clusters, unsigned maxSz, DCache& dcache) const
{
	unsigned C = clusters.size();
	if (C == 1)
		return false;

	//create full graph with distances; enforce an even number of nodes if
	//necessary by adding a dummy node
	unsigned N = C;
	if (C % 2)
		N++;

	FullGraph G(N);
	FullGraph::EdgeMap<double> weights(G);
	for (unsigned i = 0; i < C; i++)
	{
		for (unsigned j = 0; j < i; j++)
		{
			float dist = clusterDistance(D, clusters[i], clusters[j], dcache);
			weights[G.arc(G(i), G(j))] = -dist; //because the algo maximizes
		}
	}

	//init dummy node
	for (unsigned i = C; i < N; i++)
	{
		for (unsigned j = 0; j < C; j++)
		{
			if (clusters[j].size() < maxSz)
			{
				//force smaller clusters to merge
				weights[G.arc(G(i), G(j))] = -10000;
				weights[G.arc(G(j), G(i))] = -10000;
			}
			else
			{
				weights[G.arc(G(i), G(j))] = 0;
				weights[G.arc(G(j), G(i))] = 0;
			}
		}
	}

	MaxWeightedPerfectMatching<FullGraph, FullGraph::EdgeMap<double> > matcher(
			G, weights);
	matcher.run();

	//merge clusters that were matched
	vector<Cluster> newclusters;
	newclusters.reserve(N / 2 + 1);
	vector<bool> packed(N, false);
	bool didmerge = false;
	for (unsigned i = 0; i < C; i++)
	{
		if (!packed[i])
		{
			unsigned j = G.index(matcher.mate(G(i)));
			if (j == C)
			{
				newclusters.push_back(Cluster());
				newclusters.back().moveInto(clusters[i]);
			}
			else
			{
				if (clusters[i].size() + clusters[j].size() <= packSize) //merge
				{
					newclusters.push_back(Cluster());
					newclusters.back().mergeInto(clusters[i], clusters[j]);
					didmerge = true;
					packed[i] = true;
					packed[j] = true;
				}
				else
				{
					//didn't merge, keep clusters as is
					newclusters.push_back(Cluster());
					newclusters.back().moveInto(clusters[i]);
					newclusters.push_back(Cluster());
					newclusters.back().moveInto(clusters[j]);
					packed[i] = true;
					packed[j] = true;
				}
			}
		}
	}

	swap(clusters, newclusters);
	return didmerge;
}


//==================================================================================================
/**
  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/shuffle.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include "reference.hpp"

using namespace boost::simd;

STF_CASE_TPL( "binary shuffle of cardinal 2", STF_NUMERIC_TYPES)
{
  bs::pack<T,2> a{ Valmax<T>(), T(42) };
  bs::pack<T,2> b{ T(69), Valmin<T>() };

  STF_ALL_EQUAL( (shuffle<-1,-1>(a,b)), (binary_ref<-1,-1>(a,b)) );
  STF_ALL_EQUAL( (shuffle<-1, 0>(a,b)), (binary_ref<-1, 0>(a,b)) );
  STF_ALL_EQUAL( (shuffle<-1, 1>(a,b)), (binary_ref<-1, 1>(a,b)) );
  STF_ALL_EQUAL( (shuffle<-1, 2>(a,b)), (binary_ref<-1, 2>(a,b)) );
  STF_ALL_EQUAL( (shuffle<-1, 3>(a,b)), (binary_ref<-1, 3>(a,b)) );

  STF_ALL_EQUAL( (shuffle< 0,-1>(a,b)), (binary_ref< 0,-1>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 0, 0>(a,b)), (binary_ref< 0, 0>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 0, 1>(a,b)), (binary_ref< 0, 1>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 0, 2>(a,b)), (binary_ref< 0, 2>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 0, 3>(a,b)), (binary_ref< 0, 3>(a,b)) );

  STF_ALL_EQUAL( (shuffle< 1,-1>(a,b)), (binary_ref< 1,-1>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 1, 0>(a,b)), (binary_ref< 1, 0>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 1, 1>(a,b)), (binary_ref< 1, 1>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 1, 2>(a,b)), (binary_ref< 1, 2>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 1, 3>(a,b)), (binary_ref< 1, 3>(a,b)) );

  STF_ALL_EQUAL( (shuffle< 2,-1>(a,b)), (binary_ref< 2,-1>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 2, 0>(a,b)), (binary_ref< 2, 0>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 2, 1>(a,b)), (binary_ref< 2, 1>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 2, 2>(a,b)), (binary_ref< 2, 2>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 2, 3>(a,b)), (binary_ref< 2, 3>(a,b)) );

  STF_ALL_EQUAL( (shuffle< 3,-1>(a,b)), (binary_ref< 3,-1>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 3, 0>(a,b)), (binary_ref< 3, 0>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 3, 1>(a,b)), (binary_ref< 3, 1>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 3, 2>(a,b)), (binary_ref< 3, 2>(a,b)) );
  STF_ALL_EQUAL( (shuffle< 3, 3>(a,b)), (binary_ref< 3, 3>(a,b)) );

}