#pragma once

#include <cstdint>
#include <cmath>
#include <iostream>
#include <random>
#include <chrono>
#include <ctime>
#include <vector>
#include <string>
#include <queue>
#include <algorithm>
#include <functional>

// disable Eigen's own parallelization
//#define EIGEN_DONT_PARALLELIZE
#include <Eigen/Eigen>

#include "omp.h"

namespace adrianyu {

class KahanSumationEigen
{
    /*
        Kahan Summation Algorithm, see ref.:
        https://en.wikipedia.org/wiki/Kahan_summation_algorithm
    */
public:
    /*
        if the first constructor is used, then the funtion init()
        must be called before using other functions.
    */
    explicit KahanSumationEigen(void) {}
    explicit KahanSumationEigen(const int dim) {
        init(dim);
    }
    void init(const int dim) {
        sum = Eigen::VectorXd::Zero(dim);
        c = Eigen::VectorXd::Zero(dim);
    }
    const KahanSumationEigen& add(const Eigen::VectorXd & val) {
        Eigen::VectorXd y(val - c);
        Eigen::VectorXd t(sum + y);
        c = (t - sum) - y;
        sum = t;
        return *this;
    }

    Eigen::VectorXd get(void) const {
        return sum;
    }

    KahanSumationEigen & operator+=(const KahanSumationEigen & rhs) {
        this->add(rhs.sum);
        this->add(rhs.c);
        return *this;
    }
    friend KahanSumationEigen operator+(KahanSumationEigen lhs, const KahanSumationEigen &rhs) {
        lhs += rhs;
        return lhs;
    }

protected:
    Eigen::VectorXd sum;
    Eigen::VectorXd c;
};

template <class DataType>
class kmeans
{
    /*
        K-Means using kmeans++ initialization algorithm and Elkan training algorithm.
        ref.:
            Arthur, D., & Vassilvitskii, S. (2007). k-means++: the advantages of careful seeding. Symposium on Discrete Algorithms.
            Elkan, C. (2003). Using the Triangle Inequality to Accelerate k-Means. International Conference on Machine Learning.
    */
public:
    /*
        init kmeans using data and pre-specified k
        params:
            data: training data used to init the centers, must be column-major stored
            k: specify the number of centers
            return: fail on negative, otherwise return the true number of centers initialized.
    */
    int init(const Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic> &data, int k) {
        std::cerr << "in kmeans init..." << std::endl;
        this->k = kmeanspp_init(data, k, centers);
        max_iter = 2000;

        half_inter_dist = Eigen::MatrixXd::Zero(this->k, this->k);
        #pragma omp parallel for
        for (int i = 0; i < this->k - 1; ++i) {
            for (int j = i + 1; j < this->k; ++j) {
                half_inter_dist(i, j) = (centers.col(i) - centers.col(j)).norm() / 2;
                half_inter_dist(j, i) = half_inter_dist(i, j);
            }
        }
        return this->k;
    }

    /*
        train kmeans using data by Elkan algorithm
        ref: Using the Triangle Inequality to Accelerate k-Means by Charles Elkan
        params:
            data: training data used to init the centers, must be column-major stored
            k: specify the number of centers
            return: fail on negative, otherwise return the true number of centers initialized.
    */
    int train(const Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic> &data) {
        // init all params
        const int data_num = data.cols();
        const int data_dim = data.rows();
        // lower bound
        //Eigen::MatrixXd l_b = -1 * Eigen::MatrixXd::Ones(k, data_num);
        Eigen::MatrixXd l_b = Eigen::MatrixXd::Zero(k, data_num);
        // upper bound
        Eigen::VectorXd u_b = Eigen::VectorXd::Zero(data_num);
        // class assignment
        Eigen::VectorXi c_x = Eigen::VectorXi::Zero(data_num);
        // do the real assignment
        #pragma omp parallel for
        for (int i = 0; i < data_num; ++i) {
            //l_b.col(i).noalias() = -1 * Eigen::VectorXd::Ones(k);
            l_b(0, i) = (data.col(i).cast<double>() - centers.col(0)).norm();
            int class_s = 0;
            double closest_dist = l_b(0, i);
            for (int j = 1; j < k; ++j) {
                if (half_inter_dist(j, class_s) < closest_dist) {
                    l_b(j, i) = (data.col(i).cast<double>() - centers.col(j)).norm();
                    if (l_b(j, i) < closest_dist) {
                        closest_dist = l_b(j, i);
                        class_s = j;
                    }
                }
            }
            c_x[i] = class_s;
            u_b[i] = closest_dist;
        }

        // the algorithm, repeat until convergence
        size_t iter = 0;
        Eigen::VectorXd s_c = Eigen::VectorXd::Zero(k);
        //Eigen::MatrixXd inter_dist = Eigen::MatrixXd::Zero(k, k);
        std::vector<bool> r_x(data_num, false);
        int done = 0;
        std::cerr << "begin to iterate" << std::endl;
        std::clock_t done_clk = std::clock();
        while (iter < max_iter && 0 == done) {
            std::clock_t beg_clk = std::clock();
            std::cerr << "curr iter: " << iter << std::endl;
            done = 1;

            // step 4 update centers in parallel
            std::vector< std::vector<KahanSumationEigen> > m_c_th;
            std::vector< std::vector<size_t> > c_n_th;
            int nthreads;
            #pragma omp parallel
            {
                #pragma omp single
                {
                    nthreads = omp_get_num_threads();
                    m_c_th.resize(nthreads, std::vector<KahanSumationEigen>(k, KahanSumationEigen(data_dim)));
                    c_n_th.resize(nthreads, std::vector<size_t>(k, 0));
                }
                // the data number may not be divisible by nthreads
                size_t part_size = data_num / nthreads;
                if (data_num % nthreads) {
                    part_size++;
                }
                const int ithread = omp_get_thread_num();
                const size_t part_start = ithread * part_size;
                for (size_t i = part_start; i < part_start + part_size; ++i) {
                    if (i < data_num) {
                        size_t c = c_x[i];
                        c_n_th[ithread][c]++;
                        m_c_th[ithread][c].add(data.col(i).cast<double>());
                    }
                }
            }
            // do the reduction
            std::vector<KahanSumationEigen> m_c(k, KahanSumationEigen(data_dim));
            std::vector<size_t> c_n(k, 0);
            for (int th_s = 0; th_s < nthreads; ++th_s) {
                for (int i = 0; i < k; ++i) {
                    c_n[i] += c_n_th[th_s][i];
                    m_c[i] += m_c_th[th_s][i];
                }
            }

            // normalize, and get the real centers
            // step 5
            Eigen::MatrixXd c_m_c = Eigen::MatrixXd::Zero(k, k);
            #pragma omp parallel for
            for (int i = 0; i < k; ++i) {
                for (int j = 0; j < k; ++j) {
                    c_m_c(i, j) = (centers.col(i) - m_c[j].get() / c_n[j]).norm();
                }
            }
            for (int c = 0; c < k; ++c) {
                #pragma omp parallel for
                for (int x = 0; x < data_num; ++x) {
                    l_b(c, x) = std::max(l_b(c, x) - c_m_c(c, c), 0.0);
                }
            }
            // step 6
            #pragma omp parallel for
            for (int x = 0; x < data_num; ++x) {
                u_b[x] += c_m_c(c_x[x], c_x[x]);
                r_x[x] = true;
            }
            // step 7
            #pragma omp parallel for
            for (int i = 0; i < k; ++i) {
                centers.col(i).noalias() = m_c[i].get() / c_n[i];
            }

            // step 1. update min half inter class distances
            #pragma omp parallel for
            for (int i = 0; i < k; ++i) {
                Eigen::VectorXd dists = Eigen::VectorXd::Zero(k - 1);
                for (int j = 0, m = 0; j < k; ++j) {
                    if (j != i) {
                        dists[m] = (centers.col(i) - centers.col(j)).norm() / 2;
                        half_inter_dist(j, i) = dists[m];
                        m++;
                    }
                }
                s_c[i] = dists.minCoeff();
            }
            //std::cout << "half_inter_dist: " << half_inter_dist << std::endl;
            //std::cout << "s_c: " << s_c << std::endl;
            // step 2 & 3
            std::vector<bool> need_step3(data_num, true);
            // step 2
            #pragma omp parallel for
            for (int x = 0; x < data_num; ++x) {
                if (u_b[x] <= s_c[c_x[x]]) {
                    need_step3[x] = false;
                }
            }
            for (int c = 0; c < k; ++c) {
                #pragma omp parallel for
                for (int x = 0; x < data_num; ++x) {
                    //std::cerr << x << std::endl;
                    if (need_step3[x] && u_b[x] > half_inter_dist(c_x[x], c) \
                        && c != c_x[x] && u_b[x] > l_b(c, x)) {
                        double dist_org;
                        double dist_new;
                        // step 3a
                        if (r_x[x]) {
                            dist_org = (data.col(x).cast<double>() - centers.col(c_x[x])).norm();
                            u_b[x] = dist_org;
                            r_x[x] = false;
                        }
                        else {
                            dist_org = u_b[x];
                        }
                        // step 3b
                        if (dist_org > l_b(c, x) || dist_org > half_inter_dist(c_x[x], c)) {
                            dist_new = (data.col(x).cast<double>() - centers.col(c)).norm();
                            l_b(c, x) = dist_new;
                            if (dist_new < dist_org) {
                                c_x[x] = c;
                                u_b[x] = dist_new;
                                #pragma omp atomic
                                done *= 0; // for gcc, done = 0 should compile too.
                                continue;
                            }
                        }
                    }
                }
            }
            iter++;
            done_clk = std::clock();
            std::cerr << "time: "
                << static_cast<double>(done_clk - beg_clk) * 1000.0 / CLOCKS_PER_SEC
                << std::endl;
        }
        return 0;
    }

    /*
        Given one sample, return the class index starting from 0
        params:
            @sample: data sample vector, column-major
            @closest_dist: the distance between sample and its belonging class
            @return: class index (starting from 0)
    */
    int get_class(const Eigen::Matrix<DataType, Eigen::Dynamic, 1> &sample,
        double &closest_dist) {
        Eigen::VectorXd dists(k);
        dists[0] = (sample.cast<double>() - centers.col(0)).norm();
        int class_s = 0;
        closest_dist = dists[0];
        for (int j = 1; j < k; ++j) {
            if (half_inter_dist(j, class_s) < closest_dist) {
                dists[j] = (sample.cast<double>() - centers.col(j)).norm();
                if (dists[j] < closest_dist) {
                    closest_dist = dists[j];
                    class_s = j;
                }
            }
        }
        return class_s;
    }
    int get_class(const Eigen::Matrix<DataType, Eigen::Dynamic, 1> &sample) {
        double cdist;
        return get_class(sample, cdist);
    }

    /*
        Given some samples, return the class indices
        params:
            @data: data sample vectors, column-major
            @return: class indices (starting from 0)
    */
    void get_class_indices(const Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic> &data, \
        std::vector<size_t> &c_indices) {
        c_indices.clear();
        const int data_num = data.cols();
        const int data_dim = data.rows();
        c_indices.resize(data_num);
        #pragma omp parallel for
        for (int i = 0; i < data_num; ++i) {
            c_indices[i] = get_class(data.col(i));
        }
    }

    /*
        return the centers
    */
    Eigen::MatrixXd get(void) {
        return centers;
    }

protected:
    static const double EPSILON;
    size_t max_iter;
    int k; // center number
    Eigen::MatrixXd centers;
    Eigen::MatrixXd half_inter_dist;

    /*
        Given a data point, compute the distances from all centers
        params:
            @sample: data sample vector, column-major
            @return: None
    */
    void get_dists(const Eigen::Matrix<DataType, Eigen::Dynamic, 1> & sample,
        Eigen::VectorXd &dists) {
        dists = -1 * Eigen::VectorXd::Ones(k);
        for (int i = 0; i < k; ++i) {
            // Euclidean distance
            dists[i] = (sample.cast<double>() - centers.col(i)).norm();
        }
    }

    /*
        Given the data and the number of centers k,
        initialize all the k centers using kmeans++ algorithm.
        params:
            data: training data used to init the centers, must be column-major stored
            k: specify the number of centers
            centers: the initialized centers, also column-majored. Need not to be pre-allocated.
            return: fail on negative, otherwise return the true number of centers initialized.
    */
    static int kmeanspp_init(const Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic> &data,
        const int k, Eigen::MatrixXd& centers) {
        if (k < 0) {
            std::cerr << "kmeans init failed: number of centers must be greater than 0, ["
                << k << "] given" << std::endl;
            return -1;
        }
        const int data_num = data.cols();
        const int data_dim = data.rows();
        centers.resize(data_dim, k);

        std::uniform_real_distribution<double> rnddist(0.0, 1.0);
        unsigned seed = static_cast<unsigned>(\
            std::chrono::system_clock::now().time_since_epoch().count());
        std::mt19937_64 rndgen(seed);

        // randomly choose a data point as the first center
        int idx = static_cast<int>(std::floor(\
                static_cast<double>(data_num) * rnddist(rndgen)));
        if (idx < 0) {
            idx = 0;
        }
        if (idx >= data_num) {
            idx = data_num - 1;
        }
        centers.col(0).noalias() = data.col(idx).cast<double>();

        const double rnd_number = min(k * 1000, data_num);
        const double prop = rnd_number / static_cast<double>(data_num);

        // choose the rest of the centers based on their distances from the closest center.
        Eigen::VectorXd distances(data_num);
        Eigen::VectorXd maxdists(data_num);
        int real_k = k;
        // compare to the real training procedure, this is negligible?
        // so no sub-sampling is needed.
        for (int i = 1; i < k; ++i) {

            /*seed = static_cast<unsigned>(\
                std::chrono::system_clock::now().time_since_epoch().count());
            rndgen.seed(seed);
            std::vector<bool> not_compute(data_num, false);
            for (size_t j = 0; j < not_compute.size(); ++j) {
                not_compute[j] = (rnddist(rndgen) > prop);
            }*/

            distances = Eigen::VectorXd::Zero(data_num);
            maxdists = Eigen::VectorXd::Zero(data_num);
            #pragma omp parallel for
            for (int j = 0; j < data_num; ++j) {
                // sub-sample
                //if (not_compute[j]) {
                //    continue;
                //}

                Eigen::VectorXd dists_(i);
                for (int m = 0; m < i; ++m) {
                    dists_[m] = (data.col(j).cast<double>() - centers.col(m)).squaredNorm();
                    // using l1-norm to replace l2-norm
                    //dists_[m] = (data.col(j).cast<double>() - centers.col(m)).cwiseAbs().sum();
                }
                //int x, y;
                //maxdists[j] = dists_.maxCoeff(&x, &y);
                maxdists[j] = dists_.maxCoeff();
                distances[j] = dists_.minCoeff();
            }
            // just in case that there are less distinct data samples than centers.
            if (maxdists.maxCoeff() < EPSILON) {
                real_k = i;
                break;
            }
            // get the next center
            // choose the point using a weight probability distribution
            // where a point x is chosen with probability proportional to squared distance.
            double dist_sum = distances.sum();
            do {
                double posf = dist_sum * rnddist(rndgen);
                double cpos = 0;
                idx = data_num - 1;
                for (int j = 0; j < distances.size(); ++j) {
                    cpos += distances[j];
                    if (cpos > posf) {
                        idx = j;
                        break;
                    }
                }
            } while (distances[idx] < EPSILON);
            centers.col(i).noalias() = data.col(idx).cast<double>();
        }
        return real_k;
    }
};

template <class DataType> const double kmeans<DataType>::EPSILON = 1e-10;

template <class DataType>
class hkmeans
{
    /*
        Hierarchical K-Means
    */
public:

    explicit hkmeans()
    {
        hk_trees = NULL;
    }

    ~hkmeans() {
        destroy();
    }

    /*
        train hierarchical kmeans
        params:
            data: training data used to init the centers, must be column-major stored
            k: specify the number of centers
            height: the maximum height of the tree
            return: fail on false, otherwise return true
    */
    bool train(const Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic> &data, \
        const int k, const int height) {
        if (height < 1) {
            return false;
        }

        this->k = k;
        this->height = height;
        // first destroy previous model
        destroy();

        // allocate first node
        hk_trees = new KMeansNode;
        // do the real training
        return train_tree(data, hk_trees, height);
    }

protected:
    int k;
    int height;
    // 2^15 should be of enough precision for most applications: 1GB memory
    // can store more than 4 million numbers.
    //Eigen::Matrix<int16_t, Eigen::Dynamic, Eigen::Dynamic> _train_data;

    struct KMeansNode
    {
        /*
            tree node struct
        */
        kmeans<DataType> node_km;
        std::vector<KMeansNode *> children_nodes;
    };
    KMeansNode *hk_trees;

    /*
        train a hierarchical kmeans tree recursively
        params:
            data: training data used to init the centers, must be column-major stored
            root: specify the current root of the tree
            height: the height of the current root node
            return: fail on false, otherwise return true
    */
    bool train_tree(const Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic> &data, \
        KMeansNode *root, const int height) {
        const int data_num = data.cols();
        const int data_dim = data.rows();
        // init and train
        int real_k = root->node_km.init(data, k);
        //std::cout << std::endl << data << std::endl << std::endl;
        root->node_km.train(data);
        //std::cout << std::endl << root->node_km.centers << std::endl;
        if (height > 1) {
            root->children_nodes.resize(real_k, NULL);
            std::vector<size_t> indices;
            root->node_km.get_class_indices(data, indices);
            for (int i = 0; i < real_k; ++i) {
                root->children_nodes[i] = new KMeansNode;

                // TODO: use the following, and not copy to sub-class data data_p
                //std::vector<bool> idx_f(indices.size());
                //auto eq_i = std::bind(std::equal_to<size_t>(), std::placeholders::_1, i);
                //std::transform(indices.begin(), indices.end(), idx_f.begin(), eq_i);

                // get data proportion
                size_t c_count = std::count(indices.begin(), indices.end(), i);
                Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic> data_p = \
                    Eigen::Matrix<DataType, Eigen::Dynamic, Eigen::Dynamic>::Zero(data_dim, c_count);
                for (int j = 0, c = 0; j < data_num && c < c_count; ++j) {
                    if (i == indices[j]) {
                        data_p.col(c).noalias() = data.col(j);
                        ++c;
                    }
                }
                // do the training recursively.
                bool ret = train_tree(data_p, root->children_nodes[i], height - 1);
            }
        }
        else {
            root->children_nodes.clear();
        }
        return true;
    }

    /*
        destroy the hierarchical kmeans tree recursively
    */
    void destroy(void) {
        std::queue<KMeansNode *> nodes;
        nodes.push(hk_trees);
        do {
            KMeansNode *anode = nodes.front();
            nodes.pop();
            if (anode) {
                for (size_t i = 0; i < anode->children_nodes.size(); ++i) {
                    nodes.push(anode->children_nodes[i]);
                }
                anode->children_nodes.clear();
                delete anode;
            }
        } while (!nodes.empty());
    }
};

}


#include <Functions/FunctionFactory.h>
#include <Functions/geometryConverters.h>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#include <common/logger_useful.h>

#include <Columns/ColumnArray.h>
#include <Columns/ColumnTuple.h>
#include <Columns/ColumnsNumber.h>
#include <DataTypes/DataTypeArray.h>
#include <DataTypes/DataTypeTuple.h>
#include <DataTypes/DataTypesNumber.h>

#include <memory>
#include <string>

namespace DB
{
namespace ErrorCodes
{
    extern const int ILLEGAL_TYPE_OF_ARGUMENT;
}

template <typename Point>
class FunctionPolygonPerimeter : public IFunction
{
public:
    static const char * name;

    explicit FunctionPolygonPerimeter() = default;

    static FunctionPtr create(ContextConstPtr)
    {
        return std::make_shared<FunctionPolygonPerimeter>();
    }

    String getName() const override
    {
        return name;
    }

    bool isVariadic() const override
    {
        return false;
    }

    size_t getNumberOfArguments() const override
    {
        return 1;
    }

    DataTypePtr getReturnTypeImpl(const DataTypes &) const override
    {
        return std::make_shared<DataTypeFloat64>();
    }

    ColumnPtr executeImpl(const ColumnsWithTypeAndName & arguments, const DataTypePtr & /*result_type*/, size_t input_rows_count) const override
    {
        auto res_column = ColumnFloat64::create();
        auto & res_data = res_column->getData();
        res_data.reserve(input_rows_count);

        callOnGeometryDataType<Point>(arguments[0].type, [&] (const auto & type)
        {
            using TypeConverter = std::decay_t<decltype(type)>;
            using Converter = typename TypeConverter::Type;

            if constexpr (std::is_same_v<ColumnToPointsConverter<Point>, Converter>)
                throw Exception(fmt::format("The argument of function {} must not be Point", getName()), ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT);
            else
            {
                auto geometries = Converter::convert(arguments[0].column->convertToFullColumnIfConst());

                for (size_t i = 0; i < input_rows_count; i++)
                    res_data.emplace_back(boost::geometry::perimeter(geometries[i]));
            }
        }
        );

        return res_column;
    }

    bool useDefaultImplementationForConstants() const override
    {
        return true;
    }
};

template <>
const char * FunctionPolygonPerimeter<CartesianPoint>::name = "polygonPerimeterCartesian";

template <>
const char * FunctionPolygonPerimeter<SphericalPoint>::name = "polygonPerimeterSpherical";

void registerFunctionPolygonPerimeter(FunctionFactory & factory)
{
    factory.registerFunction<FunctionPolygonPerimeter<CartesianPoint>>();
    factory.registerFunction<FunctionPolygonPerimeter<SphericalPoint>>();
}

}


//
//  Slip_Weakening.hpp
//  hybrid_fem_bie
//
//  Created by Max on 2/8/18.
//
//

#ifndef Slip_Weakening_hpp
#define Slip_Weakening_hpp

#include <stdio.h>
#include <iostream>
#include <Eigen/Eigen>
#include "maplocal.hpp"

void Slip_Weakening(VectorXd &M_global_vec, ArrayXi &top_surf_index, ArrayXi &bot_surf_index, VectorXd &fe_global, double dt, double dx,double dy, int nx, ArrayXd &delt_v_n, ArrayXd &delt_u_n, ArrayXd &T_0, ArrayXd &tau_s, double mu_s, double mu_d, double Dc, int Ndofn, double M, VectorXd &F_fault, ArrayXd &T_c);
#endif /* Slip_Weakening_hpp */


//
// Add --log_level=message to see the messages!
//
#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>

#include <boost/numeric/ublas/assignment.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>

#include "core/types.h"
#include "core/random.h"
#include "core/utils.h"

#include "timer.h"
#include "test_utils.h"

using namespace std;
using namespace boost;
using namespace boost::unit_test;
using namespace boost::numeric;
using namespace yann;
using namespace yann::test;

struct TypesTestFixture
{
  TypesTestFixture()
  {
  }
  ~TypesTestFixture()
  {
  }
};

BOOST_FIXTURE_TEST_SUITE(TypesTest, TypesTestFixture);

////////////////////////////////////////////////////////////////////////////////////////////////
//
// perf functions tests
//
BOOST_AUTO_TEST_CASE(Perf_Test, * disabled())
{
  const size_t size = 1000;
  const size_t vector_size = 100000;
  const size_t data_size = size * size;

  // Eigen::Matrix * Vector
  {
      Timer timer("Eigen::Matrix*Vector: total");
      Matrix aa(size, vector_size);
      Vector bb(vector_size), cc(size);
      {
        Timer timer("Eigen::Matrix*Vector: random generation");
        unique_ptr<RandomGenerator> gen = RandomGenerator::normal_distribution(0, 1);
        gen->generate(aa);
        gen->generate(bb);
      }
      // ensure we don't do allocations in eigen
      {
        BlockAllocations block;
        {
          Timer timer("Eigen::Matrix*Vector: cc.noalias() = aa.lazyProduct(bb)");
          cc.noalias() = aa.lazyProduct(bb.transpose());
        }
      }
      {
        Timer timer("Eigen::Matrix*Vector: cc.noalias() = aa * bb;");
        cc.noalias() = aa * bb.transpose();
      }
      // ensure we don't do allocations in eigen
      {
        BlockAllocations block;
        {
          Timer timer("Eigen::Matrix*Vector: cc.noalias() = MatrixFunctions<RefConstMatrix>::product(aa, bb)");
          cc.noalias() = MatrixFunctions<RefConstMatrix>::product(aa, bb.transpose());
        }
      }
  }
  BOOST_TEST_MESSAGE("\n");

  // Eigen::Matrix * Sparse Vector
  {
      Timer timer("Eigen::Matrix*SparseVector: total");
      Matrix aa(vector_size, size);
      SparseMatrix bb(1, vector_size);
      Vector cc(size);
      {
        Timer timer("Eigen::Matrix*SparseVector: random generation");
        unique_ptr<RandomGenerator> gen = RandomGenerator::normal_distribution(0, 1);
        gen->generate(aa);
        bb.insert(1, 10) = 1.0;
      }
      // ensure we don't do allocations in eigen
      {
        BlockAllocations block;
        {
          Timer timer("Eigen::Matrix*SparseVector: cc.noalias() = bb * aa");
          cc.noalias() = bb * aa;
        }
      }
  }
  BOOST_TEST_MESSAGE("\n");

  // Eigen::Matrix * Matrix
  {
      Timer timer("Eigen::Matrix*Matrix: total");
      Matrix aa(size, size), bb(size, size), cc(size, size);
      {
        Timer timer("Eigen::Matrix*Matrix: random generation");
        unique_ptr<RandomGenerator> gen = RandomGenerator::normal_distribution(0, 1);
        gen->generate(aa);
        gen->generate(bb);
      }
      // ensure we don't do allocations in eigen
      {
        BlockAllocations block;
        {
          Timer timer("Eigen::Matrix*Matrix: cc.noalias() = aa.lazyProduct(bb)");
          cc.noalias() = aa.lazyProduct(bb);
        }
      }
      {
        Timer timer("Eigen::Matrix*Matrix: cc.noalias() = aa * bb");
        cc.noalias() = aa * bb;
      }
      // ensure we don't do allocations in eigen
      {
        BlockAllocations block;
        {
          Timer timer("Eigen::Matrix*Matrix: cc.noalias() = MatrixFunctions<RefConstSparseMatrix>::product(aa, bb)");
          cc.noalias() = MatrixFunctions<RefConstSparseMatrix>::product(aa, bb);
        }
      }
  }
  BOOST_TEST_MESSAGE("\n");

  // UBLAS::matrix
  {
      Timer timer("UBLAS Matrix*Matrix: total");
      ublas::matrix<Value> aa(size, size), bb(size, size), cc(size, size);
      {
        Timer timer("UBLAS Matrix*Matrix: random generation");
        unique_ptr<RandomGenerator> gen = RandomGenerator::normal_distribution(0, 1);
        gen->generate(aa.data().begin(), aa.data().end());
        gen->generate(bb.data().begin(), bb.data().end());
      }
      {
        Timer timer("UBLAS Matrix*Matrix: noalias(cc) = ublas::prod(aa, bb)");
        noalias(cc) = ublas::prod(aa, bb);
      }
      {
        Timer timer("UBLAS Matrix*Matrix: ublas::axpy_prod(aa, bb, cc, true)");
        ublas::axpy_prod(aa, bb, cc, true);
      }
  }
  BOOST_TEST_MESSAGE("\n");

  // std::vector
  {
    Timer timer("Value[] Matrix*Matrix: total");
    auto aa = make_unique<Value[]>(data_size);
    auto bb = make_unique<Value[]>(data_size);
    auto cc = make_unique<Value[]>(data_size);

    {
      Timer timer("Value[] Matrix*Matrix: random generation");
      unique_ptr<RandomGenerator> gen = RandomGenerator::normal_distribution(0, 1);
      gen->generate(aa.get(), aa.get() + data_size);
      gen->generate(bb.get(), bb.get() + data_size);
    }
    {
      Timer timer("Value[] Matrix*Matrix: ");
      auto aa_ptr = aa.get();
      auto bb_ptr = bb.get();
      auto cc_ptr = cc.get();
      memset(cc_ptr, 0, data_size);

      for(size_t ii = 0; ii < size; ++ii) {
        for(size_t jj = 0; jj < size; ++jj) {
          for(size_t kk = 0; kk < size; ++kk) {
            cc_ptr[ii*size + jj] += aa_ptr[ii*size + kk] * bb_ptr[kk*size + jj];
          }
        }
      }
    }
    {
      Timer timer("Value[] Matrix*Matrix 2: ");
      auto aa_ptr = aa.get();
      auto bb_ptr = bb.get();
      auto cc_ptr = cc.get();
      memset(cc_ptr, 0, data_size);

      for(size_t ii = 0; ii < size; ++ii) {
        for(size_t kk = 0; kk < size; ++kk) {
          for(size_t jj = 0; jj < size; ++jj) {
            cc_ptr[ii*size + jj] += aa_ptr[ii*size + kk] * bb_ptr[kk*size + jj];
          }
        }
      }
    }
  }
  BOOST_TEST_MESSAGE("\n");
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <stdexcept>
#include <vector>

#include <boost/serialization/split_member.hpp>
#include <boost/serialization/vector.hpp>

#include "../Variable.hpp"

namespace mist {
namespace it {

class DistributionOutOfRange : public std::out_of_range
{
public:
  DistributionOutOfRange(std::string const& method, int pos, int size)
    : out_of_range("Distribution::" + method + " : Position " +
                   std::to_string(pos) + " out of range [0," +
                   std::to_string(size))
  {}
};

using DistributionData = double;

/** Joint probability array for N variables
 */
class Distribution
{
private:
  std::vector<DistributionData> data;
  std::vector<int> factors;
  std::size_t size;
  std::size_t nvar;

public:
  using Data = DistributionData;
  using value_type = Data;

  Distribution()
    : data(0)
    , factors(0)
    , size(0)
    , nvar(0)
  {};

  /** Construct directly from dimension strides
   */
  template<class Container>
  Distribution(Container const& strides)
    : Distribution()
  {
    // Dynamically resize factors
    std::size_t next_nvar = strides.size();
    if (this->nvar < next_nvar) {
      this->factors.resize(next_nvar);
    }
    this->nvar = next_nvar;

    // Dynamically resize data
    std::size_t next_size = 1;
    for (std::size_t ii = 0; ii < this->nvar; ii++) {
      this->factors[ii] = next_size;
      next_size *= strides[ii];
    }
    if (this->size < next_size) {
      this->data.resize(next_size);
    }
    this->size = next_size;
    this->data.assign(this->size, 0);
  };

  void
  initialize(Variable::tuple const& vars, Variable::indexes const& indexes)
  {
    // Dynamically resize factors
    std::size_t next_nvar = indexes.size();
    if (this->nvar < next_nvar) {
      this->factors.resize(next_nvar);
    }
    this->nvar = next_nvar;

    // Dynamically resize data
    std::size_t next_size = 1;
    for (size_t ii = 0; ii < nvar; ii++) {
      this->factors[ii] = next_size;
      next_size *= vars[indexes[ii]].bins();
    }
    if (this->size < next_size) {
      this->data.resize(next_size);
    }
    this->size = next_size;
    this->data.assign(this->size, 0); // SEGFAULT
  };

  /** Construct from a Variable tuple
   */
  Distribution(Variable::tuple const& vars)
    : Distribution()
  {
    Variable::indexes indexes(vars.size());
    int ii = 0;
    for (auto& index : indexes) {
      index = ii;
      ii++;
    }
    initialize(vars, indexes);
  }

  Distribution(Variable::tuple const& vars, Variable::indexes const& indexes)
    : Distribution()
  {
    initialize(vars, indexes);
  }

  //
  // accessors
  //
  template<typename Integer>
  Data const& operator[](Integer index) const
  {
    return this->data[index];
  }
  template<typename Integer>
  Data& operator[](Integer index)
  {
    return this->data[index];
  }

  template<typename Iter>
  Data const& operator()(Iter it, Iter end) const
  {
    int index = 0;
    for (int ii = 0; it != end; ++it, ii++)
      index += this->factors[ii] * (*it);
    return this->data[index];
  }
  template<typename Iter>
  Data& operator()(Iter it, Iter end)
  {
    int index = 0;
    for (int ii = 0; it != end; ++it, ii++)
      index += this->factors[ii] * (*it);
    return this->data[index];
  }
  template<typename Container>
  Data const& operator()(Container const& indexes) const
  {
    return this->operator()(indexes.begin(), indexes.end());
  }
  template<typename Container>
  Data& operator()(Container const& indexes)
  {
    return this->operator()(indexes.begin(), indexes.end());
  }

  //
  // at exception throwing accessors
  //
  template<typename Iter>
  Data const& at(Iter it, Iter end) const
  {
    int index = 0;
    for (int ii = 0; it != end; ++it, ii++)
      index += this->factors[ii] * (*it);
    if (index >= this->size)
      throw DistributionOutOfRange("at", index, this->size);
    return this->data[index];
  }
  template<typename Iter>
  Data& at(Iter it, Iter end)
  {
    int index = 0;
    for (int ii = 0; it != end; ++it, ii++)
      index += this->factors[ii] * (*it);
    if (index >= this->size)
      throw DistributionOutOfRange("at", index, this->size);
    return this->data[index];
  }
  template<typename Container>
  Data const& at(Container const& indexes) const
  {
    return this->at(indexes.begin(), indexes.end());
  }
  template<typename Container>
  Data& at(Container const& indexes)
  {
    return this->at(indexes.begin(), indexes.end());
  }

  //
  // Plain data templates
  //
  template<typename Integer>
  Data const& operator()(int d, Integer indices[]) const
  {
    int index = 0;
    for (int ii = 0; ii < d; ii++)
      index += this->factors[ii] * indices[ii];
    return this->data[index];
  };
  template<typename Integer>
  Data& operator()(int d, Integer indices[])
  {
    int index = 0;
    for (int ii = 0; ii < d; ii++)
      index += this->factors[ii] * indices[ii];
    return this->data[index];
  };

  //
  // Peformance critical indexing
  //
  Data const& operator()(int v0, int b0) const { return this->data[v0]; };
  Data& operator()(int v0, int b0) { return this->data[v0]; };
  Data const& operator()(int v0, int v1, int b0, int b1) const
  {
    return this->data[v0 + b0 * v1];
  };
  Data& operator()(int v0, int v1, int b0, int b1)
  {
    return this->data[v0 + b0 * v1];
  };
  Data const& operator()(int v0, int v1, int v2, int b0, int b1, int b2) const
  {
    return this->data[v0 + b0 * v1 + b0 * b1 * v2];
  };
  Data& operator()(int v0, int v1, int v2, int b0, int b1, int b2)
  {
    return this->data[v0 + b0 * v1 + b0 * b1 * v2];
  };
  Data const& operator()(int v0, int v1, int v2, int v3, int b0, int b1, int b2, int b3) const
  {
    return this->data[v0 + b0*v1 + b0*b1*v2 + b0*b1*b2*v3];
  };
  Data& operator()(int v0, int v1, int v2, int v3, int b0, int b1, int b2, int b3)
  {
    return this->data[v0 + b0*v1 + b0*b1*v2 + b0*b1*b2*v3];
  };

  bool operator==(Distribution const& other) const noexcept
  {
    return this->data == other.data && this->factors == other.factors;
  }

  bool empty() { return data.empty(); }

  using iterator = std::vector<Data>::iterator;
  using const_iterator = std::vector<Data>::const_iterator;
  const_iterator begin() const { return this->data.begin(); };
  const_iterator end() const { return this->data.end(); };
  iterator begin() { return this->data.begin(); };
  iterator end() { return this->data.end(); };

  /** Multiply each value in distribution by factor
   */
  void scale(double factor)
  {
    for (std::size_t i = 0; i < this->size; i++) {
      this->data[i] *= factor;
    }
  };

  /** Normalize distribution
   */
  void normalize()
  {
    std::size_t norm = 0;
    for (std::size_t i = 0; i < this->size; i++) {
      norm += this->data[i];
    }
    if (norm)
      this->scale(1.0 / norm);
  };

#if 0
  //
  // Serialization
  //
  friend class boost::serialization::access;

  template<class Archive>
  void load(Archive& ar, const unsigned int version)
  {
    // members
    ar& factors;

    // explicit data
    // since this class specializes std::vector need push/pop each element
    std::size_t size = 0;
    ar& size;
    this->data.resize(size);
    for (std::size_t i = 0; i < size; i++) {
      Data dat;
      ar& dat;
      this->data[i] = dat;
    }
  }
  template<class Archive>
  void save(Archive& ar, const unsigned int version) const
  {
    // members
    ar& factors;

    // explicit data
    // since this class specializes std::vector need push/pop each element
    auto size = this->size;
    ar& size;
    for (std::size_t i = 0; i < size; i++) {
      ar(&this->data[i]);
    }
  }
  BOOST_SERIALIZATION_SPLIT_MEMBER()
#endif
};

} // it
} // mist


/***
* Copyright 2017 Marc Stevens <marc@marc-stevens.nl>, Dan Shumow <danshu@microsoft.com>
* Distributed under the MIT Software License.
* See accompanying file LICENSE.txt or copy at
* https://opensource.org/licenses/MIT
***/

#include <iostream>
#include <iomanip>
#include <vector>

#include <boost/cstdint.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random.hpp>
#include <boost/progress.hpp>
#include <boost/timer.hpp>
#include <boost/array.hpp>

#include "ubc_check_test.h"
#include "test_simd.h"

using namespace std;
using boost::uint32_t;

// on 32 bit architectures, we need to allocate a smaller buffer for performance testing
#if defined(_M_IX86) || defined(__arm__)
#define VECTOR_COUNT (1 << 19)
#else
#define VECTOR_COUNT (1 << 20)
#endif

inline uint32_t rotate_left(const uint32_t x, const unsigned n)
{
	return (x << n) | (x >> (32 - n));
}

template<typename RNG, typename SIMD_WORD>
inline
void gen_W(RNG& rng, SIMD_WORD* W)
{
	const size_t SIMD_VECSIZE = sizeof(SIMD_WORD) / sizeof(uint32_t);

	for (unsigned i = 0; i < 16; ++i)
	{
		union {
			SIMD_WORD v;
			uint32_t w[SIMD_VECSIZE];
		} tmp;
		for (unsigned j = 0; j < SIMD_VECSIZE; ++j)
			tmp.w[j] = rng();
		W[i] = tmp.v;
	}

	for (unsigned i = 16; i < 80; ++i)
	{
		for (size_t j = 0; j < SIMD_VECSIZE; j++)
			((uint32_t*)&W[i])[j] = rotate_left((((uint32_t*)&W[i - 3])[j] ^ ((uint32_t*)&W[i - 8])[j] ^ ((uint32_t*)&W[i - 14])[j] ^ ((uint32_t*)&W[i - 16])[j]), 1);
	}
}

extern bool run_correctness_checks;
extern bool run_perf_tests;

template<typename SIMD_WORD, void(*ubc_check_simd)(const SIMD_WORD*, SIMD_WORD*)>
inline
int test_ubc_check_simd(const char* simd_name_str)
{
	boost::random::random_device seeder;
	boost::random::mt19937 rng(seeder);

	const size_t SIMD_VECSIZE = sizeof(SIMD_WORD) / sizeof(uint32_t);

	SIMD_WORD W[80];
	SIMD_WORD dvmask[DVMASKSIZE];
	uint32_t dvmask_test[DVMASKSIZE];
	union {
		SIMD_WORD v;
		uint32_t w[SIMD_VECSIZE];
	} tmp;

	if (run_correctness_checks)
	{
		cout << "Verifying ubc_check" << simd_name_str << "() against ubc_check_verify():" << endl;
		boost::progress_display pd(1 << 24);
		for (unsigned ll = 0; ll < (1 << 24); ++ll, ++pd)
		{
			gen_W(rng, W);

			for (unsigned i = 0; i < DVMASKSIZE; ++i)
			{
				for (size_t j = 0; j < SIMD_VECSIZE; j++)
					((uint32_t*)&dvmask[i])[j] = 0;
			}

			ubc_check_simd(W, dvmask);

			for (unsigned w = 0; w < SIMD_VECSIZE; ++w)
			{
				uint32_t W2[80];
				for (unsigned i = 0; i < 80; ++i)
				{
					tmp.v = W[i];
					W2[i] = tmp.w[w];
				}

				ubc_check_verify(W2, dvmask_test);

				for (unsigned i = 0; i < DVMASKSIZE; ++i)
				{
					tmp.v = dvmask[i];
					if (tmp.w[w] != dvmask_test[i])
					{
						cerr << "Found error:" << endl
							<< "dvmask [" << i << "] = 0x" << hex << std::setw(8) << std::setfill('0') << tmp.w[w] << dec << endl
							<< "dvmask2[" << i << "] = 0x" << hex << std::setw(8) << std::setfill('0') << dvmask_test[i] << dec << endl
							;
						return 1;
					}
				}
			}
		}
		cout << "Found no discrepancies between ubc_check" << simd_name_str << "() and ubc_check_verify()." << endl << endl;
	}

	if (run_perf_tests)
	{
		cout << "Measuring performance of ubc_check" << simd_name_str << "() " << VECTOR_COUNT << " iterations :" << endl;

		vector< boost::array<SIMD_WORD, 80> > vec_Ws(VECTOR_COUNT);
		typename vector< boost::array<SIMD_WORD, 80> >::iterator it;
		for (it = vec_Ws.begin(); it != vec_Ws.end(); ++it)
			gen_W(rng, &((*it)[0]));

		boost::timer timer;
		SIMD_WORD x;
		int count = 1;
		double sec = 0, perf = 0;

		for (size_t j = 0; j < SIMD_VECSIZE; j++)
			((uint32_t*)&x)[j] = 0;

		cout << "(";
		while (true)
		{
			timer.restart();

			for (int ll = 0; ll < count; ++ll)
			{
				for (it = vec_Ws.begin(); it != vec_Ws.end(); ++it)
				{
					ubc_check_simd(&((*it)[0]), dvmask);
					for (unsigned i = 0; i < DVMASKSIZE; ++i)
					{
						uint32_t* px = (uint32_t*)&x;
						for (size_t j = 0; j < SIMD_VECSIZE; j++)
							px[j] = px[j] + ((uint32_t*)&dvmask[i])[j];
					}
				}
			}

			sec = timer.elapsed();
			perf = double(vec_Ws.size())*double(count) / sec;
			cout << " " << sec << flush;
			if (sec >= 10)
				break;

			count *= 2;
		}

		cout << " ) [ " << hex;
		for (size_t j = 0; j < SIMD_VECSIZE; j++)
		{
			tmp.w[j] = ((uint32_t*)&x)[j];
			cout << tmp.w[j] << " ";
		}

		cout << "]" << dec << endl;

		cout << "Performance: " << SIMD_VECSIZE << " x 2^" << log(perf) / log(2.0) << " #/s" << endl;
	}

	return 0;
}


/*
 *  RandomWithReplacementSelection.cpp
 *  MWorksCore
 *
 *  Created by labuser on 9/25/07.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "RandomWithReplacementSelection.h"
#include "Clock.h"
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>

BEGIN_NAMESPACE_MW

RandomWithReplacementSelection::RandomWithReplacementSelection(int _n_draws, bool _autoreset)
: Selection(_n_draws, _autoreset) {
	shared_ptr <Clock> clock = Clock::instance();
	rng.seed((int)clock->getSystemTimeNS());
}

RandomWithReplacementSelection::~RandomWithReplacementSelection() { }

shared_ptr <Selection> RandomWithReplacementSelection::clone(){
	shared_ptr <RandomWithReplacementSelection> newsel = shared_ptr<RandomWithReplacementSelection>(new RandomWithReplacementSelection(*this));
	return boost::dynamic_pointer_cast<Selection, RandomWithReplacementSelection>(newsel);
}

int RandomWithReplacementSelection::draw() {

	if(done_so_far >= n_draws){
        if(autoreset){
            mwarning(M_PARADIGM_MESSAGE_DOMAIN, "Autoresetting selection object");
            reset();
        } else {
            SelectionOffEdgeException e;
            throw e;
        }
	}

	// Make a random selection
	boost::uniform_int<> our_range(0, getNItems()-1);
	boost::variate_generator<random_generator&, boost::uniform_int<> > dice(rng, our_range);

	int returnval = dice();
	//int returnval = (int)(getNItems() * (float)rand()/(float)RAND_MAX);

	// Save the selection on the tentative selections list
	tentative_selections.push_back(returnval);

	done_so_far++;

	return returnval;
}

void RandomWithReplacementSelection::rejectSelections() {

	done_so_far -= tentative_selections.size();
	tentative_selections = std::vector<int>();
}

END_NAMESPACE_MW


/****************************************************************************
 * @file graph_gen.cpp
 * @author Riccardo Battistini <riccardo.battistini2(at)studio.unibo.it>
 *
 * @brief Program to generate synthetic undirected and unweighted graphs
 * using the Boost Graph Library. Based on examples available in the
 * BGL documentation.
 *
 * @see https://www.boost.org/doc/libs/1_76_0/libs/graph/doc/erdos_renyi_generator.html
 * @see https://www.boost.org/doc/libs/1_76_0/libs/graph/doc/plod_generator.html
 * @see https://www.boost.org/doc/libs/1_76_0/libs/graph/doc/small_world_generator.html
 *
 * Copyright 2021 (c) 2021 by Riccardo Battistini
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 * contributors may be used to endorse or promote products derived from this
 * software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * ---------------------------------------------------------------------------
 *
 * Compile with:
 * g++ -I/usr/include/boost -I../lib/mmio -L../lib/mmio graph_gen.cpp \
 * -o graph_gen -lmmio
 *
 * Run with:
 * ./graph_gen_boost [filename] [graph_type] [number_vertices]
 *
 ****************************************************************************/

#include "mmio.h"
#include <cstdlib>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/graph/small_world_generator.hpp>
#include <boost/graph/plod_generator.hpp>
#include <boost/random/linear_congruential.hpp>

/**
 * @brief Parsing with error handling.
 *
 * @param p pointer to the null-terminated byte string to be interpreted
 * @param endp pointer to a pointer to character
 * @param base base of the interpreted integer value
 * @return the parsed number or -1 if unsuccessful
 */
long strtol_wcheck(const char *p, char *endp, int base) {
    int done = 0;
    long i = -1;

    while (!done) {
        /*
         * errno can be set to any non-zero value by a library function call
         * regardless of whether there was an error, so it needs to be cleared
         * in order to check the error set by strtol.
         */
        errno = 0;

        i = strtol(p, &endp, base);
        p = endp;

        if (errno == ERANGE) {
            fprintf(stderr, "Range error occurred");
            return -1;
        }

        if (p == endp)
            done = 1;
    }

    return i;
}

enum GraphType {
    Random     = 1, // Erdős-Rényi random graph
    SmallWorld = 2, // Watts-Strogatz Small World graph
    ScaleFree  = 3  // Barabási-Albert Scale Free graph
};

typedef boost::adjacency_list<> Graph;

int write_bgl_to_mtx(FILE *f, const Graph &g) {

    typedef boost::property_map<Graph, boost::vertex_index_t>::type IndexMap;

    /*
     * Write the banner.
     */
    MM_typecode matcode;
    mm_initialize_typecode(&matcode);
    mm_set_matrix(&matcode);
    mm_set_coordinate(&matcode);
    mm_set_pattern(&matcode);
    mm_set_symmetric(&matcode);

    if (mm_write_banner(f, matcode)) {
        fprintf(stderr, "Error writing mm banner");
        return EXIT_FAILURE;
    }

    /*
     * Write the header and the values.
     */
    if (mm_write_mtx_crd_size(f,
                              boost::num_vertices(g),
                              boost::num_vertices(g),
                              boost::num_edges(g))) {
        fprintf(stderr, "Error writing header");
        return EXIT_FAILURE;
    }

    IndexMap index = get(boost::vertex_index, g);
    boost::graph_traits<Graph>::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
        if(fprintf(f, "%lu %lu\n", index[source(*ei, g)],
                    index[target(*ei, g)]) < 0) {
            fprintf(stderr, "Error writing values");
            return EXIT_FAILURE;
        }
    }

    return EXIT_SUCCESS;
}

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

    typedef boost::erdos_renyi_iterator<boost::minstd_rand, Graph> ERGen;
    typedef boost::small_world_iterator<boost::minstd_rand, Graph> SWGen;
    typedef boost::plod_iterator<boost::minstd_rand, Graph> SFGen;

    boost::minstd_rand gen;
    int v;
    int gtype;
    Graph g = 0;
    char* fname;

    /*
     * Basic configuration parameters.
     */
    const int min_vertices = 20;
    const int max_vertices = 5000;

    if (argc != 4) {
        fprintf(stderr, "Usage: %s [filename] [graph_type] [number_vertices]\n",
                argv[0]);
        return EXIT_FAILURE;
    }

    fname = argv[1];
    gtype = (int) strtol_wcheck(argv[2], nullptr, 10);
    v = (int) strtol_wcheck(argv[3], nullptr, 10);

    if (v < min_vertices || v > max_vertices) {
        fprintf(stderr, "A graph must have at least %d vertices and no more "
                "than %d vertices\n", min_vertices, max_vertices);
        return EXIT_FAILURE;
    }

    switch (gtype) {
        case Random: {
            // probability of an edge between a pair of nodes
            const float p = 0.02;
            g = Graph(ERGen(gen, v, p), ERGen(), v);
            break;
        }
        case SmallWorld: {
            // number of neighbours to which each node is connected
            const int k = 6;
            // rewiring probability
            const float p = 0.03;
            g = Graph(SWGen(gen, v, k, p), SWGen(), v);
            break;
        }
        case ScaleFree: {
            // controls how steeply is the power law curve
            const float alpha = 2.5;
            // determines the average degree of vertices
            const int beta = 1000;
            g = Graph(SFGen(gen, v, alpha, beta), SFGen(), v);
            break;
        }
        default:
            fprintf(stderr, "Graph types accepted are: \n"
                         "[1]: Random graph \n"
                         "[2]: Small World graph \n"
                         "[3]: Scale Free graph\n");
            return EXIT_FAILURE;
    }

    /*
     * Write BGL graph to a Matrix Market file.
     */
    FILE *f = fopen(fname, "w");

    if (write_bgl_to_mtx(f, g))
        return EXIT_FAILURE;

    fclose(f);
    return EXIT_SUCCESS;
}


#include "DataStructures/BagsImplementations.h"

#include <boost/test/unit_test.hpp>

using DataStructures::Bags::BagAsVector;
using DataStructures::Bags::bag_statistics;

BOOST_AUTO_TEST_SUITE(DataStructures)
BOOST_AUTO_TEST_SUITE(Bags)
BOOST_AUTO_TEST_SUITE(Bags_tests)

BOOST_AUTO_TEST_SUITE(BagAsVector_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DefaultConstructs)
{
  BagAsVector<double> bag;

  BOOST_TEST(bag.size() == 0);
  BOOST_TEST(bag.is_empty());
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(AddsToBagAsVector)
{
  BagAsVector<double> bag;

  BOOST_TEST_REQUIRE(bag.size() == 0);
  BOOST_TEST_REQUIRE(bag.is_empty());

  bag.add(100.0);
  BOOST_TEST(bag.size() == 1);
  BOOST_TEST(!bag.is_empty());

  bag.add(105.1);
  BOOST_TEST(bag.size() == 2);
  BOOST_TEST(!bag.is_empty());
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(IsIterable)
{
  BagAsVector<int> bag;
  bag.add(10);
  bag.add(15);
  bag.add(20);

  int i {0};
  for (int x : bag)
  {
    BOOST_TEST(x == (10 + i * 5));
    i++;
  }
}

BOOST_AUTO_TEST_SUITE_END() // BagAsVector_tests

BOOST_AUTO_TEST_SUITE(BagStatistics_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(CalculatesMeanAndStandardDeviation)
{
  const auto result = bag_statistics<BagAsVector>(
    {100, 99, 101, 120, 98, 107, 109, 81, 101, 90});

  BOOST_TEST(result.first == 100.60);
  BOOST_TEST(result.second == 10.511369505867867);
}

BOOST_AUTO_TEST_SUITE_END() // BagStatistics_tests

BOOST_AUTO_TEST_SUITE_END() // Bags_tests
BOOST_AUTO_TEST_SUITE_END() // Bags
BOOST_AUTO_TEST_SUITE_END() // DataStructures


//
// Created by Alex Beccaro on 25/01/2019.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/101-150/132/problem132.hpp"

BOOST_AUTO_TEST_SUITE( Problem132 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem132::solve(1, 4);
        BOOST_CHECK_EQUAL(res, 9414);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem132::solve();
        BOOST_CHECK_EQUAL(res, 843296);
    }

BOOST_AUTO_TEST_SUITE_END()


#include <boost/circular_buffer.hpp>
#include <iostream>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <range/v3/algorithm.hpp>
#include <range/v3/numeric.hpp>
#include <range/v3/view.hpp>
namespace views = ranges::views;

int main(int argc, char **argv)
{
  if (argc > 1) {
    std::ifstream ifs(argv[1]);
    std::string line;
    int n = 0;
    while (std::getline(ifs, line)) {
      std::istringstream iss(line);
      std::istream_iterator<int> b(iss);
      std::istream_iterator<int> e;
      std::vector<int> v;
      std::copy(b, e, std::back_inserter(v));
      auto [pmin, pmax] = std::minmax_element(v.begin(), v.end());
      n += (*pmax - *pmin);
    }
    std::cout << n << std::endl;
  }
}


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2010.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/letrec.hpp>
#include <mpllibs/metamonad/lambda_c.hpp>
#include <mpllibs/metamonad/eval_syntax.hpp>
#include <mpllibs/metamonad/lazy.hpp>
#include <mpllibs/metamonad/if_.hpp>
#include <mpllibs/metamonad/apply.hpp>
#include <mpllibs/metamonad/syntax.hpp>

#include <boost/test/unit_test.hpp>

#include "common.hpp"

#include <boost/mpl/minus.hpp>
#include <boost/mpl/assert.hpp>

BOOST_AUTO_TEST_CASE(test_letrec)
{
  using boost::mpl::equal_to;
  using boost::mpl::minus;

  using mpllibs::metamonad::letrec;
  using mpllibs::metamonad::if_;
  using mpllibs::metamonad::lambda_c;
  using mpllibs::metamonad::lazy;
  using mpllibs::metamonad::syntax;
  using mpllibs::metamonad::eval_syntax;
  using mpllibs::metamonad::apply;

  // name is replaced with a nullary metafunction evaluating to the
  // substituted expression
  // test_letrec_name
  BOOST_MPL_ASSERT((
    equal_to<int13, eval_syntax<letrec<x, syntax<int13>, syntax<x> > >::type>
  ));

  // test_letrec_not_name
  BOOST_MPL_ASSERT((
    equal_to<
      int11,
      eval_syntax<letrec<x, syntax<int13>, syntax<int11> > >::type
    >
  ));

  // test_template
  BOOST_MPL_ASSERT((
    equal_to<
      int26,
      eval_syntax<
        letrec<x, syntax<int13>, syntax<lazy_double_value<x> > >
      >::type
    >
  ));

  // test_recursion
  BOOST_MPL_ASSERT((
    equal_to<
      int24,
      eval_syntax<
        letrec<
          x,
          syntax<
            lambda_c<
              y,
              if_<
                lazy_equal_to<y, int0>,
                int1,
                lazy_times<apply<x, minus<y, int1> >, y>
              >
            >
          >,
          syntax<apply<x, int4> >
        >
      >::type
    >
  ));

  // test_letrec_lazy
  BOOST_MPL_ASSERT((
    equal_to<
      int11,
      eval_syntax<
        letrec<x, syntax<int2>, syntax<lazy<minus<int13, x> > > >
      >::type
    >
  ));
}


/*
 * MIT License
 *
 * Copyright (c) 2021 Dima Dorezyuk
 *
 * 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 DPOSE_CORE__DPOSE_CORE__HPP
#define DPOSE_CORE__DPOSE_CORE__HPP

#include <opencv2/opencv.hpp>

#include <Eigen/Dense>

#include <array>
#include <stdexcept>
#include <vector>

namespace dpose_core {

/// @brief a closed rectangle (hence 5 columns)
/// First row holds the x, and second row y values.
template <typename _T>
using rectangle = Eigen::Matrix<_T, 2, 5>;

/// @brief constructs a rectangle with the given width and height
/// @param _w width of the rectangle
/// @param _h height of the rectangle
template <typename _T>
inline rectangle<_T>
to_rectangle(const _T& w, const _T& h) noexcept {
  rectangle<_T> box;
  // clang-format off
  box << 0, w, w, 0, 0,
         0, 0, h, h, 0;
  // clang-format on
  return box;
}

/// @brief constructs a rectangle with the given size
/// @param _min lower corner
/// @param _max upper corner
template <typename _T>
inline rectangle<_T>
to_rectangle(const Eigen::Matrix<_T, 2, 1>& _min,
             const Eigen::Matrix<_T, 2, 1>& _max) noexcept {
  rectangle<_T> box;
  // clang-format off
  box << _min.x(), _max.x(), _max.x(), _min.x(), _min.x(),
         _min.y(), _min.y(), _max.y(), _max.y(), _min.y();
  // clang-format on
  return box;
}

/// @brief polygon where first row holds the x, and second row y values.
using polygon = Eigen::Matrix<int, 2UL, Eigen::Dynamic>;

// todo add alignment
using cell = Eigen::Vector2i;
using cell_vector = std::vector<cell>;

/// @brief struct holding the "core" data - just cost matrix and center cell
struct cost_data {
  cost_data() = default;
  cost_data(const polygon& _footprint, size_t _padding);

  inline const cv::Mat&
  get_data() const noexcept {
    return cost;
  }

  inline const cell&
  get_center() const noexcept {
    return center;
  }

  inline const rectangle<int>&
  get_box() const noexcept {
    return box;
  }

private:
  cv::Mat cost;        ///< cost matrix
  cell center;         ///< center cell
  rectangle<int> box;  ///< the bounding box
};

/**
 * @brief Implements bilinear interpolation
 *
 * Call first init() to fetch the cells of interest. Call then get() to the get
 * interpolated values. The functions are seperated to speedup the gradient
 * computation which uses the same cell-data.
 */
struct interpolator {
  interpolator(const cv::Mat& _image);

  bool
  init(const Eigen::Array2d& k_cell);

  double
  get(const Eigen::Array2d& k_cell);

private:
  const Eigen::Array2d bounds;
  const cv::Mat image_;
  Eigen::Array2i k_lower, k_upper;
  Eigen::Vector2d c_rel, c_rel_x, c_rel_y;
  Eigen::Matrix2d m;
};

/**
 * @brief computes a pose-gradient from the given costmap
 *
 * The input (and output) is in cell-domain (not metric).
 */
struct pose_gradient {
  /// @brief POD defining the parameters
  struct parameter {
    unsigned int padding = 2;  ///< padding of the given footprint (in cells)
  };

  // the three arguments for get_pose
  using jacobian = Eigen::Vector3d;
  using pose = Eigen::Vector3d;

  /// @brief sets up internal data based on _footprint and _param.
  /// @param _footprint the footprint (may or may not be closed)
  /// @param _param parameters for the operation
  /// @throws std::runtime_error if the _footprint is ill-formed
  /// @note will safe some images to "/tmp" if compiled as assert enabled
  pose_gradient(const polygon& _footprint, const parameter& _param);
  pose_gradient() = default;

  /// @brief returns the cost for the given se2 pose
  /// @param[in] _begin begin of a cell-vector with lethal costs
  /// @param[in] _end end of a cell-vector with lethal costs
  /// @param[in] _se2 pose of interest. should be in the global frame.
  /// @param[out] _J optional jacobian. will be ignored if nullptr
  double
  get_cost(const pose& _se2, cell_vector::const_iterator _begin,
           cell_vector::const_iterator _end, jacobian* _J) const;

  inline const cost_data&
  get_data() const noexcept {
    return data_;
  }

private:
  cost_data data_;
};

}  // namespace dpose_core

#endif  // DPOSE_CORE__DPOSE_CORE__HPP


// (C) Copyright David James Pearce 2003. Permission to copy, use,
// modify, sell and distribute this software is granted provided this
// copyright notice appears in all copies. This software is provided
// "as is" without express or implied warranty, and with no claim as
// to its suitability for any purpose.
//
// Email: david.pearce@mcs.vuw.ac.nz
//
// Notes
// =====
// This is an alternative solution to the online topological
// order problem. Although, I designed this algorithm from
// scratch myself, it turns out to have already been published:
//
// "On-Line Graph Algorithms for Incremental Compilation",
// Alberto Marchetti-Spaccamela, Umberto Nanni and Hans
// Rohnert. Information Processing Letters, 1996.

#ifndef MNR_ONLINE_TOPOLOGICAL_ORDER_HPP
#define MNR_ONLINE_TOPOLOGICAL_ORDER_HPP

#include <stdexcept>
#include <boost/property_map.hpp>
#include <boost/graph/topological_sort.hpp>
#include "oto_tags.hpp"

#ifdef MNR_GENERATE_STATS
extern unsigned int mnr_ARxy;
extern unsigned int mnr_ddfxy;
extern unsigned int mnr_ninvalid;
extern unsigned int algo_count;
#endif

template<class T, class N2I, class I2NMAP>
class mnr_online_topological_order;

template<class T, class N2I, class I2NMAP>
bool mnr_oto_dfs(typename T::vertex_descriptor h, unsigned int lb,
		 unsigned int ub,
		 typename boost::property_map<T, N2I>::type &n2i,
		 mnr_online_topological_order<T,N2I,I2NMAP> &g) {

  // some useful typedefs

  typedef typename T::vertex_descriptor vd_t;
  typedef typename T::out_edge_iterator oiterator;

  // now the actual code.
  // create worklist and guess at
  // likely max size

  std::vector<vd_t> worklist;
  worklist.reserve((ub - lb) + 1);

  // mark node h as visited

  g._visited[lb]=true;
  worklist.push_back(h);

  // now perform the depth-first-search

  while(worklist.size() > 0) {
    vd_t n(worklist.back());
    worklist.pop_back();
#ifdef MNR_GENERATE_STATS
    ++mnr_ddfxy;
    ++algo_count;
#endif

    oiterator i,iend;
    for(tie(i,iend) = out_edges(n,g); i!=iend;++i) {
      unsigned int wn2i(n2i[target(*i,g)]);
      if(wn2i == ub) {
	// this is the special case
	// where a cycle has been detected
	return true;
      } else if(wn2i < ub && !g._visited[wn2i]) {
	g._visited[wn2i]=true;
	worklist.push_back(target(*i,g));
      }
#ifdef MNR_GENERATE_STATS
    ++mnr_ddfxy;
    ++algo_count;
#endif
    }
  }
  return false;
}

template<class T, class N2I, class I2NMAP>
void mnr_oto_shift(unsigned int lb,
		   unsigned int ub,
		   typename boost::property_map<T, N2I>::type &n2i,
		   mnr_online_topological_order<T,N2I,I2NMAP> &g) {

  // some useful typedefs

  typedef typename T::vertex_descriptor vd_t;

  // the body

  unsigned int shift(0);
  std::vector<vd_t> tmp; // temporary storage
  unsigned int i;
  for(i=lb;i<=ub;++i) {
#ifdef MNR_GENERATE_STATS
    ++algo_count;
#endif
    vd_t w(g._i2n[i]);
    if(g._visited[i]) {
      tmp.push_back(w);
      ++shift;
      g._visited[i]=false;
    } else {
      g._i2n[i-shift]=w;
      n2i[w]=i-shift;
    }
  }
  i-=shift;
  for(unsigned int j=0;j<tmp.size();++j) {
#ifdef MNR_GENERATE_STATS
    ++algo_count;
#endif
    unsigned int w(tmp[j]);
    g._i2n[i+j]=w;
    n2i[w]=i+j;
  }
}

template<class T, class N2I, class I2NMAP>
std::pair<typename T::edge_descriptor, bool>
add_edge(typename mnr_online_topological_order<T,N2I,I2NMAP>::vertex_descriptor t,
	 typename mnr_online_topological_order<T,N2I,I2NMAP>::vertex_descriptor h,
	 mnr_online_topological_order<T,N2I,I2NMAP> &g) {

  std::pair<typename T::edge_descriptor, bool> r;
  r = add_edge(static_cast<typename T::vertex_descriptor>(t),
	       static_cast<typename T::vertex_descriptor>(h),
	       static_cast<T&>(g));

  typedef typename boost::property_map<T, N2I>::type N2iMap;
  N2iMap n2i = get(N2I(),g);

  unsigned int hn2i(n2i[h]);
  unsigned int tn2i(n2i[t]);

  if(r.second && hn2i < tn2i) {
    // need to reorder
    if(mnr_oto_dfs<T,N2I,I2NMAP>(h,hn2i,tn2i,n2i,g)) {
      throw std::runtime_error("loop detected");
    } else {
      mnr_oto_shift(hn2i,tn2i,n2i,g);
    }
#ifdef MNR_GENERATE_STATS
    ++mnr_ninvalid;
    mnr_ARxy += (tn2i - hn2i + 1);
#endif
  }
}

template<class InputIter, class T, class N2I, class I2NMAP>
void add_edges(InputIter b, InputIter e,
	       mnr_online_topological_order<T,N2I,I2NMAP> &g) {
  for(;b!=e;++b) {
    add_edge(b->first,b->second,g);
  }
}

template<class T, class N2I = n2i_t, class I2NMAP = std::vector<typename T::vertex_descriptor> >
class mnr_online_topological_order : public T {
public:
  typedef mnr_online_topological_order<T,N2I,I2NMAP> self;
  friend std::pair<typename T::edge_descriptor, bool> add_edge<>(typename self::vertex_descriptor,
								 typename self::vertex_descriptor,
								 self &);

  friend bool mnr_oto_dfs<>(typename T::vertex_descriptor , unsigned int, unsigned int,
			    typename boost::property_map<T, N2I>::type &,
			    self &);

  friend void mnr_oto_shift<>(unsigned int, unsigned int,
			      typename boost::property_map<T, N2I>::type &,
			      self &);
private:
  I2NMAP _i2n;
  std::vector<bool> _visited;
public:
  mnr_online_topological_order(T const &g, unsigned int acc = 1)
    : T(g), _visited(num_vertices(g),false) {

    // use the topological sort function to
    // order the vertices correctly ...
    _i2n.reserve(num_vertices(g));
    topological_sort(g,std::back_inserter(_i2n));
    // now reverse them
    reverse(_i2n.begin(),_i2n.end());
    // finally, setup the n2i map correctly
    typedef typename boost::property_map<T, N2I>::type N2iMap;
    N2iMap n2i = get(N2I(),*this);

    for(unsigned int i=0;i!=_i2n.size();++i) {
      n2i[_i2n[i]]=i;
    }
  }

  mnr_online_topological_order(typename T::vertices_size_type n,
			       unsigned int acc = 1)
    : T(n), _i2n(n), _visited(n,false) {
    typedef typename boost::property_map<T, N2I>::type N2iMap;
    N2iMap n2i = get(N2I(),*this);

    unsigned int counter(0);
    typename T::vertex_iterator i,iend;
    for(tie(i,iend) = vertices(*this);i!=iend;++i) {
      _i2n[counter]=*i;
      n2i[*i]=counter++;
    }
  }
};

#endif


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 * All rights reserved.
 *
 * See LICENSE file in the root of the mrob library.
 *
 *
 * CustomCholesky.hpp
 *
 *  Created on: Jan 27, 2019
 *      Author: Konstantin Pakulev
 *              konstantin.pakulev@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include <Eigen/SparseCholesky>
#include <Eigen/SparseCore>

using namespace Eigen;

#ifndef MROB_CUSTOMCHOLESKY_H
#define MROB_CUSTOMCHOLESKY_H

namespace Eigen {

    template<typename _MatrixType, int _UpLo = Lower, typename _Ordering = NaturalOrdering<int>> class CustomCholesky;

    namespace internal {
        template<typename _MatrixType, int _UpLo, typename _Ordering>
        struct traits<CustomCholesky<_MatrixType, _UpLo, _Ordering> > {
            typedef _MatrixType MatrixType;
            typedef _Ordering OrderingType;
            enum { UpLo = _UpLo };
            typedef typename MatrixType::Scalar                         Scalar;
            typedef typename MatrixType::StorageIndex                   StorageIndex;
            typedef SparseMatrix<Scalar, ColMajor, StorageIndex>        CholMatrixType;
            typedef TriangularView<const CholMatrixType, Eigen::Lower>  MatrixL;
            typedef TriangularView<const typename CholMatrixType::AdjointReturnType, Eigen::Upper>   MatrixU;
            static inline MatrixL getL(const MatrixType& m) { return MatrixL(m); }
            static inline MatrixU getU(const MatrixType& m) { return MatrixU(m.adjoint()); }
        };
    }

    template<typename _MatrixType, int _UpLo, typename _Ordering>
    class CustomCholesky : public Eigen::SimplicialCholeskyBase<CustomCholesky<_MatrixType, _UpLo, _Ordering> > {
    public:
        typedef _MatrixType MatrixType;
        enum { UpLo = _UpLo };
        typedef SimplicialCholeskyBase<CustomCholesky> Base;
        typedef typename MatrixType::Scalar Scalar;
        typedef internal::traits<CustomCholesky> Traits;
        typedef typename Traits::MatrixL  MatrixL;
        typedef typename Traits::MatrixU  MatrixU;
    public:
        CustomCholesky() : Base() {}

        explicit CustomCholesky(const _MatrixType &matrix)
                : Base(matrix) {}

        CustomCholesky &compute(const _MatrixType &matrix) {
            Base::template compute<false>(matrix);
            return *this;
        }

        void analyzePattern(const _MatrixType &a) {
            Base::analyzePattern(a, false);
        }

        void factorize(const _MatrixType &a) {
            Base::template factorize<false>(a);
        }

        inline const MatrixL matrixL() const {
            eigen_assert(Base::m_factorizationIsOk && "Simplicial LLT not factorized");
            return Traits::getL(Base::m_matrix);
        }

        inline const MatrixU matrixU() const {
            eigen_assert(Base::m_factorizationIsOk && "Simplicial LLT not factorized");
            return Traits::getU(Base::m_matrix);
        }

        MatrixType& getL() {
            return Base::m_matrix;
        }
    };
}

#endif //MROB_CUSTOMCHOLESKY_H


#include <mesh_array_io_xml.h>
#include <util_string_helper.h>

#include <tiny.h>

#include <tinyxml.h>

#include <boost/filesystem.hpp>

#include <cassert>

namespace mesh_array
{

  template<typename T>
  void read_xml(
                std::string const & filename
                , T4Mesh & mesh
                , VertexAttribute<T,T4Mesh> & X
                , VertexAttribute<T,T4Mesh> & Y
                , VertexAttribute<T,T4Mesh> & Z
                )
  {
    boost::filesystem::path  mypath(filename);

    std::string const ext = boost::filesystem::extension( mypath );

    assert( ext == ".xml" || !"read_xml(): illegal file name extension");

#ifdef TIXML_USE_STL
    TiXmlDocument xml_document(filename);
#else
    TiXmlDocument xml_document(filename.c_str());
#endif

    assert( xml_document.LoadFile() || !"read_xml(): file not found");

    TiXmlHandle document_handle( &xml_document );

    TiXmlElement * xml_t4mesh = document_handle.FirstChild( "T4MESH" ).Element();

    assert( xml_t4mesh                          || !"read_xml(): missing T4MESH tag");
    assert( xml_t4mesh->Attribute("nodes")      || !"read_xml(): missing nodes attribute");
    assert( xml_t4mesh->Attribute("tetrahedra") || !"read_xml(): missing tetrahedra attribute");

    size_t const cntV = util::to_value<size_t>(  xml_t4mesh->Attribute("nodes")  );
    size_t const cntT = util::to_value<size_t>(  xml_t4mesh->Attribute("tetrahedra")  );

    mesh.clear();

    mesh.set_capacity( cntV, cntT);

    for(size_t idx=0u; idx < cntV; ++idx)
      mesh.push_vertex();

    TiXmlElement * xml_tetrahedron = document_handle.FirstChild( "T4MESH" ).FirstChild( "TETRAHEDRON" ).Element();

    for( ; xml_tetrahedron; xml_tetrahedron=xml_tetrahedron->NextSiblingElement("TETRAHEDRON") )
    {
      assert( xml_tetrahedron->Attribute("i") || !"read_xml(): missing i index");
      assert( xml_tetrahedron->Attribute("j") || !"read_xml(): missing j index");
      assert( xml_tetrahedron->Attribute("k") || !"read_xml(): missing k index");
      assert( xml_tetrahedron->Attribute("m") || !"read_xml(): missing m index");

      size_t const i = util::to_value<size_t>( xml_tetrahedron->Attribute("i") );
      size_t const j = util::to_value<size_t>( xml_tetrahedron->Attribute("j") );
      size_t const k = util::to_value<size_t>( xml_tetrahedron->Attribute("k") );
      size_t const m = util::to_value<size_t>( xml_tetrahedron->Attribute("m") );

      assert( i < cntV || !"read_xml(): i was out of range");
      assert( j < cntV || !"read_xml(): j was out of range");
      assert( k < cntV || !"read_xml(): k was out of range");
      assert( m < cntV || !"read_xml(): m was out of range");

      Vertex const & vi = mesh.vertex(i);
      Vertex const & vj = mesh.vertex(j);
      Vertex const & vk = mesh.vertex(k);
      Vertex const & vm = mesh.vertex(m);

      mesh.push_tetrahedron(vi,vj,vk,vm);
    }

    X.bind(mesh);
    Y.bind(mesh);
    Z.bind(mesh);

    TiXmlElement * xml_point = document_handle.FirstChild( "T4MESH" ).FirstChild( "NODE" ).Element();

    for( ; xml_point; xml_point=xml_point->NextSiblingElement("NODE") )
    {
      assert( xml_point->Attribute("idx") || !"read_xml(): missing index on point");
      assert( xml_point->Attribute("x")   || !"read_xml(): missing x coordinate");
      assert( xml_point->Attribute("y")   || !"read_xml(): missing y coordinate");
      assert( xml_point->Attribute("z")   || !"read_xml(): missing z coordinate");

      size_t const idx = util::to_value<size_t>( xml_point->Attribute("idx") ) ;

      assert( idx < cntV || !"read_xml(): index out of range");

      T const x = util::to_value<T>(xml_point->Attribute("x"));
      T const y = util::to_value<T>(xml_point->Attribute("y"));
      T const z = util::to_value<T>(xml_point->Attribute("z"));

      assert(is_number(x) || !"read_xml(): x coordinate was not a number");
      assert(is_finite(x) || !"read_xml(): x coordinate was not finite");
      assert(is_number(y) || !"read_xml(): y coordinate was not a number");
      assert(is_finite(y) || !"read_xml(): y coordinate was not finite");
      assert(is_number(z) || !"read_xml(): z coordinate was not a number");
      assert(is_finite(z) || !"read_xml(): z coordinate was not finite");

      Vertex const & vertex = mesh.vertex(idx);

      X( vertex ) = x;
      Y( vertex ) = y;
      Z( vertex ) = z;
    }

    xml_document.Clear();
  }

  template
  void read_xml<float>(
                std::string const & filename
                , T4Mesh & mesh
                , VertexAttribute<float,T4Mesh> & X
                , VertexAttribute<float,T4Mesh> & Y
                , VertexAttribute<float,T4Mesh> & Z
                );

  template
  void read_xml<double>(
                std::string const & filename
                , T4Mesh & mesh
                , VertexAttribute<double,T4Mesh> & X
                , VertexAttribute<double,T4Mesh> & Y
                , VertexAttribute<double,T4Mesh> & Z
                );

  template<typename T>
  void write_xml(
                 std::string const & filename
                 , T4Mesh const & mesh
                 , VertexAttribute<T,T4Mesh> const & X
                 , VertexAttribute<T,T4Mesh> const & Y
                 , VertexAttribute<T,T4Mesh> const & Z
                 )
  {
    assert(X.size()==mesh.vertex_size() || !"write_xml(): X coordinates and vertex size mismatch");
    assert(Y.size()==mesh.vertex_size() || !"write_xml(): Y coordinates and vertex size mismatch");
    assert(Z.size()==mesh.vertex_size() || !"write_xml(): Z coordinates and vertex size mismatch");

    boost::filesystem::path  mypath(filename);

    std::string const ext = boost::filesystem::extension( mypath );

    assert( ext == ".xml" || !"write_xml(): illegal file name extension");

    // build document
    TiXmlDocument doc;

    TiXmlDeclaration * decl = new TiXmlDeclaration( "1.0", "", "" );
    TiXmlElement * meshelem = new TiXmlElement( "T4MESH" );

    meshelem->SetAttribute( "nodes", mesh.vertex_size() );
    meshelem->SetAttribute( "tetrahedra", mesh.tetrahedron_size() );

    doc.LinkEndChild(decl);
    doc.LinkEndChild(meshelem);

    for(size_t idx = 0u; idx < mesh.tetrahedron_size(); ++idx)
    {
      TiXmlElement * elem = new TiXmlElement( "TETRAHEDRON" );

      Tetrahedron const & tetrahedron = mesh.tetrahedron( idx );

      elem->SetAttribute( "i", util::to_string( tetrahedron.i() )  );
      elem->SetAttribute( "j", util::to_string( tetrahedron.j() )  );
      elem->SetAttribute( "k", util::to_string( tetrahedron.k() )  );
      elem->SetAttribute( "m", util::to_string( tetrahedron.m() )  );

      meshelem->LinkEndChild( elem );
    }

    for(size_t idx = 0u; idx < mesh.vertex_size(); ++idx)
    {
      TiXmlElement * elem = new TiXmlElement( "NODE" );

      Vertex const & vertex = mesh.vertex(idx);

      elem->SetAttribute( "idx", util::to_string( idx )  );
      elem->SetAttribute( "x", util::to_string( X(vertex) ) );
      elem->SetAttribute( "y", util::to_string( Y(vertex) ) );
      elem->SetAttribute( "z", util::to_string( Z(vertex) ) );

      meshelem->LinkEndChild( elem );
    }

    // write the document
#ifdef TIXML_USE_STL
    doc.SaveFile(filename);
#else
    doc.SaveFile(filename.c_str());
#endif
  }

  template
  void write_xml<float>(
                 std::string const & filename
                 , T4Mesh const & mesh
                 , VertexAttribute<float,T4Mesh> const & X
                 , VertexAttribute<float,T4Mesh> const & Y
                 , VertexAttribute<float,T4Mesh> const & Z
                 );

  template
  void write_xml<double>(
                 std::string const & filename
                 , T4Mesh const & mesh
                 , VertexAttribute<double,T4Mesh> const & X
                 , VertexAttribute<double,T4Mesh> const & Y
                 , VertexAttribute<double,T4Mesh> const & Z
                 );

} // namespace mesh_array


#include "cluster.h"

#include <deque>
#include <limits>
#include <boost/unordered_map.hpp>

#include "solver.h"

Cluster::Cluster(const uint64_t id, const Board &input) :
    m_ID(id),
    m_Canonical(false),
    m_Solvable(false),
    m_Minimal(false),
    m_NumStates(0)
{
    // move generation buffer
    std::vector<Move> moves;

    // exploration queue
    std::deque<Board> queue;
    queue.push_back(input);

    // unsolve queue
    std::deque<Board> unsolveQueue;

    // large sentinel distance when distance is not yet known
    const int sentinel = std::numeric_limits<int>::max();

    // maps keys to distance from nearest goal state
    boost::unordered_map<BoardKey, int> distance;
    distance[input.Key()] = sentinel;

    // explore reachable nodes
    while (!queue.empty()) {
        Board &board = queue.front();
        if (board.Solved()) {
            m_Solvable = true;
            distance[board.Key()] = 0;
            unsolveQueue.push_back(board);
        }
        board.Moves(moves);
        for (const auto &move : moves) {
            board.DoMove(move);
            if (board < input) {
                // not canonical, exit early
                // and don't count non-canonical solvable boards
                m_Solvable = false;
                return;
            }
            if (distance.emplace(board.Key(), sentinel).second) {
                queue.push_back(board);
            }
            board.UndoMove(move);
        }
        queue.pop_front();
    }

    m_Canonical = true;
    m_NumStates = distance.size();

    if (!m_Solvable) {
        // nothing else to do if it's not solvable
        return;
    }

    // determine how far each state is from a goal state
    int maxDistance = 0;
    m_Unsolved = input;
    while (!unsolveQueue.empty()) {
        Board &board = unsolveQueue.front();
        const int d = distance[board.Key()] + 1;
        board.Moves(moves);
        for (const auto &move : moves) {
            board.DoMove(move);
            const auto item = distance.find(board.Key());
            if (item->second > d) {
                item->second = d;
                unsolveQueue.push_back(board);
                if (d > maxDistance) {
                    maxDistance = d;
                    m_Unsolved = board;
                } else if (d == maxDistance) {
                    if (board < m_Unsolved) {
                        m_Unsolved = board;
                    }
                }
            }
            board.UndoMove(move);
        }
        unsolveQueue.pop_front();
    }

    // determine if unsolved board is minimal
    Solver solver;
    const auto solution = solver.Solve(m_Unsolved);
    const int numPieces = input.Pieces().size();
    std::vector<bool> pieceMoved(numPieces, false);
    for (const auto &move : solution.Moves()) {
        pieceMoved[move.Piece()] = true;
    }
    for (int i = 1; i < pieceMoved.size(); i++) {
        if (pieceMoved[i]) {
            continue;
        }
        Board board(m_Unsolved);
        board.RemovePiece(i);
        if (solver.CountMoves(board) == maxDistance) {
            return;
        }
    }
    m_Minimal = true;

    // record number of states by distance to goal
    m_Distances.resize(maxDistance + 1);
    for (const auto &item : distance) {
        m_Distances[item.second]++;
    }
}


//==================================================================================================
/*!
  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)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/raw.hpp>
#include <simd_test.hpp>

template <typename T, int N, typename Env>
void test(Env& runtime)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T, N>;

  T a1[N], b[N], a2[N], c[N];
  for(int i = 0; i < N; ++i)
  {
    a1[i] = ((i%2) ? T(2*i) : T(-2*i))/T(3);
    a2[i] = T(2*i+1)/T(2);
    b[i] = bs::raw_(bs::nearbyint)(a1[i]) ;
    c[i] = bs::raw_(bs::nearbyint)(a2[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t aa2(&a2[0], &a2[0]+N);
  p_t bb (&b[0], &b[0]+N);
  p_t cc (&c[0], &c[0]+N);
  STF_IEEE_EQUAL(bs::raw_(bs::nearbyint)(aa1), bb);
  STF_IEEE_EQUAL(bs::raw_(bs::nearbyint)(aa2), cc);
}

STF_CASE_TPL("Check raw_(nearbyint) on pack" , STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}


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

  Copyright 2015 NumScale SAS

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

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

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

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

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

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

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

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

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

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

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

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

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


#include <array>
#include <iostream>
#include <boost/math/constants/constants.hpp>
#include "bicycle/whipple.h"
#include "parameters.h"

namespace {
    const double fs = 1000; // sample rate [Hz]
    const double dt = 1.0/fs; // sample time [s]
    const size_t N = 1000; // length of simulation in samples

    std::array<model::BicycleWhipple::state_t, N> continuous_time_system_state;
} // namespace

int main(int argc, char* argv[]) {
    (void)argc;
    (void)argv;

    double v = 5.0;
    model::BicycleWhipple bicycle(v, dt);
    model::BicycleWhipple::state_t x;
    x << 0, 0, 10, 10, 0; // define in degrees
    x *= constants::as_radians;

    std::cout << "initial state: [" << x.transpose() << "]' rad" << std::endl;
    std::cout <<
        "states are: [yaw angle, roll angle, steer angle, roll rate, steer rate]'" <<
        std::endl << std::endl;

    std::cout <<
        "simulating (zero input) continuous time system at constant speed v = " <<
        v << " ..." << std::endl;

    for (auto& state: continuous_time_system_state) {
        state = bicycle.integrate_state(x, model::BicycleWhipple::input_t::Zero(), dt);
        x = state;
    }

    std::cout << std::endl;
    std::cout << "state at end of simulation (" << N << " steps)" << std::endl;
    std::cout << "continuous time (zero input):   " <<
        continuous_time_system_state.back().transpose() << std::endl;
    std::cout << std::endl;

    v = 6.0;
    bicycle.set_v(v);
    x << 0, 0, 10, 10, 0; // define in degrees
    x *= constants::as_radians;

    std::cout << "initial state: [" << x.transpose() << "]' rad" << std::endl;
    std::cout <<
        "states are: [yaw angle, roll angle, steer angle, roll rate, steer rate]'" <<
        std::endl << std::endl;

    std::cout <<
        "simulating (zero input) continuous time system at constant speed v = " <<
        v << " ..." << std::endl;

    for (auto& state: continuous_time_system_state) {
        state = bicycle.integrate_state(x, model::BicycleWhipple::input_t::Zero(), dt);
        x = state;
    }

    std::cout << std::endl;
    std::cout << "state at end of simulation (" << N << " steps)" << std::endl;
    std::cout << "continuous time (zero input):   " <<
        continuous_time_system_state.back().transpose() << std::endl;
    std::cout << std::endl;

    return EXIT_SUCCESS;
}


#include <iostream>
#include <boost/simd/pack.hpp>
#include <vector>
#include <list>
int main()
{
  namespace bs = boost::simd;
  using pack_t = bs::pack<float,8>;
  pack_t res;
  pack_t tens{10};
  pack_t elevens(11);
  std::vector<float> values {10, 11, 12, 13, 14, 15, 16, 17};
  pack_t ptr_pack(values.data());
//   std::list<float> values {10, 11, 12, 13, 14, 15, 16, 17};
//   pack_t ptr_pack(values.begin(), values.end());
  res = (tens + elevens) * 2.f;
  std::cout << res << std::endl;
  std::cout << ptr_pack << std::endl;
  return 0;
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/ieee/include/functions/ulpdist.hpp>

#include <boost/dispatch/functor/meta/call.hpp>

#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>

#include <boost/simd/sdk/config.hpp>

#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/eps.hpp>

NT2_TEST_CASE_TPL ( ulpdist_real,  BOOST_SIMD_REAL_TYPES)
{
  using boost::simd::ulpdist;
  using boost::simd::tag::ulpdist_;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<ulpdist_(T,T)>::type
                  , T
                  );

#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(ulpdist(boost::simd::Inf<T>(), boost::simd::Inf<T>()), boost::simd::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(boost::simd::Minf<T>(), boost::simd::Minf<T>()), boost::simd::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(boost::simd::Nan<T>(), boost::simd::Nan<T>()), boost::simd::Zero<T>());
#endif

  NT2_TEST_EQUAL(ulpdist(boost::simd::Mone<T>(), boost::simd::Mone<T>()), boost::simd::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(boost::simd::Zero<T>(), boost::simd::Zero<T>()), boost::simd::Zero<T>());

  NT2_TEST_EQUAL( ulpdist(boost::simd::One<T>(), boost::simd::One<T>()+boost::simd::Eps<T>())
                , T(0.5)
                );

  NT2_TEST_EQUAL( ulpdist(boost::simd::One<T>(), boost::simd::One<T>()-boost::simd::Eps<T>())
                , T(0.5)
                );

  NT2_TEST_EQUAL( ulpdist(boost::simd::One<T>(), boost::simd::One<T>()-boost::simd::Eps<T>()/2)
                , T(0.25)
                );
}

NT2_TEST_CASE_TPL ( ulpdist_signed_integral,  BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{
  using boost::simd::ulpdist;
  using boost::simd::tag::ulpdist_;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<ulpdist_(T,T)>::type
                  , T
                  );

  NT2_TEST_EQUAL(ulpdist(boost::simd::Mone<T>(), boost::simd::Mone<T>()), boost::simd::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(boost::simd::Zero<T>(), boost::simd::Zero<T>()), boost::simd::Zero<T>());

  NT2_TEST_EQUAL( ulpdist(boost::simd::Zero<T>(), boost::simd::Valmin<T>())
                , boost::simd::Valmax<T>()
                );

  NT2_TEST_EQUAL( ulpdist(boost::simd::Valmin<T>(), boost::simd::Zero<T>())
                , boost::simd::Valmax<T>()
                );

  NT2_TEST_EQUAL( ulpdist(boost::simd::Zero<T>(), boost::simd::Valmax<T>())
                , boost::simd::Valmax<T>()
                );

  NT2_TEST_EQUAL( ulpdist(boost::simd::Valmax<T>(), boost::simd::Zero<T>())
                , boost::simd::Valmax<T>()
                );

  NT2_TEST_EQUAL( ulpdist(boost::simd::Valmin<T>(), boost::simd::Valmax<T>())
                , boost::simd::Valmax<T>()
                );
}

NT2_TEST_CASE_TPL ( ulpdist_unsigned_integral,  BOOST_SIMD_UNSIGNED_TYPES)
{
  using boost::simd::ulpdist;
  using boost::simd::tag::ulpdist_;

  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<ulpdist_(T,T)>::type
                  , T
                  );

  NT2_TEST_EQUAL(ulpdist(boost::simd::Mone<T>(), boost::simd::Mone<T>()), boost::simd::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::Zero<T>());
  NT2_TEST_EQUAL(ulpdist(boost::simd::Zero<T>(), boost::simd::Zero<T>()), boost::simd::Zero<T>());

  NT2_TEST_EQUAL( ulpdist(boost::simd::Zero<T>(), boost::simd::Valmin<T>())
                , boost::simd::Zero<T>()
                );

  NT2_TEST_EQUAL( ulpdist(boost::simd::Valmin<T>(), boost::simd::Zero<T>())
                , boost::simd::Zero<T>()
                );

  NT2_TEST_EQUAL( ulpdist(boost::simd::Zero<T>(), boost::simd::Valmax<T>())
                , boost::simd::Valmax<T>()
                );

  NT2_TEST_EQUAL( ulpdist(boost::simd::Valmax<T>(), boost::simd::Zero<T>())
                , boost::simd::Valmax<T>()
                );

  NT2_TEST_EQUAL( ulpdist(boost::simd::Valmin<T>(), boost::simd::Valmax<T>())
                , boost::simd::Valmax<T>()
                );
}


#include <algorithm>
#include <vector>
#include <list>
#include <queue>
#include <string>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <numeric>
#include <limits>
#include <utility>
#include <functional>
#include <iterator>
#include <iostream>
#include <sstream>
#include <cctype>
#include <cmath>
#include <cstdlib>
//#include <boost/multiprecision/cpp_int.hpp>

using namespace std;
//using namespace boost::multiprecision;

void comp(int tc){
	int N;
	cin >> N;
	vector<int> ori(N);
	for (int i = 0; i < N; ++i){
		cin >> ori[i];
	}

	int count = 0;

	for (int i = 0; i < N; ++i){
		auto it = min_element(ori.begin(), ori.end());
		count += min((int)(it - ori.begin()), (int)(ori.end() - 1 - it));
		ori.erase(it);
	}

	cout << "Case #" << tc << ": " << count << endl;
}

int main(){
	int T;
	cin >> T;
	for (int tc = 1; tc <= T; ++tc){
		comp(tc);
	}
}