#include <Ziran/CS/Util/RandomNumber.h>
#include <Eigen/Geometry>
#include <chrono>
#include <math.h>

namespace ZIRAN {

template <class T>
RandomNumber<T>::RandomNumber(unsigned s)
    : generator(s)
{
}
template <class T>
RandomNumber<T>::~RandomNumber()
{
}

template <class T>
void RandomNumber<T>::resetSeed(T s)
{
    generator.seed(s);
}

template <class T>
void RandomNumber<T>::resetSeedUsingTime()
{
    auto s = std::chrono::high_resolution_clock::now().time_since_epoch().count();
    generator.seed(s);
}

template <class T>
T RandomNumber<T>::randReal()
{
    std::uniform_real_distribution<T> distribution((T)0, (T)1);
    return distribution(generator);
}

template <class T>
T RandomNumber<T>::randReal(T a, T b)
{
    std::uniform_real_distribution<T> distribution(a, b);
    return distribution(generator);
}

template <class T>
int RandomNumber<T>::randInt(int a, int b)
{
    std::uniform_int_distribution<> distribution(a, b);
    return distribution(generator);
}

template <class T>
template <int d, int flags>
Vector<T, d> RandomNumber<T>::randInBox(const Eigen::Matrix<T, d, 1, flags, d, 1>& min_corner, const Eigen::Matrix<T, d, 1, flags, d, 1>& max_corner)
{
    Vector<T, d> r;
    for (int i = 0; i < d; i++)
        r(i) = randReal(min_corner(i), max_corner(i));
    return r;
}

template <class T>
template <int d>
Vector<T, d> RandomNumber<T>::randomBarycentricWeights()
{
    Vector<T, d> r;
    T sum;
    do {
        sum = 0;
        for (int i = 0; i < d - 1; i++) {
            r(i) = randReal();
            sum += r(i);
        }
    } while (sum > 1);
    r(d - 1) = 1 - sum;
    return r;
}

template <class T>
template <int d, int flags>
Vector<T, d> RandomNumber<T>::randInBall(const Eigen::Matrix<T, d, 1, flags, d, 1>& center, T radius)
{
    Vector<T, d> min_corner = center.array() - radius;
    Vector<T, d> max_corner = center.array() + radius;

    Vector<T, d> r;
    do {
        r = randInBox(min_corner, max_corner);
    } while ((r - center).squaredNorm() > radius * radius);
    return r;
}

template <class T>
void RandomNumber<T>::randRotation(Matrix<T, 2, 2>& R)
{
    T theta = randReal(0, 2 * M_PI);
    T c = std::cos(theta);
    T s = std::sin(theta);
    R << c, -s, s, c;
}

template <class T>
void RandomNumber<T>::randRotation(Matrix<T, 3, 3>& R)
{
    std::normal_distribution<T> n;
    Eigen::Quaternion<T> q(n(generator), n(generator), n(generator), n(generator));
    q.normalize();
    R = q.toRotationMatrix();
}

template <class T, class Derived>
static void fillHelper(RandomNumber<T>& rand, Eigen::DenseBase<Derived>& x, T a, T b)
{
    for (typename Derived::Index i = 0; i < x.size(); i++)
        x(i) = rand.randReal(a, b);
}

template <class T, class T2>
static std::enable_if_t<std::is_arithmetic<T2>::value> fillHelper(RandomNumber<T>& rand, T2& x, T a, T b)
{
    x = rand.randReal(a, b);
}

template <class T>
template <class T2>
void RandomNumber<T>::fill(T2& x, T a, T b)
{
    fillHelper(*this, x, a, b);
}

template <class T, class Derived>
static void fillIntHelper(RandomNumber<T>& rand, Eigen::DenseBase<Derived>& x, int a, int b)
{
    for (typename Derived::Index i = 0; i < x.size(); i++)
        x(i) = rand.randInt(a, b);
}

template <class T, class T2>
static std::enable_if_t<std::is_arithmetic<T2>::value> fillIntHelper(RandomNumber<T>& rand, T2& x, int a, int b)
{
    x = rand.randInt(a, b);
}

template <class T>
template <class T2>
void RandomNumber<T>::fillInt(T2& x, int a, int b)
{
    fillIntHelper(*this, x, a, b);
}

template Eigen::Matrix<double, 2, 1, 0, 2, 1> RandomNumber<double>::randInBall<2, 0>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, double);
template Eigen::Matrix<double, 2, 1, 0, 2, 1> RandomNumber<double>::randInBall<2, 2>(Eigen::Matrix<double, 2, 1, 2, 2, 1> const&, double);
template Eigen::Matrix<double, 3, 1, 0, 3, 1> RandomNumber<double>::randInBox<3, 0>(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&);
template Eigen::Matrix<double, 3, 1, 0, 3, 1> RandomNumber<double>::randInBall<3, 0>(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, double);
template Eigen::Matrix<double, 3, 1, 0, 3, 1> RandomNumber<double>::randInBall<3, 2>(Eigen::Matrix<double, 3, 1, 2, 3, 1> const&, double);
template Eigen::Matrix<double, 3, 1, 0, 3, 1> RandomNumber<double>::randomBarycentricWeights<3>();
template Eigen::Matrix<float, 2, 1, 0, 2, 1> RandomNumber<float>::randInBall<2, 0>(Eigen::Matrix<float, 2, 1, 0, 2, 1> const&, float);
template Eigen::Matrix<float, 2, 1, 0, 2, 1> RandomNumber<float>::randInBall<2, 2>(Eigen::Matrix<float, 2, 1, 2, 2, 1> const&, float);
template Eigen::Matrix<float, 2, 1, 0, 2, 1> RandomNumber<float>::randInBox<2, 0>(Eigen::Matrix<float, 2, 1, 0, 2, 1> const&, Eigen::Matrix<float, 2, 1, 0, 2, 1> const&);
template Eigen::Matrix<float, 3, 1, 0, 3, 1> RandomNumber<float>::randInBall<3, 0>(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&, float);
template Eigen::Matrix<float, 3, 1, 0, 3, 1> RandomNumber<float>::randInBall<3, 2>(Eigen::Matrix<float, 3, 1, 2, 3, 1> const&, float);
template Eigen::Matrix<float, 3, 1, 0, 3, 1> RandomNumber<float>::randInBox<3, 0>(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&, Eigen::Matrix<float, 3, 1, 0, 3, 1> const&);
template Eigen::Matrix<float, 3, 1, 0, 3, 1> RandomNumber<float>::randomBarycentricWeights<3>();
template Vector<double, 2> RandomNumber<double>::randInBox<2, 0>(Eigen::Matrix<double, 2, 1, 0, 2, 1> const&, Eigen::Matrix<double, 2, 1, 0, 2, 1> const&);
template class RandomNumber<double>;
template class RandomNumber<float>;
template void RandomNumber<double>::fill<double>(double&, double, double);
template void RandomNumber<float>::fill<float>(float&, float, float);
template void RandomNumber<double>::fill<Eigen::Matrix<double, 2, 1, 0, 2, 1>>(Eigen::Matrix<double, 2, 1, 0, 2, 1>&, double, double);
template void RandomNumber<double>::fill<Eigen::Matrix<double, 3, 1, 0, 3, 1>>(Eigen::Matrix<double, 3, 1, 0, 3, 1>&, double, double);
template void RandomNumber<double>::fill<Eigen::Matrix<double, -1, -1, 0, -1, -1>>(Eigen::Matrix<double, -1, -1, 0, -1, -1>&, double, double);
template void RandomNumber<double>::fill<Eigen::Matrix<double, 2, -1, 0, 2, -1>>(Eigen::Matrix<double, 2, -1, 0, 2, -1>&, double, double);
template void RandomNumber<double>::fill<Eigen::Matrix<double, 2, 2, 0, 2, 2>>(Eigen::Matrix<double, 2, 2, 0, 2, 2>&, double, double);
template void RandomNumber<double>::fill<Eigen::Matrix<double, 3, -1, 0, 3, -1>>(Eigen::Matrix<double, 3, -1, 0, 3, -1>&, double, double);
template void RandomNumber<double>::fill<Eigen::Matrix<double, 3, 3, 0, 3, 3>>(Eigen::Matrix<double, 3, 3, 0, 3, 3>&, double, double);
template void RandomNumber<double>::fill<Eigen::Matrix<double, 3, 2, 0, 3, 2>>(Eigen::Matrix<double, 3, 2, 0, 3, 2>&, double, double);
template void RandomNumber<double>::fill<Eigen::Matrix<double, 4, 2, 0, 4, 2>>(Eigen::Matrix<double, 4, 2, 0, 4, 2>&, double, double);
template void RandomNumber<double>::fill<Eigen::Matrix<double, 4, 3, 0, 4, 3>>(Eigen::Matrix<double, 4, 3, 0, 4, 3>&, double, double);
template void RandomNumber<double>::fill<Eigen::Matrix<double, 8, 2, 0, 8, 2>>(Eigen::Matrix<double, 8, 2, 0, 8, 2>&, double, double);
template void RandomNumber<double>::fill<Eigen::Matrix<double, 7, 3, 0, 7, 3>>(Eigen::Matrix<double, 7, 3, 0, 7, 3>&, double, double);
template void RandomNumber<double>::fill<Eigen::Matrix<double, 8, 3, 0, 8, 3>>(Eigen::Matrix<double, 8, 3, 0, 8, 3>&, double, double);
template void RandomNumber<double>::fill<Eigen::Matrix<double, 26, 3, 0, 26, 3>>(Eigen::Matrix<double, 26, 3, 0, 26, 3>&, double, double);
template void RandomNumber<double>::fillInt<Eigen::Matrix<double, 2, 2, 0, 2, 2>>(Eigen::Matrix<double, 2, 2, 0, 2, 2>&, int, int);
template void RandomNumber<double>::fillInt<Eigen::Matrix<double, 3, 3, 0, 3, 3>>(Eigen::Matrix<double, 3, 3, 0, 3, 3>&, int, int);

template void RandomNumber<float>::fill<Eigen::Array<float, 10000, 1, 0, 10000, 1>>(Eigen::Array<float, 10000, 1, 0, 10000, 1>&, float, float);
template void RandomNumber<float>::fill<Eigen::Matrix<float, 3, 1, 0, 3, 1>>(Eigen::Matrix<float, 3, 1, 0, 3, 1>&, float, float);
template void RandomNumber<float>::fill<Eigen::Matrix<float, 3, 2, 0, 3, 2>>(Eigen::Matrix<float, 3, 2, 0, 3, 2>&, float, float);
template void RandomNumber<float>::fill<Eigen::Matrix<float, -1, 1, 0, -1, 1>>(Eigen::Matrix<float, -1, 1, 0, -1, 1>&, float, float);
template void RandomNumber<float>::fill<Eigen::Matrix<float, 1, 1, 0, 1, 1>>(Eigen::Matrix<float, 1, 1, 0, 1, 1>&, float, float);
template void RandomNumber<float>::fill<Eigen::Matrix<float, 2, 1, 0, 2, 1>>(Eigen::Matrix<float, 2, 1, 0, 2, 1>&, float, float);
template void RandomNumber<float>::fill<Eigen::Matrix<float, 2, 2, 0, 2, 2>>(Eigen::Matrix<float, 2, 2, 0, 2, 2>&, float, float);
template void RandomNumber<float>::fill<Eigen::Matrix<float, 3, 3, 0, 3, 3>>(Eigen::Matrix<float, 3, 3, 0, 3, 3>&, float, float);
template void RandomNumber<float>::fill<Eigen::Matrix<float, 4, 2, 0, 4, 2>>(Eigen::Matrix<float, 4, 2, 0, 4, 2>&, float, float);
template void RandomNumber<float>::fill<Eigen::Matrix<float, 4, 3, 0, 4, 3>>(Eigen::Matrix<float, 4, 3, 0, 4, 3>&, float, float);
template void RandomNumber<float>::fill<Eigen::Matrix<float, 8, 2, 0, 8, 2>>(Eigen::Matrix<float, 8, 2, 0, 8, 2>&, float, float);
template void RandomNumber<float>::fill<Eigen::Matrix<float, 8, 3, 0, 8, 3>>(Eigen::Matrix<float, 8, 3, 0, 8, 3>&, float, float);
template void RandomNumber<float>::fill<Eigen::Matrix<float, 7, 3, 0, 7, 3>>(Eigen::Matrix<float, 7, 3, 0, 7, 3>&, float, float);
template void RandomNumber<float>::fill<Eigen::Matrix<float, 26, 3, 0, 26, 3>>(Eigen::Matrix<float, 26, 3, 0, 26, 3>&, float, float);
template void RandomNumber<float>::fillInt<Eigen::Matrix<float, 2, 2, 0, 2, 2>>(Eigen::Matrix<float, 2, 2, 0, 2, 2>&, int, int);
template void RandomNumber<float>::fillInt<Eigen::Matrix<float, 3, 3, 0, 3, 3>>(Eigen::Matrix<float, 3, 3, 0, 3, 3>&, int, int);
} // namespace ZIRAN


#include <iostream>
#include <vector>
#include <string>
#include <Eigen/Dense>
#include "../include/layer.h"
#include <map>
#include <memory>

using namespace Eigen;

int main()
{
    using namespace MyDL;
    using std::cout;
    using std::endl;
    using std::map;
    using std::shared_ptr;
    using std::string;
    using std::unique_ptr;
    using std::vector;

    // Params
    // auto W = std::make_shared<MatrixXd>(2, 2); // 左辺はautoで問題なし
    // auto b = std::make_shared<MatrixXd>(1, 2);
    // *W = MatrixXd::Random(2, 2);
    // *b = MatrixXd::Zero(1, 2);
    map<string, shared_ptr<MatrixXd>> params;

    // Layers
    // vector<unique_ptr<BaseLayer>> layers; // 基底クラスのポインタのコンテナを用意する
    vector<shared_ptr<BaseLayer>> layers; // 基底クラスのポインタのコンテナを用意する
    map<string, unique_ptr<BaseLayer>> layers_map;
    // 抽象クラスのポインタに格納するので、unique_ptrとしてはBaseLayerに格納
    // unique_ptr<BaseLayer> p_layer1(new AddLayer()); // C++11ではmake_uniqueが存在しない
    // unique_ptr<BaseLayer> p_layer2(new MulLayer());
    // unique_ptr<BaseLayer> p_layer3(new ReLU());
    // unique_ptr<BaseLayer> p_layer4(new Sigmoid());
    // unique_ptr<BaseLayer> p_layer5(new MyDL::Affine(W, b));
    // unique_ptr<BaseLayer> p_layer6(new SoftmaxWithLoss());

    shared_ptr<BaseLayer> p_layer1 = std::make_shared<AddLayer>();
    shared_ptr<BaseLayer> p_layer2 = std::make_shared<MulLayer>();
    shared_ptr<BaseLayer> p_layer3 = std::make_shared<ReLU>();
    shared_ptr<BaseLayer> p_layer4 = std::make_shared<Sigmoid>();
    // shared_ptr<BaseLayer> p_layer5 = std::make_shared<MyDL::Affine>(W, b);
    shared_ptr<BaseLayer> p_layer5 = std::make_shared<MyDL::Affine>(2, 2); // サイズを入力するようにしてみる
    shared_ptr<BaseLayer> p_layer6 = std::make_shared<SoftmaxWithLoss>();

    // AddLayer    layer1;
    // MulLayer    layer2;
    // ReLU        layer3;
    // Sigmoid     layer4;
    // MyDL::Affine    layer5(W, b);
    // SoftmaxWithLoss layer6;

    // I/O containers
    vector<MatrixXd> inputs;
    vector<MatrixXd> outputs;
    // MatrixXd X = -2 * MatrixXd::Identity(2, 2) + MatrixXd::Ones(2, 2);
    MatrixXd X = MatrixXd::Identity(2, 2);
    MatrixXd Y = MatrixXd::Ones(2, 2);
    vector<MatrixXd> dout;
    dout.push_back(MatrixXd::Ones(2, 2));

    // inputs
    inputs.push_back(X);
    inputs.push_back(Y);

    // layers
    // layers.push_back(std::move(p_layer1)); // Layerの派生クラス(Add)のポインタをコンテナに格納
    // layers.push_back(std::move(p_layer2)); // Layerの派生クラス(Mul)のポインタをコンテナに格納
    // layers.push_back(std::move(p_layer3)); // Layerの派生クラス(ReLU)のポインタをコンテナに格納
    // layers.push_back(std::move(p_layer4)); // Layerの派生クラス(Sigmoid)のポインタをコンテナに格納
    // layers.push_back(std::move(p_layer5)); // Layerの派生クラス(Affine)のポインタをコンテナに格納
    // layers.push_back(std::move(p_layer6)); // Layerの派生クラス(SoftmaxWithLoss)のポインタをコンテナに格納

    layers.push_back(p_layer1); // Layerの派生クラス(Add)のポインタをコンテナに格納
    layers.push_back(p_layer2); // Layerの派生クラス(Mul)のポインタをコンテナに格納
    layers.push_back(p_layer3); // Layerの派生クラス(ReLU)のポインタをコンテナに格納
    layers.push_back(p_layer4); // Layerの派生クラス(Sigmoid)のポインタをコンテナに格納
    layers.push_back(p_layer5); // Layerの派生クラス(Affine)のポインタをコンテナに格納
    layers.push_back(p_layer6); // Layerの派生クラス(SoftmaxWithLoss)のポインタをコンテナに格納

    if (auto affine = std::dynamic_pointer_cast<MyDL::Affine>(p_layer5))
    {
        params["W"] = affine->pW;
        params["b"] = affine->pb;
    }

    // layers_map["Add"] = p_layer1;
    // layers_map["Affine"] = p_layer5;

    // cout << "--- map forward ---" << endl;
    // outputs = layers_map["Add"]->forward(inputs);
    // cout << outputs[0] << endl;
    // outputs = layers_map["Affine"]->forward(inputs);
    // cout << outputs[0] << endl;

    cout << "----forward----" << endl;
    for (int i = 0; i < layers.size(); i++)
    {
        cout << "----layer" << i << "----" << endl;
        outputs = layers[i]->forward(inputs); // コンテナに格納したLayerのアドレスから、メンバ関数を呼び出し(ポリモーフィズム)

        cout << outputs[0] << endl;
    }

    // Affineレイヤのパラメータを更新できるのか確認したいので、backwardの処理を走らせた後に、
    // gradsの内容を加えるという処理をし、その処理の前後でパラメータの内容が変化しているかを確認する。
    cout << "----backward----" << endl;
    for (int i = 0; i < layers.size(); i++)
    {
        vector<MatrixXd> grads;
        cout << "----layer" << i << "----" << endl;
        if (i < layers.size() - 1)
        {
            grads = layers[i]->backward(dout);
        }
        else
        {
            vector<MatrixXd> init_dout = {MatrixXd::Ones(1, 1)};
            grads = layers[i]->backward(init_dout);
        }

        for (int j = 0; j < grads.size(); j++)
        {
            cout << grads[j] << endl;
        }
    }

    if (auto affine = std::dynamic_pointer_cast<MyDL::Affine>(layers[4]))
    {
        cout << "--- Affine Param b ---" << endl;
        cout << *(affine->pb) << endl;
        cout << "params[b]:" << endl;
        cout << *(params["b"]) << endl;
        cout << "--- Affine Param W ---" << endl;
        cout << *(affine->pW) << endl;
        cout << "params[W]:" << endl;
        cout << *(params["W"]) << endl;

        *(params["b"]) -= affine->db;
        *(params["W"]) -= affine->dW;
    }
    // 別のスコープでアクセスしたときに更新されているか？
    if (auto re_affine = std::dynamic_pointer_cast<MyDL::Affine>(layers[4]))
    {
        cout << "--- after update ---" << endl; // 内部で処理しないと、affineが宣言されていない、となる。スコープ外？
        cout << *(re_affine->pb) << endl;
        cout << *(re_affine->pW) << endl;
    }

    cout << "params[b]" << endl;
    cout << *(params["b"]) << endl;
    cout << "params[W]" << endl;
    cout << *(params["W"]) << endl;

    return 0;
}


/* Copyright (c) 2016, the Cap authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#define BOOST_TEST_MODULE TestPostProcessor

#include "main.cc"

#include <cap/post_processor.h>
#include <cap/utils.h>
#include <boost/format.hpp>
#include <boost/foreach.hpp>
#include <boost/test/unit_test.hpp>
#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <cmath>

BOOST_AUTO_TEST_CASE(test_compute_energy)
{
  // We need to use a large tolerance because the error increases at each time
  // step.
  double const tolerance = 1e-6;

  double const pi = M_PI;
  double const initial_time = 0.0;
  double const final_time = 2.0 * pi;
  std::size_t const n = 10001;
  std::vector<double> time(n);
  std::vector<double> power(n);

  // INITIALIZE SOLUTION
  for (std::size_t i = 0; i < n; ++i)
  {
    time[i] = initial_time +
              static_cast<double>(i) / (n - 1) * (final_time - initial_time);
    power[i] = std::cos(time[i]);
  }

  // COMPUTE ENERGY
  std::vector<double> energy(n);
  cap::compute_energy(time, power, energy);

  // CHECK THE ANSWER
  std::vector<double> exact(n);
  std::vector<double> error(n);
  exact[0] = 0.0;
  for (std::size_t i = 0; i < n; ++i)
  {
    exact[i] = std::sin(time[i]);
    error[i] = energy[i] - exact[i];
    BOOST_REQUIRE(std::abs(error[i]) < tolerance);
  }
}


/*
 * Copyright (C) 2018 Swift Navigation Inc.
 * Contact: Swift Navigation <dev@swiftnav.com>
 *
 * This source is subject to the license found in the file 'LICENSE' which must
 * be distributed together with this source. All other rights reserved.
 *
 * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
 * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
 */

#include "covariance_functions/distance_metrics.h"
#include "test_utils.h"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <gtest/gtest.h>
#include <iostream>
#include <vector>

namespace albatross {

TEST(test_distance_metrics, test_euclidean_distance) {

  Eigen::VectorXd x(3);
  Eigen::VectorXd y(3);
  EuclideanDistance dist;

  x << 1., 1., 1.;
  y << 1., 1., 2.;
  EXPECT_DOUBLE_EQ(dist(x, y), 1.);

  x << 1., 1., 1.;
  y << 2., 2., 2.;
  EXPECT_DOUBLE_EQ(dist(x, y), sqrt(3.));

  x << 2., 2., 2.;
  y << 2., 2., 2.;
  EXPECT_DOUBLE_EQ(dist(x, y), 0.);
}

TEST(test_distance_metrics, test_radial_distance) {

  Eigen::VectorXd x(3);
  Eigen::VectorXd y(3);
  RadialDistance dist;

  x << 0., 0., 1.;
  y << 0., 0., 1.;
  EXPECT_DOUBLE_EQ(dist(x, y), 0.);

  x << 0., 0., 1.;
  y << 0., 1., 0.;
  EXPECT_DOUBLE_EQ(dist(x, y), 0.);

  x << 0., 1., 1.;
  y << 1., 0., 0.;
  EXPECT_DOUBLE_EQ(dist(x, y), sqrt(2.) - 1.);
}

TEST(test_distance_metrics, test_angular_distance) {

  Eigen::VectorXd x(3);
  Eigen::VectorXd y(3);
  AngularDistance dist;

  x << 0., 0., 1.;
  y << 0., 0., 1.;
  EXPECT_DOUBLE_EQ(dist(x, y), 0.);

  x << 0., 0., 1.;
  y << 0., 0., -1.;
  EXPECT_DOUBLE_EQ(dist(x, y), M_PI);

  x << 0., 0., 1.;
  y << 0., 1., 0.;
  EXPECT_DOUBLE_EQ(dist(x, y), M_PI / 2.);
}

TEST(test_distance_metrics, test_distance_matrix) {

  const auto points = random_spherical_points(10);

  EuclideanDistance dist;

  const auto dist_matrix = distance_matrix(dist, points);
  EXPECT_EQ(dist_matrix.rows(), points.size());
  EXPECT_EQ(dist_matrix.cols(), points.size());
}

} // namespace albatross


#include <iostream>
#include "Solver.hpp"

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/Dense>

#include <chrono>

namespace py = pybind11;

using namespace Eigen;
using namespace std;

VectorXd solveQP( const py::EigenDRef<const MatrixXd> &P, const py::EigenDRef<const VectorXd> &q, const py::EigenDRef<const VectorXd> &warm_start,const double epsilon =1e-10, const double mu_prox = 1e-7, const int max_iter=1000, const bool adaptative_rho=true){
    Solver solver;
    VectorXd solution(q.size());
    solution = solver.solveQP(P,q,warm_start,epsilon,mu_prox,max_iter,adaptative_rho);
    return solution;
}

VectorXd solveDerivativesQP(const py::EigenDRef<const MatrixXd> &P, const py::EigenDRef<const VectorXd> &q, const py::EigenDRef<const VectorXd> &l, const py::EigenDRef<const VectorXd> &grad_l, const double epsilon =1e-10){
    Solver solver;
    VectorXd gamma(l.size()),bl(l.size());
    gamma = solver.dualFromPrimalQP(P,q,l,epsilon);
    bl = solver.solveDerivativesQP(P,q,l,gamma,grad_l,epsilon);
    return bl;
}

VectorXd solveQCQP( const py::EigenDRef<const MatrixXd> &P, const py::EigenDRef<const VectorXd> &q,const py::EigenDRef<const VectorXd> &l_n, const py::EigenDRef<const VectorXd> &mu, const py::EigenDRef<const VectorXd> &warm_start,const double epsilon=1e-10,const double mu_prox = 1e-7, const int max_iter = 1000, const bool adaptative_rho = true){
    Solver solver;
    VectorXd solution(q.size()), mul_n(l_n.size());
    mul_n = l_n.cwiseProduct(mu);
    solution = solver.solveQCQP(P,q,mul_n,warm_start,epsilon,mu_prox,max_iter,adaptative_rho);
    return solution;
}

std::tuple<MatrixXd,MatrixXd,VectorXd> solveDerivativesQCQP(const py::EigenDRef<const MatrixXd> &P, const py::EigenDRef<const VectorXd> &q, const py::EigenDRef<const VectorXd> &l_n, const py::EigenDRef<const VectorXd> &mu, const py::EigenDRef<const VectorXd> &l, const py::EigenDRef<const VectorXd> &grad_l, const double epsilon =1e-10){
    Solver solver;
    MatrixXd E1(l_n.size(),l_n.size()),E2(l_n.size(),l_n.size());
    VectorXd mul_n(l_n.size()),gamma(l.size()),blgamma(l.size());
    mul_n = l_n.cwiseProduct(mu);
    gamma = solver.dualFromPrimalQCQP(P,q,mul_n,l,epsilon);
    std::tie(E1,E2) = solver.getE12QCQP(l_n, mu, gamma);
    blgamma = solver.solveDerivativesQCQP(P,q,mul_n,l,gamma,grad_l,epsilon);
    return std::make_tuple(E1,E2,blgamma);
}

VectorXd solveLCQP( const py::EigenDRef<const MatrixXd> &P, const py::EigenDRef<const VectorXd> &q, const py::EigenDRef<const VectorXd> &warm_start,const double epsilon=1e-10,const double mu_prox = 1e-7, const int max_iter = 1000, const bool adaptative_rho = true){
    Solver solver;
    VectorXd solution(q.size());
    solution = solver.solveLCQP(P,q,warm_start,epsilon,mu_prox,max_iter,adaptative_rho);
    return solution;
}

VectorXd solveDerivativesLCQP(const py::EigenDRef<const MatrixXd> &P, const py::EigenDRef<const VectorXd> &q, const py::EigenDRef<const VectorXd> &l, const py::EigenDRef<const VectorXd> &grad_l, const double epsilon =1e-10){
    Solver solver;
    return solver.solveDerivativesLCQP(P,q,l,grad_l,epsilon);
}

PYBIND11_MODULE(diffsolvers, m) {
    m.doc() = "module solving QCQP and QP with ADMM, and computing the derivatives of the solution using implicit differentiation of KKT optimality conditions";
    m.def("solveQP", &solveQP, "A function which solves a QP problem with a regularized ADMM algorithm",py::arg("P"), py::arg("q"),py::arg("warm_start"),py::arg("epsilon") = 1e-10,py::arg("mu_prox")= 1e-7,py::arg("max_iter")= 1000,py::arg("adaptative_rho")= true, py::return_value_policy::reference_internal );
    m.def("solveQCQP", &solveQCQP, "A function which solves a QCQP problem with a regularized ADMM algorithm",py::arg("P"), py::arg("q"), py::arg("l_n"),py::arg("mu"), py::arg("warm_start"),py::arg("epsilon")= 1e-10,py::arg("mu_prox")= 1e-7,py::arg("max_iter")= 1000,py::arg("adaptative_rho")= true, py::return_value_policy::reference_internal );
    m.def("solveLCQP", &solveLCQP, "A function which solves a QCQP, Lorentz-cone-constrained problem with regularized ADMM algorithm",py::arg("P"), py::arg("q"), py::arg("warm_start"),py::arg("epsilon")= 1e-10,py::arg("mu_prox")= 1e-7,py::arg("max_iter")= 1000,py::arg("adaptative_rho")= true, py::return_value_policy::reference_internal );
    m.def("solveDerivativesQP", &solveDerivativesQP, "A function which solves the differentiated KKT system of a QP",py::arg("P"), py::arg("q"), py::arg("l"), py::arg("grad_l"), py::arg("epsilon")=1e-10);
    m.def("solveDerivativesQCQP", &solveDerivativesQCQP, "A function which solves the differentiated KKT system of a QCQP",py::arg("P"), py::arg("q"), py::arg("l_n"),py::arg("mu"), py::arg("l"), py::arg("grad_l"), py::arg("epsilon")=1e-10 );
    m.def("solveDerivativesLCQP", &solveDerivativesLCQP, "A function which solves the differentiated KKT system of a Lorentz-cone-constrained QCQP",py::arg("P"), py::arg("q"), py::arg("l"), py::arg("grad_l"), py::arg("epsilon")=1e-10 );

}


//
// boost/radix/detail/bits.hpp
//
// Copyright (c) Chris Glover, 2017-2018
//
// 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)
//

#ifndef BOOST_RADIX_DETAIL_BITS_HPP
#define BOOST_RADIX_DETAIL_BITS_HPP

#include <boost/radix/common.hpp>

#include <boost/integer/common_factor.hpp>
#include <boost/integer/static_log2.hpp>

namespace boost { namespace radix {
namespace detail {

template <std::size_t Bits>
struct bits_lcm
{
    typedef typename boost::integer::static_lcm<Bits, 8> type;
    BOOST_STATIC_CONSTANT(std::size_t, value = type::value);
};

} // namespace detail

namespace bits {

template <std::size_t AlphabetSize>
struct from_alphabet_size
{
    BOOST_STATIC_CONSTANT(
        std::size_t, value = boost::static_log2<AlphabetSize>::value);
};

template <std::size_t Bits>
struct to_alphabet_size
{
    BOOST_STATIC_CONSTANT(
        std::size_t, value = 1 << Bits);
};

template <std::size_t Bits>
struct to_packed_segment_size
{
    BOOST_STATIC_CONSTANT(
        std::size_t, value = detail::bits_lcm<Bits>::value / 8);
};

template <std::size_t Bits>
struct to_unpacked_segment_size
{
    BOOST_STATIC_CONSTANT(
        std::size_t, value = detail::bits_lcm<Bits>::value / Bits);
};

} // namespace bits
}} // namespace boost::radix

#endif // BOOST_RADIX_DETAIL_BITS_HPP


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/
//  Based on https://github.com/sportdeath/audio_transport/
// T.Henderson and J.Solomon, Audio Transport:
// a generalized portamento via optimal transport.
// Proceedings of DAFX 2019.

#pragma once

#include "STFT.hpp"
#include "WindowFuncs.hpp"
#include "../util/FFT.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cmath>

namespace fluid {
namespace algorithm {

struct SpetralMass
{
  index  startBin;
  index  centerBin;
  index  endBin;
  double mass;
};

class AudioTransport
{
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXi = Eigen::ArrayXi;
  using ArrayXcd = Eigen::ArrayXcd;
  template <typename T>
  using Ref = Eigen::Ref<T>;
  using TransportMatrix = std::vector<std::tuple<index, index, double>>;
  template <typename T>
  using vector = std::vector<T>;

public:
  AudioTransport(index maxFFTSize)
      : mWindowSize(maxFFTSize), mFFTSize(maxFFTSize),
        mBins(maxFFTSize / 2 + 1), mFFT(maxFFTSize),
        mSTFT(maxFFTSize, maxFFTSize, maxFFTSize / 2),
        mISTFT(maxFFTSize, maxFFTSize, maxFFTSize / 2),
        mReassignSTFT(maxFFTSize, maxFFTSize, maxFFTSize / 2)
  {}

  void init(index windowSize, index fftSize, index hopSize)
  {
    mWindowSize = windowSize;
    mWindow = ArrayXd::Zero(mWindowSize);
    WindowFuncs::map()[WindowFuncs::WindowTypes::kHann](mWindowSize, mWindow);
    mWindowSquared = mWindow * mWindow;
    mFFTSize = fftSize;
    mHopSize = hopSize;
    mBins = fftSize / 2 + 1;
    mPhase = ArrayXd::Zero(mBins);
    mChanged = ArrayXi::Zero(mBins);
    mBinFreqs = ArrayXd::LinSpaced(mBins, 0, mBins - 1) * (2 * pi) / mFFTSize;
    mPhaseDiff = mBinFreqs * mHopSize;
    mSTFT = STFT(windowSize, fftSize, hopSize);
    mISTFT = ISTFT(windowSize, fftSize, hopSize);
    mReassignSTFT = STFT(windowSize, fftSize, hopSize,
                         static_cast<index>(WindowFuncs::WindowTypes::kHannD));
    mInitialized = true;
  }

  bool initialized() const { return mInitialized; }

  void processFrame(RealVectorView in1, RealVectorView in2, double weight,
                    RealMatrixView out)
  {
    using namespace _impl;
    using namespace Eigen;
    assert(mInitialized);
    ArrayXd  frame1 = asEigen<Array>(in1);
    ArrayXd  frame2 = asEigen<Array>(in2);
    ArrayXcd spectrum1(mBins);
    ArrayXcd spectrum1Dh(mBins);
    ArrayXcd spectrum2(mBins);
    ArrayXcd spectrum2Dh(mBins);
    ArrayXd  output(frame1.size());
    mSTFT.processFrame(frame1, spectrum1);
    mReassignSTFT.processFrame(frame1, spectrum1Dh);
    mSTFT.processFrame(frame2, spectrum2);
    mReassignSTFT.processFrame(frame2, spectrum2Dh);
    ArrayXcd result =
        interpolate(spectrum1, spectrum1Dh, spectrum2, spectrum2Dh, weight);
    mISTFT.processFrame(result, output);
    out.row(0) <<= asFluid(output);
    out.row(1) <<= asFluid(mWindowSquared);
  }

  vector<SpetralMass> segmentSpectrum(const Ref<ArrayXd> mag,
                                      const Ref<ArrayXd> reasignedFreq)
  {

    vector<SpetralMass> masses;
    double              totalMass = mag.sum() + epsilon;
    ArrayXi             sign = (reasignedFreq > mBinFreqs).cast<int>();
    mChanged.setZero();
    mChanged.segment(1, mBins - 1) =
        sign.segment(1, mBins - 1) - sign.segment(0, mBins - 1);
    SpetralMass currentMass{0, 0, 0, 0};
    for (index i = 1; i < mChanged.size(); i++)
    {
      if (mChanged(i) == -1)
      {
        double d1 = reasignedFreq(i - 1) - mBinFreqs(i - 1);
        double d2 = mBinFreqs(i) - reasignedFreq(i);
        currentMass.centerBin = d1 < d2 ? i - 1 : i;
      }
      if (mChanged(i) == 1)
      {
        currentMass.endBin = i;
        currentMass.mass =
            mag.segment(currentMass.startBin, i - currentMass.startBin).sum() /
            totalMass;
        masses.emplace_back(currentMass);
        currentMass = SpetralMass{i, i, i, 0};
      }
    }
    currentMass.endBin = mBins;
    currentMass.mass =
        mag.segment(currentMass.startBin, mBins - currentMass.startBin).sum() /
        totalMass;
    masses.emplace_back(currentMass);
    return masses;
  }

  TransportMatrix computeTransportMatrix(std::vector<SpetralMass> m1,
                                         std::vector<SpetralMass> m2)
  {
    TransportMatrix matrix;
    index           index1 = 0, index2 = 0;
    double          mass1 = m1[0].mass;
    double          mass2 = m2[0].mass;
    while (true)
    {
      if (mass1 < mass2)
      {
        matrix.emplace_back(index1, index2, mass1);
        mass2 -= mass1;
        index1++;
        if (index1 >= asSigned(m1.size())) break;
        mass1 = m1[asUnsigned(index1)].mass;
      }
      else
      {
        matrix.emplace_back(index1, index2, mass2);
        mass1 -= mass2;
        index2++;
        if (index2 >= asSigned(m2.size())) break;
        mass2 = m2[asUnsigned(index2)].mass;
      }
    }
    return matrix;
  }

  void placeMass(const SpetralMass mass, index bin, double scale,
                 double centerPhase, Ref<ArrayXcd> input, Ref<ArrayXcd> output,
                 double nextPhase, Ref<ArrayXd> amplitudes, Ref<ArrayXd> phases)
  {
    double phaseShift = centerPhase - std::arg(input(mass.centerBin));
    for (index i = mass.startBin; i < mass.endBin; i++)
    {
      index pos = i + bin - mass.centerBin;
      if (pos < 0 || pos >= output.size()) continue;
      double phase = phaseShift + std::arg(input(i));
      double mag = scale * std::abs(input(i));
      output(pos) += std::polar(mag, phase);
      if (mag > amplitudes(pos))
      {
        amplitudes(pos) = mag;
        phases(pos) = nextPhase;
      }
    }
  }

  ArrayXcd interpolate(Ref<ArrayXcd> in1, Ref<ArrayXcd> in1Dh,
                       Ref<ArrayXcd> in2, Ref<ArrayXcd> in2Dh,
                       double interpolation)
  {
    ArrayXd  mag1 = in1.abs().real();
    ArrayXd  mag2 = in2.abs().real();
    ArrayXcd result = ArrayXcd::Zero(mBins);
    double   mag1Sum = mag1.sum();
    double   mag2Sum = mag2.sum();
    if (mag1Sum <= 0 && mag2Sum <= 0) { return result; }
    else if (mag1Sum > 0 && mag2Sum <= 0)
    {
      return in1;
    }
    else if (mag1Sum <= 0 && mag2Sum > 0)
    {
      return in2;
    }
    ArrayXd                  phase1 = in1.arg().real();
    ArrayXd                  phase2 = in2.arg().real();
    ArrayXd                  reasignedW1 = mBinFreqs - (in1Dh / in1).imag();
    ArrayXd                  reasignedW2 = mBinFreqs - (in2Dh / in2).imag();
    ArrayXd                  newAmplitudes = ArrayXd::Zero(mBins);
    ArrayXd                  newPhases = ArrayXd::Zero(mBins);
    std::vector<SpetralMass> s1 = segmentSpectrum(mag1, reasignedW1);
    std::vector<SpetralMass> s2 = segmentSpectrum(mag2, reasignedW2);
    if (s1.size() == 0 || s2.size() == 0) { return result; }

    TransportMatrix matrix = computeTransportMatrix(s1, s2);
    for (auto t : matrix)
    {
      SpetralMass m1 = s1[asUnsigned(std::get<0>(t))];
      SpetralMass m2 = s2[asUnsigned(std::get<1>(t))];
      index  interpolatedBin = std::lrint((1 - interpolation) * m1.centerBin +
                                         interpolation * m2.centerBin);
      double interpolationFactor = interpolation;
      if (m1.centerBin != m2.centerBin)
      {
        interpolationFactor =
            ((double) interpolatedBin - (double) m1.centerBin) /
            ((double) m2.centerBin - (double) m1.centerBin);
      }
      double interpolatedFreq =
          (1 - interpolationFactor) * reasignedW1(m1.centerBin) +
          interpolationFactor * reasignedW2(m2.centerBin);
      double nextPhase = mPhase(interpolatedBin) + interpolatedFreq * mHopSize;
      double centerPhase = nextPhase - mPhaseDiff(interpolatedBin);
      placeMass(m1, interpolatedBin,
                (1 - interpolation) * std::get<2>(t) / m1.mass, centerPhase,
                in1, result, nextPhase, newAmplitudes, newPhases);
      placeMass(m2, interpolatedBin, interpolation * std::get<2>(t) / m2.mass,
                centerPhase, in2, result, nextPhase, newAmplitudes, newPhases);
    }
    mPhase = newPhases;
    return result;
  }

  index   mWindowSize{1024};
  index   mHopSize{512};
  ArrayXd mBinFreqs;
  ArrayXd mWindow;
  ArrayXd mWindowSquared;
  index   mFFTSize{1024};
  index   mBins{513};
  FFT     mFFT;
  bool    mInitialized{false};
  ArrayXd mPhase;
  ArrayXd mPhaseDiff;
  ArrayXi mChanged;
  STFT    mSTFT;
  ISTFT   mISTFT;
  STFT    mReassignSTFT;
};
} // namespace algorithm
} // namespace fluid


//
// Created by przestaw on 16.12.2020.
//
#include <boost/test/unit_test.hpp>
#include <gpuclassifier/GPUCalculateClassP.hpp>

BOOST_AUTO_TEST_SUITE(nbc4gpu_TestSuite)

BOOST_AUTO_TEST_SUITE(ClassPropability_TestSuite)

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size0) {
  boost::compute::device device = boost::compute::system::default_device();
  boost::compute::context context(device);
  boost::compute::command_queue queue(context, device);

  nbc4gpu::GPUCalculateClassP<float>::Statistics stats = {};
  BOOST_CHECK_THROW((nbc4gpu::GPUCalculateClassP<float>(stats, 1, queue)),
                    nbc4gpu::error::ZeroValuesProvided);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_MismatchSize) {
  boost::compute::device device = boost::compute::system::default_device();
  boost::compute::context context(device);
  boost::compute::command_queue queue(context, device);

  nbc4gpu::GPUCalculateClassP<float>::Statistics stats = {{1, 1}};
  nbc4gpu::GPUCalculateClassP<float> classifier(stats, 0, queue);
  BOOST_CHECK_THROW(classifier({1, 2, 3, 4}), nbc4gpu::error::MismatchedSize);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size1_1) {
  boost::compute::device device = boost::compute::system::default_device();
  boost::compute::context context(device);
  boost::compute::command_queue queue(context, device);

  nbc4gpu::GPUCalculateClassP<float>::Statistics stats = {{1, 1}};
  nbc4gpu::GPUCalculateClassP<float> classifier(stats, 0, queue);
  // P = 1/(sqr(2*pi) * sqr(var)) * exp(-((x-avg)^2 / 2*var))

  // P = 1/(sqr(2*pi) * 1) * exp(-((1-1)^2 / 2*1)) = 0.3989... * exp(0)  =
  // 0.3989
  BOOST_CHECK_CLOSE(classifier.operator()({1}), 0.3989, 0.1);

  BOOST_CHECK_CLOSE(classifier.operator()({0}), 0.2419, 0.1);

  BOOST_CHECK_CLOSE(classifier.operator()({2}), 0.2419, 0.1);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size1_2) {
  boost::compute::device device = boost::compute::system::default_device();
  boost::compute::context context(device);
  boost::compute::command_queue queue(context, device);

  nbc4gpu::GPUCalculateClassP<float>::Statistics stats = {{2, 5}};
  nbc4gpu::GPUCalculateClassP<float> classifier(stats, 0, queue);

  BOOST_CHECK_CLOSE(classifier.operator()({1}), 0.161449, 0.1);

  BOOST_CHECK_CLOSE(classifier.operator()({2}), 0.178429, 0.1);

  BOOST_CHECK_CLOSE(classifier.operator()({0}), 0.119605, 0.1);

  BOOST_CHECK_CLOSE(classifier.operator()({3}), 0.161449, 0.1);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size4_1) {
  boost::compute::device device = boost::compute::system::default_device();
  boost::compute::context context(device);
  boost::compute::command_queue queue(context, device);

  nbc4gpu::GPUCalculateClassP<float>::Statistics stats = {
      {2, 1}, {2, 1}, {2, 1}, {2, 1}};
  nbc4gpu::GPUCalculateClassP<float> classifier(stats, 0, queue);

  BOOST_CHECK_CLOSE(classifier.operator()({1, 0, 2, 3}), 0.001261597, 0.1);
  BOOST_CHECK_CLOSE(classifier.operator()({0, 1, 2, 3}), 0.001261597, 0.1);
  BOOST_CHECK_CLOSE(classifier.operator()({3, 1, 0, 2}), 0.001261597, 0.1);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size4_2) {
  boost::compute::device device = boost::compute::system::default_device();
  boost::compute::context context(device);
  boost::compute::command_queue queue(context, device);

  nbc4gpu::GPUCalculateClassP<float>::Statistics stats = {
      {2, 5}, {2, 3}, {0.15, 4}, {3.5, 1.5}};
  nbc4gpu::GPUCalculateClassP<float> classifier(stats, 0, queue);

  BOOST_CHECK_CLOSE(classifier.operator()({1, 2, 0, 3}), 0.002217, 0.1);
  BOOST_CHECK_CLOSE(classifier.operator()({2.5, 3, 0.5, 4}), 0.001998, 0.1);
  BOOST_CHECK_CLOSE(classifier.operator()({1, 1, 1, 1}), 0.00023274, 0.1);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size0_double) {
  boost::compute::device device = boost::compute::system::default_device();
  boost::compute::context context(device);
  boost::compute::command_queue queue(context, device);

  nbc4gpu::GPUCalculateClassP<double>::Statistics stats = {};
  BOOST_CHECK_THROW((nbc4gpu::GPUCalculateClassP<double>(stats, 1.0, queue)),
                    nbc4gpu::error::ZeroValuesProvided);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_MismatchSize_double) {
  boost::compute::device device = boost::compute::system::default_device();
  boost::compute::context context(device);
  boost::compute::command_queue queue(context, device);

  nbc4gpu::GPUCalculateClassP<double>::Statistics stats = {{1, 1}};
  nbc4gpu::GPUCalculateClassP<double> classifier(stats, 0, queue);
  BOOST_CHECK_THROW(classifier({1, 2, 3, 4}), nbc4gpu::error::MismatchedSize);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size1_1_double) {
  boost::compute::device device = boost::compute::system::default_device();
  boost::compute::context context(device);
  boost::compute::command_queue queue(context, device);

  nbc4gpu::GPUCalculateClassP<double>::Statistics stats = {{1, 1}};
  nbc4gpu::GPUCalculateClassP<double> classifier(stats, 0, queue);
  // P = 1/(sqr(2*pi) * sqr(var)) * exp(-((x-avg)^2 / 2*var))

  // P = 1/(sqr(2*pi) * 1) * exp(-((1-1)^2 / 2*1)) = 0.3989... * exp(0)  =
  // 0.3989
  BOOST_CHECK_CLOSE(classifier.operator()({1}), 0.3989, 0.1);

  BOOST_CHECK_CLOSE(classifier.operator()({0}), 0.2419, 0.1);

  BOOST_CHECK_CLOSE(classifier.operator()({2}), 0.2419, 0.1);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size1_2_double) {
  boost::compute::device device = boost::compute::system::default_device();
  boost::compute::context context(device);
  boost::compute::command_queue queue(context, device);

  nbc4gpu::GPUCalculateClassP<double>::Statistics stats = {{2, 5}};
  nbc4gpu::GPUCalculateClassP<double> classifier(stats, 0, queue);

  BOOST_CHECK_CLOSE(classifier.operator()({1}), 0.161449, 0.1);

  BOOST_CHECK_CLOSE(classifier.operator()({2}), 0.178429, 0.1);

  BOOST_CHECK_CLOSE(classifier.operator()({0}), 0.119605, 0.1);

  BOOST_CHECK_CLOSE(classifier.operator()({3}), 0.161449, 0.1);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size4_1_double) {
  boost::compute::device device = boost::compute::system::default_device();
  boost::compute::context context(device);
  boost::compute::command_queue queue(context, device);

  nbc4gpu::GPUCalculateClassP<double>::Statistics stats = {
      {2, 1}, {2, 1}, {2, 1}, {2, 1}};
  nbc4gpu::GPUCalculateClassP<double> classifier(stats, 0, queue);

  BOOST_CHECK_CLOSE(classifier.operator()({1, 0, 2, 3}), 0.001261597, 0.1);
  BOOST_CHECK_CLOSE(classifier.operator()({0, 1, 2, 3}), 0.001261597, 0.1);
  BOOST_CHECK_CLOSE(classifier.operator()({3, 1, 0, 2}), 0.001261597, 0.1);
}

BOOST_AUTO_TEST_CASE(PrecalulatedTest_Size4_2_double) {
  boost::compute::device device = boost::compute::system::default_device();
  boost::compute::context context(device);
  boost::compute::command_queue queue(context, device);

  nbc4gpu::GPUCalculateClassP<double>::Statistics stats = {
      {2, 5}, {2, 3}, {0.15, 4}, {3.5, 1.5}};
  nbc4gpu::GPUCalculateClassP<double> classifier(stats, 0, queue);

  BOOST_CHECK_CLOSE(classifier.operator()({1, 2, 0, 3}), 0.002217, 0.1);
  BOOST_CHECK_CLOSE(classifier.operator()({2.5, 3, 0.5, 4}), 0.001998, 0.1);
  BOOST_CHECK_CLOSE(classifier.operator()({1, 1, 1, 1}), 0.00023274, 0.1);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>
#include "bayesian/graph.hpp"
#include "bayesian/inference/rejection_sampling.hpp"

BOOST_AUTO_TEST_CASE( rejection_sampling_standard )
{
    bn::graph_t graph;
    auto const vertex_1 = graph.add_vertex();
    auto const vertex_2 = graph.add_vertex();
    auto const vertex_3 = graph.add_vertex();
    auto const vertex_4 = graph.add_vertex();
    auto const vertex_5 = graph.add_vertex();
    auto edge_12 = graph.add_edge(vertex_1, vertex_2);
    auto edge_13 = graph.add_edge(vertex_1, vertex_3);
    auto edge_24 = graph.add_edge(vertex_2, vertex_4);
    auto edge_25 = graph.add_edge(vertex_2, vertex_5);
    auto edge_35 = graph.add_edge(vertex_3, vertex_5);

    {
        vertex_1->selectable_num = 2;
        vertex_1->cpt.assign({}, vertex_1);

        bn::condition_t const cond;
        vertex_1->cpt[cond].second = {0.5, 0.5};
    }
    {
        vertex_2->selectable_num = 2;
        vertex_2->cpt.assign({vertex_1}, vertex_2);

        bn::condition_t const cond_0 = {{vertex_1, 0}};
        bn::condition_t const cond_1 = {{vertex_1, 1}};
        vertex_2->cpt[cond_0].second = {0.8, 0.2};
        vertex_2->cpt[cond_1].second = {0.1, 0.9};
    }
    {
        vertex_3->selectable_num = 2;
        vertex_3->cpt.assign({vertex_1}, vertex_3);

        bn::condition_t const cond_0 = {{vertex_1, 0}};
        bn::condition_t const cond_1 = {{vertex_1, 1}};
        vertex_3->cpt[cond_0].second = {0.7, 0.3};
        vertex_3->cpt[cond_1].second = {0.4, 0.6};
    }
    {
        vertex_4->selectable_num = 2;
        vertex_4->cpt.assign({vertex_2}, vertex_4);

        bn::condition_t const cond_0 = {{vertex_2, 0}};
        bn::condition_t const cond_1 = {{vertex_2, 1}};
        vertex_4->cpt[cond_0].second = {0.6, 0.4};
        vertex_4->cpt[cond_1].second = {0.1, 0.9};
    }
    {
        vertex_5->selectable_num = 2;
        vertex_5->cpt.assign({vertex_2, vertex_3}, vertex_5);

        bn::condition_t const cond_00 = {{vertex_2, 0}, {vertex_3, 0}};
        bn::condition_t const cond_01 = {{vertex_2, 0}, {vertex_3, 1}};
        bn::condition_t const cond_10 = {{vertex_2, 1}, {vertex_3, 0}};
        bn::condition_t const cond_11 = {{vertex_2, 1}, {vertex_3, 1}};
        vertex_5->cpt[cond_00].second = {0.1, 0.9};
        vertex_5->cpt[cond_01].second = {0.2, 0.8};
        vertex_5->cpt[cond_10].second = {0.3, 0.7};
        vertex_5->cpt[cond_11].second = {0.4, 0.6};
    }

    bn::inference::rejection_sampling func(graph);
    auto const result = func({{vertex_4,1}, {vertex_1, 0}});

    BOOST_CHECK_CLOSE(result.at(vertex_2)[0][0], 0.62, 10);
    BOOST_CHECK_CLOSE(result.at(vertex_2)[0][1], 0.38, 10);
}


#include <stdlib.h>
#include <vector>
#include <stdint.h>
#include <iostream>
//#include <boost/range.hpp>
#include <algorithm>
#include <boost/range/irange.hpp>
//#include <boost/range/algorithm.hpp>
#include <boost/range/adaptors.hpp>
#include <random>
#include <qs.hpp>
#include <stack>
#include "PredictableRandomGenerator.hpp"

void printList( const std::vector< int64_t > & list, const char * text = "" )
{
    std::cout << text;

    for( int64_t i = 0; i < list.size(); ++i )
    {
        std::cout << list[i] << " ";
    }

    printf( "\n\n" );
}

void printList( const std::vector< int64_t > & list, const std::vector< int64_t > & pivots, const int64_t to_swap, const char * text = "" )
{
    std::cout << text;

    for( int64_t i = 0; i < list.size(); ++i )
    {
        auto iter = std::find( std::begin(pivots), std::end(pivots), i);
        if( iter != std::end(pivots) )
        {
            std::cout << "[" << list[i] << "],";
        }
        else if( i == to_swap )
        {
            std::cout << "{" << list[i] << "},";
        }
        else
            std::cout << list[i] << ",";
    }

    printf( "\n\n" );
}

void printPivots( const std::vector< int64_t > & arr, const std::vector< int64_t > & pivots )
{
    std::cout << "PivotValues=";

    for( auto & pivot : pivots ) {
        std::cout << arr[pivot] << ",";
    }

    std::cout << "\n\n";
}

int64_t Number_of_comparisons = 0;
int64_t Number_of_swaps = 0;

inline std::vector<int64_t> sort_pivots( std::vector<int64_t> & arr, int64_t start, int64_t end, std::vector<int64_t> && pivots )
{
    std::sort( std::begin( pivots ), std::end( pivots ) );

    std::vector<int64_t> new_pivots;
    new_pivots.reserve( pivots.size() );

    int64_t first_pivot = start;
    for( auto p : pivots )
    {
        new_pivots.push_back( first_pivot );

        std::swap( arr[p], arr[first_pivot]);

        ++first_pivot;
    }

    Number_of_swaps += first_pivot - start;

    auto iter = std::begin( arr ) + start;

    std::sort( iter, iter + pivots.size() );

    return new_pivots;
}

static std::random_device rd; // obtain a random number from hardware
static std::mt19937 gen(rd()); // seed the generator

inline std::vector< int64_t > get_pivot( std::vector< int64_t > & arr, int64_t l, int64_t r, int64_t n )
{
    RandomNumberStream prg(n);

    //std::uniform_int_distribution<> distr(l, r); // define the range

    if( abs(r-l) < n ) {
        n = round( (abs(r-l)) / 2 );
    }

    std::vector< int64_t > pivots;
    pivots.reserve(n);

    for( int64_t i = 0; i < n; ++i )
    {
        while( true ) {
            auto new_pivot = prg.RandomInteger(l,r);//distr(gen);
            auto iter = std::find( std::begin( pivots ), std::end( pivots ), new_pivot );

            if( iter != std::end( pivots ) ) continue;

            pivots.push_back( new_pivot );
            break;
        }
    }

    // sorting part
    pivots = sort_pivots( arr, l, r, std::move(pivots) );

    //printPivots( arr, pivots );

    return pivots;
}

inline std::vector< bool > less_or_equal( const std::vector< int64_t > & arr, const std::vector<int64_t> & pivots, const int64_t value )
{
    std::vector< bool > output( pivots.size() + 1, true );

    int64_t index = 0;

    for( const auto p : pivots ) {
        const auto res = arr[p] >= value;

        if( res ) break;

        output[index++] = res;
    }

    Number_of_comparisons += index;

    return output;
}

inline std::vector< bool > greater( const std::vector< int64_t > & arr, const std::vector<int64_t> & pivots, const int64_t value )
{
    std::vector< bool > output( pivots.size() + 1, true );

    int64_t index = pivots.size();

    for( const auto p : pivots | boost::adaptors::reversed ) {
        const auto res = arr[p] < value;

        // since all the pivot values are sorted, thus if value greater
        // then last pivot implies that for the remaining pivots it also greater
        if( res ) break;

        output[index--] = res;
    }

    Number_of_comparisons += pivots.size() - index;

    return output;
}

// TODO: cover with test
inline int64_t identify_new_sector( const std::vector< int64_t > & arr, const std::vector<int64_t> & pivots, const int64_t value )
{
    const auto lq = less_or_equal( arr, pivots, value );
    const auto gt = greater( arr, pivots, value );

    int64_t index = 0;

    for( const auto l : lq | boost::adaptors::indexed(0) )
    {
        const auto idx = l.index();

        if( l.value() && gt[index] )
        {
            index = idx;
            break;
        }
    }

    return index;
}

std::vector<int64_t> general_partition( std::vector<int64_t> & arr, int64_t l, int64_t r, std::vector<int64_t> & pivots )
{
    for( const auto i : boost::irange<int64_t>( l + pivots.size()-1, r+1 ) )
    {
        const auto iter = std::find( std::begin( pivots ), std::end( pivots ), i );

        if( iter != std::end( pivots ) )
            continue;

        const auto new_sector = identify_new_sector( arr, pivots, arr[i] );

        // check if value already in right sector
        if( new_sector == pivots.size() )
            continue;

        int64_t new_i = i;
        for( const auto sector : boost::irange<int64_t>( new_sector, pivots.size() ) | boost::adaptors::reversed )
        {
            auto & pivot( pivots[sector] );

            if( new_i > sector+1 )
            {
                std::swap( arr[ pivot + 1 ], arr[new_i] );
                Number_of_swaps++;
            }

            std::swap( arr[ pivot ], arr[ pivot + 1 ] );
            Number_of_swaps++;

            new_i = pivot;
            pivot += 1;
        }
    }

    return std::move( pivots );
}

inline int64_t find_value_sector( const std::vector<int64_t> & pivots, int64_t value_position )
{
    auto pivot_iter = std::find_if( std::begin(pivots), std::end(pivots), [value_position]( const auto & pivot )
        {
            return pivot > value_position;
        } );

    return pivot_iter - std::begin(pivots) + ( pivot_iter == std::end(pivots) ? 1 : 0 );
}

inline void sort_pivots2( std::vector<int64_t> & arr, int64_t start, int64_t end, std::vector<int64_t> & pivots )
{
    std::sort( std::begin( pivots ), std::end( pivots ) );

    std::vector< int64_t > values;
    values.reserve( pivots.size() );

    for( const auto p : pivots ) {
        values.push_back( arr[p] );
    }

    std::sort( std::begin( values ), std::end( values ) );

    for( auto entry : values | boost::adaptors::indexed(0) )
    {
        arr[pivots[entry.index()]] = entry.value();
    }
}

inline std::vector< int64_t > get_pivot2( std::vector< int64_t > & arr, int64_t l, int64_t r, int64_t n )
{
    std::uniform_int_distribution<> distr(l, r); // define the range

    if( abs(r-l) < n ) {
        //std::cout << "Changed " << n << " to " << abs(r-l) << std::endl;
        n = round( (abs(r-l)) / 2 );
    }

    std::vector< int64_t > pivots;
    pivots.reserve(n);

    for( int64_t i = 0; i < n; ++i )
    {
        while( true ) {
            auto new_pivot = distr(gen);

            auto iter = std::find( std::begin( pivots ), std::end( pivots ), new_pivot );
            if( iter != std::end( pivots ) ) continue;

            auto iter2 = std::find( std::begin( pivots ), std::end( pivots ), new_pivot+1 );
            if( iter2 != std::end( pivots ) ) continue;

            auto iter3 = std::find( std::begin( pivots ), std::end( pivots ), new_pivot-1 );
            if( iter3 != std::end( pivots ) ) continue;

            pivots.push_back( new_pivot );
            break;
        }
    }

    // sorting part
    sort_pivots2( arr, l, r, pivots );

    return pivots;
}

std::vector<int64_t> general_partition2( std::vector<int64_t> & arr, int64_t l, int64_t r, std::vector<int64_t> & pivots )
{
    int64_t i = l;
    int64_t pivot_boundary = 0;
    int64_t pivot_id = 0;
    int64_t after_pivot = -1;
    int64_t prev_pivot = -1;

    for( auto & pivot : pivots ) {

        const auto pivot_value = arr[pivot];

        while( i < pivot )
        {
            if( arr[i] > pivot_value )
            {
                if( pivot-1 > pivot_boundary )
                {
                    std::swap( arr[pivot], arr[pivot-1] );
                    std::swap( arr[i], arr[pivot] );
                    // updating pivot
                    pivot -= 1;
                }
                else
                {
                    std::swap( arr[i], arr[pivot] );
                    // updating pivot
                    pivot -= 1;
                }
            }
            else if( pivot_id != 0 )
            {
                std::swap( arr[after_pivot++], arr[i] );
            }

            ++i;
        }

        if( pivot_id != 0 )
        {
            std::swap( arr[prev_pivot], arr[after_pivot-1] );
            pivots[pivot_id] = after_pivot-1;
        }

        prev_pivot = pivot;

        after_pivot = pivot+1;
        pivot_boundary = pivot+1;
        pivot_id += 1;
    }

    return std::move( pivots );
}

void __quicksort( std::vector< int64_t > & arr, int64_t l, int64_t r, int64_t num_pivots )
{
    if( l >= r ) return;

    if( l + 1 == r )
    {
        if( arr[r] < arr[l] )
            std::swap(arr[r], arr[l]);

        return;
    }

    if( abs(r-l) <= 10 ) {
        std::sort( std::begin( arr )+l, std::begin( arr )+r+1 );
        return;
    }

    auto pivots = get_pivot( arr, l, r, num_pivots );

    pivots = general_partition( arr, l, r, pivots );

    for( const auto pivot : pivots )
    {
        __quicksort(arr, l, pivot - 1, num_pivots);
        l = pivot + 1;
    }

    __quicksort(arr, pivots.back()+1, r, num_pivots);
}

struct stack_node {
    int64_t l;
    int64_t r;
};

std::pair<int64_t,int64_t> quicksort( std::vector< int64_t > & arr, int64_t num_pivots )
{
    Number_of_comparisons = 0;
    Number_of_swaps = 0;

    std::stack< stack_node > stck;
    stck.push( stack_node{0, static_cast<int64_t>(arr.size() - 1) } );

    while( !stck.empty() )
    {
        auto sn = stck.top();
        stck.pop();

        int64_t l = sn.l;
        int64_t r = sn.r;

        if( abs(r-l) <= 10 ) {
            std::sort( std::begin( arr )+l, std::begin( arr )+r+1 );
            continue;
        }

        auto pivots = get_pivot( arr, l, r, num_pivots );
        pivots = general_partition( arr, l, r, pivots );

        for( const auto pivot : pivots )
        {
            stck.push( stack_node{l, pivot - 1} );

            l = pivot + 1;
        }

        stck.push( stack_node{pivots.back()+1, r} );
    }

    return std::make_pair( Number_of_comparisons, Number_of_swaps );
}

std::vector<int64_t> general_partition3( std::vector<int64_t> & arr, int64_t l, int64_t r, std::vector<int64_t> & pivots )
{
    int64_t current_pivot_index = 0;

    for( auto & pivot : pivots )
    {
        const auto pivot_value = arr[pivot];

        int64_t low_boundary = l;

        auto start_position = ( pivot-16 <= low_boundary ? low_boundary : pivot-16 );

        int64_t new_left_pivot_position = -1;

        // checking elements on left side of the pivot
        for( int64_t i = start_position; i < pivot; ++i )
        {
            if( arr[i] > pivot_value && new_left_pivot_position == -1 )
                new_left_pivot_position = i;
            else
                new_left_pivot_position = -1;
        }

        int64_t high_boundary = r;
        if( current_pivot_index+1 < pivots.size() )
        {
            high_boundary = pivots[current_pivot_index+1];
        }

        auto end_position = ( pivot+16 >= high_boundary ? high_boundary : pivot+16 );

        int64_t new_right_pivot_position = -1;

        // checking elements on left side of the pivot
        for( int64_t i = pivot+1; i < end_position; ++i )
        {
            if( arr[i] <= pivot_value && new_right_pivot_position == -1 )
                new_right_pivot_position = i;
            else
                new_right_pivot_position = -1;
        }

        if( new_left_pivot_position >= new_right_pivot_position && new_left_pivot_position != -1 )
        {
            std::swap(arr[new_left_pivot_position], arr[pivot]);
            pivot = new_left_pivot_position;
        }
        else if( new_right_pivot_position != -1 ){
            std::swap(arr[new_right_pivot_position], arr[pivot]);
            pivot = new_left_pivot_position;
        }

        ++current_pivot_index;

        printPivots( arr, pivots );
        //printList( arr, pivots );
    }

    return std::move( pivots );
}

// void quicksort( std::vector< int64_t > & arr, int64_t num_pivots )
// {
//     if( arr.size() <= 1 )
//         return;

//     __quicksort(arr, 0, arr.size() - 1, num_pivots );
// }

/* TODO:
 Problems:
 1. unknown crash on linux
 2. implementation is slow as hell

 Tasks:
 1. Support visual studio
 2. Test coverage - DONE
 2.1. I will set up project - DONE
 3. Fix bug on linux - DONE
 4. Improve performance( mainly for paper ) - PARTIALLY DONE
 5. Implement counting of number swaps and comparisons
 6. Vargring to check all the cache misses
 7. Maybe dynamic pivoting would be interesting

 Aral TODO: Tasks - [1, 4 ]
 Rustam TODO: Tasks - [2.1, 3, 4 ]

 */


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

#include <iostream>
#include <Eigen/Eigen>

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
Matrix<float,2,3> m = Matrix<float,2,3>::Random();
Matrix2f y = Matrix2f::Random();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Here is the matrix y:" << endl << y << endl;
Matrix<float,3,2> x = m.fullPivLu().solve(y);
if((m*x).isApprox(y))
{
  cout << "Here is a solution x to the equation mx=y:" << endl << x << endl;
}
else
  cout << "The equation mx=y does not have any solution." << endl;

}
  return 0;
}


#pragma once

#include <Eigen/Sparse>

#include <crest/util/eigen_extensions.hpp>

namespace crest
{

    enum class Norm
    {
        L2,
        H1Semi,
        H1
    };

    template <typename Scalar>
    struct Assembly
    {
        Eigen::SparseMatrix<Scalar> stiffness;
        Eigen::SparseMatrix<Scalar> mass;
    };

    template <typename Scalar, typename Impl>
    class Basis
    {
    public:
        virtual ~Basis() {}

        virtual std::vector<int> interior_nodes() const = 0;
        virtual std::vector<int> boundary_nodes() const = 0;

        virtual Assembly<Scalar> assemble() const = 0;

        /**
         * The number of degrees of freedom associated with this basis.
         * @return
         */
        virtual int num_dof() const = 0;

        /**
         * Given a continuous function f, interpolate it in the space
         * represented by this basis, and return a vector of weights, such that
         * element i in the vector corresponds to the weight factor of basis function i.
         *
         * Naturally, there are multiple ways to interpolate a function in a finite
         * element space, so to have a single function for this is a simplifaction,
         * and for now we'll leave it up to the concrete implementation which
         * interpolation this corresponds to.
         *
         * @param f
         * @return
         */
        template <typename Function2d>
        VectorX<Scalar> interpolate(const Function2d &f) const
        {
            return static_cast<const Impl *>(this)->template interpolate<Function2d>(f);
        }

        /*
         * Given a continuous function g, evaluate the function at each boundary node
         * and return a vector of weights.
         */
        template <typename Function2d>
        VectorX<Scalar> interpolate_boundary(const Function2d &f) const
        {
            return static_cast<const Impl *>(this)->template interpolate_boundary<Function2d>(f);
        }

        /**
         * Computes the L2 inner product of a continuous function f
         * and every basis function b_i for every degree of freedom i.
         * More precisely, returns a vector whose ith element is
         * determined by the L2 inner product (f, b_i), where b_i
         * is the basis function associated with degree of freedom i.
         *
         * This is frequently used to compute the load vector in FEM
         * applications.
         * @param weights
         * @param f
         * @return
         */
        template <int QuadStrength, typename Function2d>
        VectorX<Scalar> load(const Function2d &f) const
        {
            return static_cast<const Impl *>(this)->template load<QuadStrength, Function2d>(f);
        };

        /**
         * Computes an approximation of the error between a continuous function f
         * and a function g in the space spanned by the basis given by
         * its basis weights in the L2 norm.
         *
         * More precisely, if g = sum w_i b_i for all degrees of freedom i,
         * where w_i is given by weights(i) and b_i denotes the basis function
         * associated with the degree of freedom i, then this function computes
         *
         * ||f - g||
         *
         * in the L2 norm.
         *
         * @param f
         * @param weights
         * @param norm
         * @return
         */
        template <int QuadStrength, typename Function2d>
        Scalar error_l2(const Function2d &f, const VectorX<Scalar> & weights) const;

        /**
         * Computes an approximation of the error between a continuous function f
         * and a function g in the space spanned by the basis given by
         * its basis weights in the H1 semi-norm.
         *
         * More precisely, if g = sum w_i b_i for all degrees of freedom i,
         * where w_i is given by weights(i) and b_i denotes the basis function
         * associated with the degree of freedom i, then this function computes
         *
         * ||f - g||
         *
         * in the H1 semi-norm, which is equivalent to
         *
         * || grad(f) - grad(g) ||
         *
         * in the L2 norm. Note that one specifies the derivatives f_x and f_y
         * for the computation of the H1 semi-norm.
         *
         * @param f
         * @param weights
         * @param norm
         * @return
         */
        template <int QuadStrength, typename Function2d_x, typename Function2d_y>
        Scalar error_h1_semi(const Function2d_x & f_x,
                             const Function2d_y & f_y,
                             const VectorX<Scalar> & weights) const;

        /**
         * Computes an approximation of the error between a continuous function f
         * and a function g in the space spanned by the basis given by
         * its basis weights in the H1 norm.
         *
         * More precisely, if g = sum w_i b_i for all degrees of freedom i,
         * where w_i is given by weights(i) and b_i denotes the basis function
         * associated with the degree of freedom i, then this function computes
         *
         * ||f - g||
         *
         * in the H1 norm. Note that the computation requires f, as well as its
         * spatial derivatives f_x and f_y.
         *
         * Also note that implementers of subclasses do not need to reimplement this
         * function, as it is implemented in terms of error_l2 and error_h1_semi.
         *
         * @param f
         * @param weights
         * @param norm
         * @return
         */
        template <int QuadStrength, typename Function2d, typename Function2d_x, typename Function2d_y>
        Scalar error_h1(const Function2d & f,
                        const Function2d_x & f_x,
                        const Function2d_y & f_y,
                        const VectorX<Scalar> & weights) const;
    };

    template <typename Scalar, typename Impl>
    template <int QuadStrength, typename Function2d>
    Scalar Basis<Scalar, Impl>::error_l2(const Function2d & f, const VectorX<Scalar> & weights) const
    {
        return static_cast<const Impl *>(this)->template error_l2<QuadStrength>(f, weights);
    };

    template <typename Scalar, typename Impl>
    template <int QuadStrength, typename Function2d_x, typename Function2d_y>
    Scalar Basis<Scalar, Impl>::error_h1_semi(const Function2d_x & f_x,
                                              const Function2d_y & f_y,
                                              const VectorX<Scalar> & weights) const
    {
        return static_cast<const Impl *>(this)->template error_h1_semi<QuadStrength>(f_x, f_y, weights);
    };

    template <typename Scalar, typename Impl>
    template <int QuadStrength, typename Function2d, typename Function2d_x, typename Function2d_y>
    Scalar Basis<Scalar, Impl>::error_h1(const Function2d & f,
                                         const Function2d_x & f_x,
                                         const Function2d_y & f_y,
                                         const VectorX<Scalar> & weights) const
    {
        const auto l2 = error_l2<QuadStrength>(f, weights);
        const auto h1_semi = error_h1_semi<QuadStrength>(f_x, f_y, weights);
        return std::sqrt(l2 * l2 + h1_semi * h1_semi);
    };
}


#include <iostream>
#include <fstream>
#include <iostream>
#include <sstream>
#include <istream>
#include <string>
#include <cstdio>
#include <cstdlib>
#include <vector>
#include <map>
#include <cmath>
#include <algorithm>
#include <boost/range/adaptor/indexed.hpp>

using namespace std;

vector<vector<int>> examples;
vector<vector<int>> centroids;
int iteration = 0;
bool finished = false;

int closest_centroid(vector<int> example)
{
    map<string, int> closest = {{"id", -1}, {"distance", INT_MAX}};
    for (auto centroid : centroids | boost::adaptors::indexed(1))
    {
        long long int distance = 0;
        for (auto axis : example | boost::adaptors::indexed(1))
        {
            if (axis.index() - 1 == centroid.value().size())
                break;
            int centroid_point = centroid.value().at(axis.index() - 1);
            int axis_value = axis.value();
            int sub = centroid_point - axis_value;
            distance += pow(sub, 2);
        }
        distance = sqrt(distance);
        if (distance < closest.at("distance"))
        {
            closest.at("id") = centroid.index() - 1;
            closest.at("distance") = distance;
        }
    }
    return closest.at("id");
}

vector<int> recalculate(int centroid_id, vector<int> centroid)
{
    int total = 0;
    vector<int> new_centroid(examples.at(0).size() - 1, 0);
    for (auto example : examples)
        if (example.at(example.size() - 1) == centroid_id)
        {
            for (auto example_axis : example | boost::adaptors::indexed(1))
            {
                if (example_axis.index() - 1 == new_centroid.size())
                    break;
                new_centroid.at(example_axis.index() - 1) += example_axis.value();
            }
            total++;
        }

    if (total)
        for (auto centroid_axis : new_centroid | boost::adaptors::indexed(1))
        {
            centroid_axis.value() /= total;
            if (centroid_axis.value() != centroid.at(centroid_axis.index() - 1))
                finished = false;
        }
    return total ? new_centroid : centroid;
}

void k_means()
{
    while (!finished)
    {
        for (auto &example : examples)
            example.at(example.size() - 1) = closest_centroid(example);
        finished = true;
        for (auto centroid : centroids | boost::adaptors::indexed(1))
            centroid.value() = recalculate(centroid.index() - 1, centroid.value());
        iteration++;
    }
}

int main(int argc, char *argv[])
{
    string extracted, comma, basename(argv[1]);
    ifstream base, centroid;
    for (int i = 0; i < 1; i++)
    {
        vector<int> temp;

        base.open("bases/int_base_" + basename + ".data");
        while (!base.eof())
        {
            getline(base, extracted);
            stringstream is(extracted);
            if (extracted.size() > 0)
            {
                while (getline(is, comma, ','))
                    temp.push_back(stoi(comma));
                temp.push_back(-1);
                examples.push_back(temp);
                temp.clear();
            }
        }
        base.close();
        centroid.open("bases/int_centroid_" + basename + "_20.data");
        while (!centroid.eof())
        {
            getline(centroid, extracted);
            istringstream is(extracted);
            if (extracted.size() > 0)
            {
                while (getline(is, comma, ','))
                    temp.push_back(stoi(comma));
                centroids.push_back(temp);
                temp.clear();
            }
        }
        centroid.close();
        k_means();
        ofstream myfile("results/my_saida_" + basename + "_seq");
        myfile << "numero de iteracoes:" << iteration;
        for (auto value : examples | boost::adaptors::indexed(1))
            myfile << "id=" << value.index() - 1 << ", classe=" << value.value().at(value.value().size() - 1) << endl;
    }
    return 0;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <simd_test.hpp>
#include <boost/simd/function/sincospi.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/function/std.hpp>

namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  using p_t = bs::pack<T, N>;

  T a1[N], c[N], s[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(2*i)/N : -T(2*i)/N;
    std::tie(s[i], c[i])= bs::sincospi(a1[i]) ;
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t ss (&s[0], &s[0]+N);
  p_t cc (&c[0], &c[0]+N);
  p_t ss1, cc1;
  std::tie(ss1, cc1)= bs::sincospi(aa1) ;
  STF_ULP_EQUAL(ss1, ss, 0.5);
  STF_ULP_EQUAL(cc1, cc, 0.5);
}

STF_CASE_TPL("Check sincospi on pack" , STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

template <typename T, std::size_t N, typename Env>
void testr(Env& $)
{
  using p_t = bs::pack<T, N>;

  T a1[N], c[N], s[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = ((i%2) ? T(i) : -T(i))*T(0.25)/N;
    std::tie(s[i], c[i])= bs::restricted_(bs::sincospi)(a1[i]);
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t ss (&s[0], &s[0]+N);
  p_t cc (&c[0], &c[0]+N);
  p_t ss1, cc1;
  std::tie(ss1, cc1)= bs::restricted_(bs::sincospi)(aa1);

  STF_ULP_EQUAL(ss1, ss,0.5);
  STF_ULP_EQUAL(cc1, cc,0.5);
}

STF_CASE_TPL("Check restricted sincospi on pack" , STF_IEEE_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  testr<T, N>($);
  testr<T, N/2>($);
  testr<T, N*2>($);
}


/* Boost numeric test of the adams-bashforth-moulton steppers test file

 Copyright 2013 Karsten Ahnert
 Copyright 2013-2015 Mario Mulansky

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

// disable checked iterator warning for msvc
#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE numeric_adams_bashforth_moulton

#include <iostream>
#include <cmath>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>

#include <boost/numeric/odeint.hpp>

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

typedef double value_type;

typedef value_type state_type;

// simple time-dependent rhs, analytic solution x = 0.5*t^2
struct simple_rhs
{
    void operator()( const state_type& x , state_type &dxdt , const double t ) const
    {
        dxdt = t;
    }
};

BOOST_AUTO_TEST_SUITE( numeric_abm_time_dependent_test )

/* generic test for all adams bashforth moulton steppers */
template< class Stepper >
struct perform_abm_time_dependent_test
{
    void operator()( void )
    {
        Stepper stepper;
        const int o = stepper.order()+1; //order of the error is order of approximation + 1

        const state_type x0 = 0.0;
        state_type x1 = x0;
        double t = 0.0;
        double dt = 0.1;
        const int steps = 10;

        integrate_n_steps( boost::ref(stepper) , simple_rhs(), x1 , t , dt , steps );
        BOOST_CHECK_LT( std::abs( 0.5 - x1 ) , std::pow( dt , o ) );
    }
};

typedef mpl::vector<
    adams_bashforth_moulton< 2 , state_type > ,
    adams_bashforth_moulton< 3 , state_type > ,
    adams_bashforth_moulton< 4 , state_type > ,
    adams_bashforth_moulton< 5 , state_type > ,
    adams_bashforth_moulton< 6 , state_type > ,
    adams_bashforth_moulton< 7 , state_type > ,
    adams_bashforth_moulton< 8 , state_type >
    > adams_bashforth_moulton_steppers;

BOOST_AUTO_TEST_CASE_TEMPLATE( abm_time_dependent_test , Stepper, adams_bashforth_moulton_steppers )
{
    perform_abm_time_dependent_test< Stepper > tester;
    tester();
}

BOOST_AUTO_TEST_SUITE_END()


#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN
#include <math.h>
#include <doctest/doctest.h>
#include <Eigen/Dense>
#include <dynkin/dynkin.hpp>

#define DEG2RAD M_PI/180.0f

TEST_CASE("Single frame translation"){
    using namespace dynkin;
    Eigen::Vector3d expected;

    Frame f1 = create_frame();
    f1->position() << 1, 1, 1;
    f1->set_attitude({0,0,90*DEG2RAD});

    Transform t = transform(nullptr, f1);

    expected << 1, 1, 1;
    CHECK(t.HTM.translation().isApprox(expected));

    t = transform(f1, nullptr);

    expected << -1, 1, -1;
    CHECK(t.HTM.translation().isApprox(expected));
}

TEST_CASE("Single frame position"){
    using namespace dynkin;
    Eigen::Vector3d expected;

    Frame f1 = create_frame();
    f1->position() << 1, 1, 1;
    f1->set_attitude({90*DEG2RAD,0,90*DEG2RAD});

    Transform t = transform(nullptr, f1);

    expected << 1, 1, 1;
    CHECK(t.apply_position({0,0,0}).isApprox(expected));

    expected << 1, 2, 1;
    CHECK(t.apply_position({1,0,0}).isApprox(expected));

    expected << 1, 1, 2;
    CHECK(t.apply_position({0,1,0}).isApprox(expected));

    expected << 2, 1, 1;
    CHECK(t.apply_position({0,0,1}).isApprox(expected));
}

TEST_CASE("Single frame vector"){
    using namespace dynkin;
    Eigen::Vector3d expected;

    Frame f1 = create_frame();
    f1->position() << 1, 1, 1;
    f1->set_attitude({0,90*DEG2RAD,90*DEG2RAD});

    Transform t = transform(nullptr, f1);

    expected << 0, 0, 0;
    CHECK(t.apply_vector({0,0,0}).isApprox(expected));

    expected << 0, 0, -1;
    CHECK(t.apply_vector({1,0,0}).isApprox(expected));

    expected << -1, 0, 0;
    CHECK(t.apply_vector({0,1,0}).isApprox(expected));

    expected << 0, 1, 0;
    CHECK(t.apply_vector({0,0,1}).isApprox(expected));
}

TEST_CASE("Single frame wrench"){
    using namespace dynkin;
    Eigen::Vector6d expected, wrench;

    Frame f1 = create_frame();
    f1->position() << 1, 1, 1;
    f1->set_attitude({0,90*DEG2RAD,90*DEG2RAD});

    Transform t = transform(nullptr, f1);

    expected << 0, 0, -1, -1, 1, 0;
    wrench << 1, 0, 0, 0, 0, 0;
    CHECK(t.apply_wrench(wrench).isApprox(expected));
}

TEST_CASE("Single frame HTM inverse"){
    using namespace dynkin;

    Frame f1 = create_frame();
    f1->position() << 124,-343,-13;
    f1->set_attitude({27*DEG2RAD,49*DEG2RAD,62*DEG2RAD});

    Transform t = transform(nullptr, f1);
    Transform t_ = transform(f1, nullptr);

    CHECK(t.HTM.isApprox(t_.inverse().HTM));
}

TEST_CASE("Single frame velocity"){
    using namespace dynkin;
    Eigen::Vector6d expected;

    Frame f1 = create_frame();
    f1->position() << 1, 1, 1;
    f1->set_attitude({0,90*DEG2RAD,90*DEG2RAD});
    f1->linear_velocity() << 1, 0, 0;
    f1->angular_velocity() << 0, 0, 1;

    expected << 1, 0, 0, 0, 0, 1;
    CHECK(f1->get_twist().isApprox(expected));
}

TEST_CASE("Chained frame velocity"){
    using namespace dynkin;
    Eigen::Vector6d expected;

    Frame f1 = create_frame();
    f1->set_attitude({0,0,90*DEG2RAD});
    f1->angular_velocity() << 0, 0, 1;

    Frame f2 = f1->create_child();
    f2->position() << 1, 1, 0;
    f2->linear_velocity() << 1, 0, 0;
    f2->angular_velocity() << 1, 0, 0;

    expected << 0, 1, 0, 1, 0, 1;
    CHECK(f2->get_twist().isApprox(expected));
}

TEST_CASE("Test inertia constructor"){
    using namespace dynkin;
    using namespace dynkin::rigidbody;
    CHECK_THROWS_AS(generalized_inertia_matrix(-1, {1, 1, 1}), std::invalid_argument);
    CHECK_THROWS_AS(generalized_inertia_matrix(1, {-1, 1, 1}), std::invalid_argument);

    Eigen::Matrix6d expected = Eigen::Matrix6d::Identity();

    CHECK(generalized_inertia_matrix(1, {1,1,1}).isApprox(expected));
}

TEST_CASE("Test generalized coordinates"){
    using namespace dynkin;
    using namespace dynkin::rigidbody;

    RigidBody rb = RigidBody(generalized_inertia_matrix(1, {1,1,1}));
    rb.origin->position() << 1,2,3;
    rb.origin->set_attitude({0,1,1});
    Eigen::Vector6d pose = rb.generalized_coordinates();

    Eigen::Vector6d expected;
    expected << 1,2,3,0,1,1;

    CHECK(pose.isApprox(expected));
}

TEST_CASE("Test generalized velocities"){
    using namespace dynkin;
    using namespace dynkin::rigidbody;

    RigidBody rb = RigidBody(generalized_inertia_matrix(1, {1,1,1}));
    rb.origin->position() << 1,1,1;
    rb.origin->set_attitude({M_PI_2, 0, M_PI_2});
    rb.origin->linear_velocity() << 1, 2, 3;
    rb.origin->angular_velocity() << 0, 0, 1;
    Eigen::Vector6d vel = rb.generalized_velocities();

    Eigen::Vector6d expected;
    expected << 3, 1, 2, 0, -1, 0;

    CHECK(vel.isApprox(expected));
}

TEST_CASE("Test Coriolis-Centripetal acceleration with CoG offset"){
    using namespace dynkin;
    using namespace dynkin::rigidbody;

    RigidBody rb = RigidBody(
        generalized_inertia_matrix(1, {1,1,1}),
        {1, 0, 0}
    );
    rb.origin->angular_velocity() << 0, 0, 1;

    Eigen::Vector6d expected;
    expected << 1, 0, 0, 0, 0, 0;

    CHECK_EQ(rb.acceleration(Eigen::Vector6d::Zero()), expected);
}

TEST_CASE("Test Coriolis-Centripetal acceleration due to linear velocity"){
    using namespace dynkin;
    using namespace dynkin::rigidbody;

    RigidBody rb = RigidBody(generalized_inertia_matrix(1, {1,1,1}));
    rb.origin->linear_velocity() << 1, 0, 0;
    rb.origin->angular_velocity() << 0, 0, 1;

    Eigen::Vector6d expected;
    expected << 0, -1, 0, 0, 0, 0;

    CHECK_EQ(rb.acceleration(Eigen::Vector6d::Zero()), expected);
}

TEST_CASE("Test acceleration -> wrench"){
    using namespace dynkin;
    using namespace dynkin::rigidbody;

    RigidBody rb = RigidBody(generalized_inertia_matrix(1, {1,1,1}));
    Eigen::Vector6d wrench = Eigen::Vector6d::Ones();
    Eigen::Vector6d acc = rb.acceleration(wrench);

    CHECK_EQ(acc, Eigen::Vector6d::Ones());

    rb.CoG->position() << 1, 0, 0;
    acc = rb.acceleration(wrench);
    Eigen::Vector6d expected;
    expected << 1, 1, 3, 1, 2, 0;

    CHECK_EQ(acc, expected);

    Eigen::Vector6d f = rb.wrench(acc);
    CHECK_EQ(wrench, f);

}

TEST_CASE("Test wrench -> acceleration"){
    using namespace dynkin;
    using namespace dynkin::rigidbody;

    RigidBody rb = RigidBody(generalized_inertia_matrix(1, {1,1,1}));
    Eigen::Vector6d acc = Eigen::Vector6d::Ones();
    Eigen::Vector6d wrench = rb.wrench(acc);

    CHECK_EQ(wrench, Eigen::Vector6d::Ones());

    rb.CoG->position() << 1, 0, 0;
    wrench = rb.wrench(acc);
    Eigen::Vector6d expected;
    expected << 1, 2, 0, 1, 1, 3;

    CHECK_EQ(wrench, expected);

    Eigen::Vector6d a = rb.acceleration(wrench);
    CHECK_EQ(acc, a);

}


#include "generate_edges.hpp"
#include "partial_shuffle.hpp"
#include <algorithm>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <random>
#include <unordered_set>

namespace {

struct indexed_vec2 {
    indexed_vec2(size_t index, const sssp::vec2& pos) : index(index), x(pos.x), y(pos.y) {}
    size_t index;
    double x;
    double y;
};

} // namespace

BOOST_GEOMETRY_REGISTER_POINT_2D(indexed_vec2, double, boost::geometry::cs::cartesian, x, y)

void sssp::generate_uniform_edges(int seed,
                                  double edge_probability,
                                  const edge_cost_fn& edge_cost,
                                  graph& graph,
                                  const node_map<vec2>& positions) {
    std::mt19937 rng(seed);

#if 0
    // This is the naive n*(n-1) steps algorithm. Each possible edge is considered
    // using a simple Bernulli(p) distribution.
    std::bernoulli_distribution allow_edge(edge_probability);
    for (size_t source = 0; source < graph.node_count(); ++source) {
        for (size_t destination = 0; destination < graph.node_count(); ++destination) {
            if (destination != source && allow_edge(rng)) {
                graph.add_edge(source, destination, edge_cost(line(positions[source], positions[destination])));
            }
        }
    }
#else
    // An more efficient variant is to draw for each node k ~ Binom(n-1, p), and then
    // generate edges to k random other nodes.
    std::binomial_distribution<size_t> edge_count_dist(graph.node_count() - 1, edge_probability);
    node_map<size_t> destinations = graph.make_node_map([](size_t i) { return i; });
    for (size_t source = 0; source < graph.node_count(); ++source) {
        size_t edge_count = edge_count_dist(rng);
        partial_shuffle(destinations.begin(), destinations.begin() + edge_count + 1, destinations.end(), rng);
        for (size_t i = 0; i < edge_count; ++i) {
            size_t destination;
            if (destinations[i] == source) {
                // Note that I shuffled k+1 elements above, for exactly this case. If one of the
                // randomly chosen edges is the source itself I use the (k+1)th element.
                destination = destinations[edge_count];
            } else {
                destination = destinations[i];
            }
            graph.add_edge(source, destination, edge_cost(line(positions[source], positions[destination])));
        }
    }
#endif
}

void sssp::generate_planar_edges(int seed,
                                 double edge_probability,
                                 const edge_cost_fn& edge_cost,
                                 graph& graph,
                                 const node_map<vec2>& positions) {
    using namespace boost::geometry;

    std::mt19937 rng(seed);
    std::binomial_distribution<size_t> edge_count_dist(graph.node_count() - 1, edge_probability);

    boost::geometry::index::rtree<indexed_vec2, index::quadratic<16>> points;
    for (size_t i = 0; i < positions.size(); ++i) {
        points.insert(indexed_vec2(i, positions[i]));
    }

    boost::geometry::index::rtree<line, index::quadratic<16>> lines;
    std::vector<indexed_vec2> result;

    for (size_t source = 0; source < graph.node_count(); ++source) {
        int edge_count = static_cast<int>(edge_count_dist(rng));
        if (edge_count > 0) {
            result.clear();
            // I find the 2 * edge_count closest nodes and pick edge_count random from them.
            points.query(index::nearest(indexed_vec2(-1, positions[source]), edge_count * 2),
                         std::back_inserter(result));
            std::shuffle(result.begin(), result.end(), rng);
            int done = 0;
            for (const auto& dest : result) {
                if (done >= edge_count) {
                    break;
                }
                if (dest.index == source) {
                    continue;
                }
                line candidate(positions[source], positions[dest.index]);
                auto line_endings_unequal = [&](const line& l) {
                    return l.start != candidate.start && l.end != candidate.start && l.start != candidate.end &&
                           l.end != candidate.end;
                };
                if (lines.qbegin(index::intersects(candidate) && index::satisfies(line_endings_unequal)) ==
                    lines.qend()) {
                    graph.add_edge(source, dest.index, edge_cost(candidate));
                    lines.insert(candidate);
                    done += 1;
                }
            }
        }
    }
}

int sssp::y_bucket(int layers, double y) {
    BOOST_ASSERT(layers >= 1);
    BOOST_ASSERT(0.0 <= y && y <= 1.0);
    // The std::min is just for the rare case that some y is exactly 1.
    return std::min(static_cast<int>(std::floor(y * layers)), layers - 1);
}

void sssp::generate_layered_edges(int seed,
                                  double edge_probability,
                                  int layers,
                                  const edge_cost_fn& edge_cost,
                                  graph& graph,
                                  const node_map<vec2>& positions) {
    std::mt19937 rng(seed);

    std::vector<std::vector<size_t>> buckets(layers);
    for (size_t n = 0; n < graph.node_count(); ++n) {
        buckets[y_bucket(layers, positions[n].y)].emplace_back(n);
    }

    for (size_t source = 0; source < graph.node_count(); ++source) {
        std::vector<size_t> destinations;
        int source_bucket = y_bucket(layers, positions[source].y);
        if (source_bucket > 0) {
            destinations.insert(
                destinations.end(), buckets[source_bucket - 1].begin(), buckets[source_bucket - 1].end());
        }
        if (source_bucket < layers - 1) {
            destinations.insert(
                destinations.end(), buckets[source_bucket + 1].begin(), buckets[source_bucket + 1].end());
        }
        size_t edge_count = std::binomial_distribution<size_t>(destinations.size(), edge_probability)(rng);
        partial_shuffle(destinations.begin(), destinations.begin() + edge_count, destinations.end(), rng);
        for (size_t i = 0; i < edge_count; ++i) {
            graph.add_edge(source, destinations[i], edge_cost(line(positions[source], positions[destinations[i]])));
        }
    }
}

void sssp::generate_kronecker_graph(int seed,
                                    std::vector<double> matrix,
                                    int k,
                                    const edge_cost_fn& edge_cost,
                                    graph& graph,
                                    node_map<vec2>& positions) {
    std::mt19937 rng(seed);

    size_t start_size = 1;
    while (start_size * start_size != matrix.size()) {
        BOOST_ASSERT(start_size * start_size < matrix.size());
        start_size += 1;
    }

    size_t final_size = 1;
    for (int i = 0; i < k; ++i) {
        final_size *= start_size;
    }

    positions.resize(final_size, vec2(0.0, 0.0));
    for (size_t n = 0; n < final_size; ++n) {
        graph.add_node();
    }

    std::vector<double> matrix_prefix_sum(matrix.size());
    matrix_prefix_sum[0] = matrix[0];
    for (int i = 1; i < matrix.size(); ++i) {
        matrix_prefix_sum[i] = matrix_prefix_sum[i - 1] + matrix[i];
    }

    // The matrix can be interpret as parameters of a poisson binomial distribution
    // The Kronecker product multiplies the expected values:
    // Consider: M = [a b; c d] and M' = [a' b'; c' d'].
    // The Kronecker product leads to [aM' bM'; cM' dM'], therefore the new sum is
    // sum(aM') + sum(bM') + sum(cM') + sum(dM') = (a + b + c + d) * sum(M') = sum(M) * sum(M').
    double edges_expected_value = std::pow(matrix_prefix_sum.back(), k);
    // Now the poisson bionmial distribution can be apprximated well by a poisson distribution if
    // the probabilites are very low (this is given here due to the nature of potentiation of lots of
    // probabilities).
    size_t edges = std::poisson_distribution<size_t>(edges_expected_value)(rng);

    std::uniform_real_distribution<double> cell_dist(0.0, matrix_prefix_sum.back());
    std::unordered_set<size_t> used_cells;
    int tries = 0;
    for (size_t e = 0; e < edges; ++e) {
        // Sample the cell for the edge
        size_t cell = 0;
        size_t granularity = 1;
        for (int i = 0; i < k; ++i) {
            double value = cell_dist(rng);
            size_t index = std::distance(matrix_prefix_sum.begin(),
                                         std::lower_bound(matrix_prefix_sum.begin(), matrix_prefix_sum.end(), value));
            if (index == matrix_prefix_sum.size()) {
                // This should not happen, but could happen due to double inaccuracies
                index = matrix_prefix_sum.size() - 1;
            }
            cell += index * granularity;
            granularity *= start_size * start_size;
        }
        size_t u = cell / final_size;
        size_t v = cell % final_size;
        if (u == v || used_cells.find(cell) != used_cells.end()) {
            tries += 1;
            if (tries < 30) {
                // Try again
                e -= 1;
                continue;
            } else {
                // Throw this edge away
                continue;
            }
        }
        tries = 0;
        used_cells.insert(cell);
        graph.add_edge(u, v, edge_cost(line(vec2(0.0, 0.0), vec2(0.0, 0.0))));
    }
}


#include <Eigen/Dense>
#include <algorithm>
#include <vector>

int atom(int ao_index);
int indexOf(std::vector<std::string> my_list, std::string element);
bool list_contains(std::vector<std::string>  my_list, std::string element);
std::string orb(int ao_index, std::vector<std::string> orbital_types);
int ao_index(int atom_p, std::string orb_p, std::vector<std::string>  orbital_types);
float chi_on_atom(std::string o1, std::string o2, std::string o3, std::vector<std::string> p_orbitals,double dipole);
Eigen::MatrixXd fast_fock_matrix(Eigen::MatrixXd hamiltonian, Eigen::MatrixXd interaction, Eigen::MatrixXd rho,double dipole);
int main(void);


#pragma once
#include <armadillo>

class LinearRegression
{
protected:
    arma::mat X;
    arma::vec Y, coef;
    bool fit_intercept;

public:
    LinearRegression(arma::mat &x, arma::vec &y);
    void fit(bool fit_intercept);
    arma::vec predict(arma::mat &X_pred);
    arma::vec getCoef() const { return coef; }
    //~LinearRegression();
};


//
// Created by michaelpollind on 1/11/18.
//

#ifndef QSYS_SIMULATOR_H
#define QSYS_SIMULATOR_H

#include <Godot.hpp>
#include <Reference.hpp>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include <complex>
#include <random>
#include <vector>
#include <map>

class Simulator : public godot::GodotScript<godot::Reference>{
    GODOT_CLASS(Simulator)

private:
    Eigen::MatrixXcd _hamiltonian;
    Eigen::MatrixXcd _propagator;
    Eigen::VectorXcd _psi0;
    Eigen::VectorXcd _currentState;
    std::vector<double> _probabilityDensity;
    void _setPropagator(Eigen::MatrixXcd arr);
    double _time;
    int _size;
    int _result;
    int _sampleProbabilityDensity();
    Eigen::MatrixXcd _getPOVM(std::map<int, int>);
    std::default_random_engine _gen;

public:
    Simulator();
    void _init();
    void _setSize(const int size);
    void _setHamiltonian(godot::PoolVector2Array arr);
    void _setPsi0(godot::PoolVector2Array arr);
    void _runOneStep(float delta);
	int _getCurrentStateSize();
	int _getPropagatorRows();
	int _getPropagatorCols();
    float _getTime();

    godot::String _getErrorMessage();
    godot::PoolRealArray _getProbabilityDensity();
    godot::PoolVector2Array _getHamiltonian();
    godot::PoolVector2Array _getPsi0();
    int _measure();
    godot::PoolVector2Array _getCurrentState();
    godot::PoolVector2Array _getPropagator();

    static void _register_methods();
};

#endif //QSYS_SIMULATOR_H


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

/*
mult           0.0163621 s
solveInverse   0.0201562 s
solveFullPivLu 0.202951 s
solveColPivHh  0.547893 s
solveFullPivHh 0.621699 s
solveComplete  0.844406 s

*/

Eigen::Vector3f mult(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m * v;
}

Eigen::Vector3f solveInverse(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.inverse() * v;
}

Eigen::Vector3f solveFullPivLu(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.fullPivLu().solve(v);
}

Eigen::Vector3f solveColPivHh(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.colPivHouseholderQr().solve(v);
}

Eigen::Vector3f solveFullPivHh(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.fullPivHouseholderQr().solve(v);
}

Eigen::Vector3f solveComplete(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.completeOrthogonalDecomposition().solve(v);
}

Eigen::Vector3f solveBdcsvd(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.bdcSvd().solve(v);
}

Eigen::Vector3f solveJacobiSvd(Eigen::Matrix3f const &m, Eigen::Vector3f const &v) {
  return m.jacobiSvd().solve(v);
}

typedef Eigen::Vector3f func(Eigen::Matrix3f const &m, Eigen::Vector3f const &v);

int measure(char const *t, func &f) {
  Eigen::Matrix3f m;
  Eigen::Vector3f v,s;
  s << 0.0f, 0.0f, 0.0f;
  int n = 0;
  auto start = std::chrono::high_resolution_clock::now();
  for(int i = 0; i < 1000000; ++i) {
    m << ++n, ++n, ++n, ++n, ++n, ++n, ++n, ++n, ++n;
    v << ++n, ++n, ++n;
    n %= 71;
    s += f(m, v);
  }
  auto end = std::chrono::high_resolution_clock::now();
  std::chrono::duration<double> diff = end - start;
  std::cout << t << ' ' << diff.count() << " s\n";
  return s(0);
}

int main() {
  return measure("mult", mult) +
  measure("solveInverse", solveInverse) +
  measure("solveFullPivLu", solveFullPivLu) +
  measure("solveColPivHh", solveColPivHh) +
  measure("solveFullPivHh", solveFullPivHh) +
  measure("solveComplete", solveComplete)/* +
  measure("solveBdcSvd", solveBdcsvd) +
  measure("solveJacobiSvd", solveJacobiSvd)*/;
}