#pragma once

#include <boost/hana/string.hpp>
#include <boost/hana/value.hpp>
#include <boost/hana/power.hpp>
#include <boost/hana/div.hpp>

namespace tvm {

namespace hana = boost::hana;

template<unsigned radix>
constexpr size_t get_magnitude(size_t num) {
  unsigned i = 0;
  while (num > 0) {
    num /= radix;
    ++i;
  }
  return i;
}

constexpr char hex_sym(unsigned num) {
  constexpr char arr[] = "0123456789abcdef";
  return arr[std::min(num, 16u)];
}

template <unsigned radix, class X, size_t ...i>
constexpr auto to_string(X x, std::index_sequence<i...>) {
  constexpr size_t mag = get_magnitude<radix>(X::value);
  return hana::string<
    hana::size_c<hex_sym(
      hana::value(x / hana::power(hana::size_c<radix>, hana::size_c<mag - i - 1>)
                    % hana::size_c<radix>))
      >...>{};
}

template <unsigned radix, class X>
constexpr auto to_string(X) {
  using namespace hana::literals;
  if constexpr (X::value == 0)
    return "0"_s;
  else
    return to_string<radix>(hana::size_c<static_cast<size_t>(X::value)>,
                            std::make_index_sequence<get_magnitude<radix>(X::value)>());
}

} // namespace tvm


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

#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/laplacian_setup.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/recursion/predefined_masks.hpp>
#include <boost/numeric/mtl/operation/print.hpp>
#include <boost/numeric/mtl/operation/rank_two_update.hpp>

using namespace std;

inline double value(double)
{
    return 1.0;
}

inline complex<double> value(complex<double>)
{
    return complex<double>(1.0, 1.0);
}

inline double test_value(double)
{
    return 10.0;
}

inline complex<double> test_value(complex<double>)
{
    return complex<double>(10.0, -10.0);
}

template <typename Matrix>
void test(Matrix& matrix, const char* name)
{
    using mtl::conj; using mtl::Collection;
    const unsigned max_print_size= 25;

    cout << "\n" << name << "\n";
    set_to_zero(matrix);

    typename Collection<Matrix>::size_type          nr= num_rows(matrix), nc= num_cols(matrix);
    typedef typename Collection<Matrix>::value_type value_type;
    value_type                                      zero(0.0);
    mtl::dense_vector<value_type>                   x(nr, zero), y(nc, zero);

    x[1]= 1.0; x[2]= 2.0;
    value_type ref(0), v= value(ref);

    y[4]= 4.0*v; y[5]= 5.0*v; y[6]= 6.0*v;

    rank_two_update(matrix, x, y);
    if (nr <= max_print_size)
	cout << "\nx= " << x << "y= " << y << "matrix = \n" << matrix << "\n";

    MTL_THROW_IF(matrix[2][5] != test_value(v), mtl::runtime_error("wrong value"));
    MTL_THROW_IF(matrix[5][2] != conj(test_value(v)), mtl::runtime_error("wrong value"));
}

int main(int argc, char* argv[])
{
    using namespace mtl;

    cout << "matrix size must be at least 7 x 7\n";
    unsigned size= 7;
    if (argc > 1) size= atoi(argv[1]);

    dense2D<double>                                      dr(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);
    morton_dense<double, recursion::morton_z_mask>       mzd(size, size);
    morton_dense<double, recursion::doppled_2_row_mask>  d2r(size, size);
    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<complex<double> >                            drc(size, size);
    compressed2D<complex<double> >                       crc(size, size);

    test(dr, "Dense row major");
    test(dc, "Dense column major");
    test(mzd, "Morton Z-order");
    test(d2r, "Hybrid 2 row-major");
    test(cr, "Compressed row major");
    test(cc, "Compressed column major");
    test(drc, "Dense row major complex");
    test(crc, "Compressed row major complex");

    return 0;
}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
    This is an example illustrating the use of the tools in dlib for doing distribution
    estimation or detecting anomalies using one-class support vector machines.

    Unlike regular classifiers, these tools take unlabeled points and try to learn what
    parts of the feature space normally contain data samples and which do not.  Typically
    you use these tools when you are interested in finding outliers or otherwise
    identifying "unusual" data samples.

    In this example, we will sample points from the sinc() function to generate our set of
    "typical looking" points.  Then we will train some one-class classifiers and use them
    to predict if new points are unusual or not.  In this case, unusual means a point is
    not from the sinc() curve.
*/

#include <iostream>
#include <vector>
#include <dlib/svm.h>
#include <dlib/gui_widgets.h>
#include <dlib/array2d.h>
#include <dlib/image_transforms.h>

using namespace std;
using namespace dlib;

// Here is the sinc function we will be trying to learn with the one-class SVMs
double sinc(double x)
{
    if (x == 0)
        return 2;
    return 2*sin(x)/x;
}

int main()
{
    // We will use column vectors to store our points.  Here we make a convenient typedef
    // for the kind of vector we will use.
    typedef matrix<double,0,1> sample_type;

    // Then we select the kernel we want to use.  For our present problem the radial basis
    // kernel is quite effective.
    typedef radial_basis_kernel<sample_type> kernel_type;

    // Now make the object responsible for training one-class SVMs.
    svm_one_class_trainer<kernel_type> trainer;
    // Here we set the width of the radial basis kernel to 4.0.  Larger values make the
    // width smaller and give the radial basis kernel more resolution.  If you play with
    // the value and observe the program output you will get a more intuitive feel for what
    // that means.
    trainer.set_kernel(kernel_type(4.0));

    // Now sample some 2D points.  The points will be located on the curve defined by the
    // sinc() function.
    std::vector<sample_type> samples;
    sample_type m(2);
    for (double x = -15; x <= 8; x += 0.3)
    {
        m(0) = x;
        m(1) = sinc(x);
        samples.push_back(m);
    }

    // Now train a one-class SVM.  The result is a function, df(), that outputs large
    // values for points from the sinc() curve and smaller values for points that are
    // anomalous (i.e. not on the sinc() curve in our case).
    decision_function<kernel_type> df = trainer.train(samples);

    // So for example, let's look at the output from some points on the sinc() curve.
    cout << "Points that are on the sinc function:\n";
    m(0) = -1.5; m(1) = sinc(m(0)); cout << "   " << df(m) << endl;
    m(0) = -1.5; m(1) = sinc(m(0)); cout << "   " << df(m) << endl;
    m(0) = -0;   m(1) = sinc(m(0)); cout << "   " << df(m) << endl;
    m(0) = -0.5; m(1) = sinc(m(0)); cout << "   " << df(m) << endl;
    m(0) = -4.1; m(1) = sinc(m(0)); cout << "   " << df(m) << endl;
    m(0) = -1.5; m(1) = sinc(m(0)); cout << "   " << df(m) << endl;
    m(0) = -0.5; m(1) = sinc(m(0)); cout << "   " << df(m) << endl;

    cout << endl;
    // Now look at some outputs for points not on the sinc() curve.  You will see that
    // these values are all notably smaller.
    cout << "Points that are NOT on the sinc function:\n";
    m(0) = -1.5; m(1) = sinc(m(0))+4;   cout << "   " << df(m) << endl;
    m(0) = -1.5; m(1) = sinc(m(0))+3;   cout << "   " << df(m) << endl;
    m(0) = -0;   m(1) = -sinc(m(0));    cout << "   " << df(m) << endl;
    m(0) = -0.5; m(1) = -sinc(m(0));    cout << "   " << df(m) << endl;
    m(0) = -4.1; m(1) = sinc(m(0))+2;   cout << "   " << df(m) << endl;
    m(0) = -1.5; m(1) = sinc(m(0))+0.9; cout << "   " << df(m) << endl;
    m(0) = -0.5; m(1) = sinc(m(0))+1;   cout << "   " << df(m) << endl;

    // The output is as follows:
    /*
    Points that are on the sinc function:
        0.000389691
        0.000389691
        -0.000239037
        -0.000179978
        -0.000178491
        0.000389691
        -0.000179978

    Points that are NOT on the sinc function:
        -0.269389
        -0.269389
        -0.269389
        -0.269389
        -0.269389
        -0.239954
        -0.264318
    */

    // So we can see that in this example the one-class SVM correctly indicates that
    // the non-sinc points are definitely not points from the sinc() curve.

    // It should be noted that the svm_one_class_trainer becomes very slow when you have
    // more than 10 or 20 thousand training points.  However, dlib comes with very fast SVM
    // tools which you can use instead at the cost of a little more setup.  In particular,
    // it is possible to use one of dlib's very fast linear SVM solvers to train a one
    // class SVM.  This is what we do below.  We will train on 115,000 points and it only
    // takes a few seconds with this tool!
    //
    // The first step is constructing a feature space that is appropriate for use with a
    // linear SVM.  In general, this is quite problem dependent.  However, if you have
    // under about a hundred dimensions in your vectors then it can often be quite
    // effective to use the empirical_kernel_map as we do below (see the
    // empirical_kernel_map documentation and example program for an extended discussion of
    // what it does).
    //
    // But putting the empirical_kernel_map aside, the most important step in turning a
    // linear SVM into a one-class SVM is the following.  We append a -1 value onto the end
    // of each feature vector and then tell the trainer to force the weight for this
    // feature to 1.  This means that if the linear SVM assigned all other weights a value
    // of 0 then the output from a learned decision function would always be -1.  The
    // second step is that we ask the SVM to label each training sample with +1.  This
    // causes the SVM to set the other feature weights such that the training samples have
    // positive outputs from the learned decision function.  But the starting bias for all
    // the points in the whole feature space is -1.  The result is that points outside our
    // training set will not be affected, so their outputs from the decision function will
    // remain close to -1.

    empirical_kernel_map<kernel_type> ekm;
    ekm.load(trainer.get_kernel(),samples);

    samples.clear();
    std::vector<double> labels;
    // make a vector with just 1 element in it equal to -1.
    sample_type bias(1);
    bias = -1;
    sample_type augmented;
    // This time sample 115,000 points from the sinc() function.
    for (double x = -15; x <= 8; x += 0.0002)
    {
        m(0) = x;
        m(1) = sinc(x);
        // Apply the empirical_kernel_map transformation and then append the -1 value
        augmented = join_cols(ekm.project(m), bias);
        samples.push_back(augmented);
        labels.push_back(+1);
    }
    cout << "samples.size(): "<< samples.size() << endl;

    // The svm_c_linear_dcd_trainer is a very fast SVM solver which only works with the
    // linear_kernel.  It has the nice feature of supporting this "force_last_weight_to_1"
    // mode we discussed above.
    svm_c_linear_dcd_trainer<linear_kernel<sample_type> > linear_trainer;
    linear_trainer.force_last_weight_to_1(true);

    // Train the SVM
    decision_function<linear_kernel<sample_type> > df2 = linear_trainer.train(samples, labels);

    // Here we test it as before, again we note that points from the sinc() curve have
    // large outputs from the decision function.  Note also that we must remember to
    // transform the points in exactly the same manner used to construct the training set
    // before giving them to df2() or the code will not work.
    cout << "Points that are on the sinc function:\n";
    m(0) = -1.5; m(1) = sinc(m(0)); cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;
    m(0) = -1.5; m(1) = sinc(m(0)); cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;
    m(0) = -0;   m(1) = sinc(m(0)); cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;
    m(0) = -0.5; m(1) = sinc(m(0)); cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;
    m(0) = -4.1; m(1) = sinc(m(0)); cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;
    m(0) = -1.5; m(1) = sinc(m(0)); cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;
    m(0) = -0.5; m(1) = sinc(m(0)); cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;

    cout << endl;
    // Again, we see here that points not on the sinc() function have small values.
    cout << "Points that are NOT on the sinc function:\n";
    m(0) = -1.5; m(1) = sinc(m(0))+4;   cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;
    m(0) = -1.5; m(1) = sinc(m(0))+3;   cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;
    m(0) = -0;   m(1) = -sinc(m(0));    cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;
    m(0) = -0.5; m(1) = -sinc(m(0));    cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;
    m(0) = -4.1; m(1) = sinc(m(0))+2;   cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;
    m(0) = -1.5; m(1) = sinc(m(0))+0.9; cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;
    m(0) = -0.5; m(1) = sinc(m(0))+1;   cout << "   " << df2(join_cols(ekm.project(m),bias)) << endl;

    // The output is as follows:
    /*
    Points that are on the sinc function:
        1.00454
        1.00454
        1.00022
        1.00007
        1.00371
        1.00454
        1.00007

    Points that are NOT on the sinc function:
        -1
        -1
        -1
        -1
        -0.999998
        -0.781231
        -0.96242
    */

    // Finally, to help you visualize what is happening here we are going to plot the
    // response of the one-class classifiers on the screen.  The code below creates two
    // heatmap images which show the response.  In these images you can clearly see where
    // the algorithms have identified the sinc() curve.  The hotter the pixel looks, the
    // larger the value coming out of the decision function and therefore the more "normal"
    // it is according to the classifier.
    const long size = 500;
    array2d<double> img1(size,size);
    array2d<double> img2(size,size);
    for (long r = 0; r < img1.nr(); ++r)
    {
        for (long c = 0; c < img1.nc(); ++c)
        {
            double x = 30.0*c/size - 19;
            double y = 8.0*r/size - 4;
            m(0) = x;
            m(1) = y;
            img1[r][c] = df(m);
            img2[r][c] = df2(join_cols(ekm.project(m),bias));
        }
    }
    image_window win1(heatmap(img1), "svm_one_class_trainer");
    image_window win2(heatmap(img2), "svm_c_linear_dcd_trainer");
    win1.wait_until_closed();
}


/*************************************************************************
	> File Name: example.cpp
	> Author: ziqiang
	> Mail: ziqiang_free@163.com
	> Created Time: Fri 05 May 2017 11:17:16 PM CST
 ************************************************************************/

#include <boost/lambda/lambda.hpp>
#include <iostream>
#include <iterator>
#include <algorithm>
using namespace std;

int main()
{
	using namespace boost::lambda;
	typedef std::istream_iterator<int> in;

	std::for_each(
			in(std::cin), in(), std::cout << (_1 * 3) << " ");

}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2020, LAAS-CNRS, New York University, Max Planck Gesellschaft
//                          University of Edinburgh, INRIA
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API

#include <iterator>
#include <Eigen/Dense>
#include <pinocchio/fwd.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/bind.hpp>
#include "crocoddyl/core/activation-base.hpp"
#include "crocoddyl/core/activations/quadratic-barrier.hpp"
#include "crocoddyl/core/activations/quadratic.hpp"
#include "crocoddyl/core/activations/smooth-abs.hpp"
#include "crocoddyl/core/activations/weighted-quadratic.hpp"
#include "crocoddyl/core/numdiff/activation.hpp"
#include "crocoddyl/core/utils/exception.hpp"

using namespace boost::unit_test;

struct TestTypes {
  enum Type {
    ActivationModelQuadraticBarrier,
    ActivationModelQuad,
    ActivationModelSmoothAbs,
    ActivationModelWeightedQuad,
    NbTestTypes
  };
  static std::vector<Type> init_all() {
    std::vector<Type> v;
    v.clear();
    for (int i = 0; i < NbTestTypes; ++i) {
      v.push_back((Type)i);
    }
    return v;
  }
  static const std::vector<Type> all;
};
const std::vector<TestTypes::Type> TestTypes::all(TestTypes::init_all());

class Factory {
 public:
  Factory(TestTypes::Type type) {
    test_type_ = type;

    nr_ = 5;
    num_diff_modifier_ = 1e4;
    Eigen::VectorXd lb = Eigen::VectorXd::Random(nr_);
    Eigen::VectorXd ub = lb + Eigen::VectorXd::Ones(nr_) + Eigen::VectorXd::Random(nr_);
    Eigen::VectorXd weights = Eigen::VectorXd::Random(nr_);

    switch (test_type_) {
      case TestTypes::ActivationModelQuadraticBarrier:
        model_ = boost::make_shared<crocoddyl::ActivationModelQuadraticBarrier>(crocoddyl::ActivationBounds(lb, ub));
        break;
      case TestTypes::ActivationModelQuad:
        model_ = boost::make_shared<crocoddyl::ActivationModelQuad>(nr_);
        break;
      case TestTypes::ActivationModelSmoothAbs:
        model_ = boost::make_shared<crocoddyl::ActivationModelSmoothAbs>(nr_);
        break;
      case TestTypes::ActivationModelWeightedQuad:
        model_ = boost::make_shared<crocoddyl::ActivationModelWeightedQuad>(weights);
        break;
      default:
        throw_pretty(__FILE__ ":\n Construct wrong TestTypes::Type");
        break;
    }
  }

  ~Factory() {}

  boost::shared_ptr<crocoddyl::ActivationModelAbstract> get_model() { return model_; }
  const std::size_t& get_nr() { return nr_; }
  double get_num_diff_modifier() { return num_diff_modifier_; }

 private:
  double num_diff_modifier_;
  std::size_t nr_;
  boost::shared_ptr<crocoddyl::ActivationModelAbstract> model_;
  TestTypes::Type test_type_;
};

//----------------------------------------------------------------------------//

void test_construct_data(TestTypes::Type test_type) {
  // create the model
  Factory factory(test_type);
  const boost::shared_ptr<crocoddyl::ActivationModelAbstract>& model = factory.get_model();

  // create the corresponding data object
  boost::shared_ptr<crocoddyl::ActivationDataAbstract> data = model->createData();
}

void test_calc_returns_a_value(TestTypes::Type test_type) {
  // create the model
  Factory factory(test_type);
  const boost::shared_ptr<crocoddyl::ActivationModelAbstract>& model = factory.get_model();

  // create the corresponding data object
  boost::shared_ptr<crocoddyl::ActivationDataAbstract> data = model->createData();

  // Generating random input vector
  const Eigen::VectorXd& r = Eigen::VectorXd::Random(model->get_nr());
  data->a_value = nan("");

  // Getting the state dimension from calc() call
  model->calc(data, r);

  // Checking that calc returns a value
  BOOST_CHECK(!std::isnan(data->a_value));
}

void test_partial_derivatives_against_numdiff(TestTypes::Type test_type) {
  // create the model
  Factory factory(test_type);
  const boost::shared_ptr<crocoddyl::ActivationModelAbstract>& model = factory.get_model();

  // create the corresponding data object and set the cost to nan
  boost::shared_ptr<crocoddyl::ActivationDataAbstract> data = model->createData();

  crocoddyl::ActivationModelNumDiff model_num_diff(model);
  boost::shared_ptr<crocoddyl::ActivationDataAbstract> data_num_diff = model_num_diff.createData();

  // Generating random values for the state and control
  const Eigen::VectorXd& r = Eigen::VectorXd::Random(model->get_nr());

  // Computing the action derivatives
  model->calcDiff(data, r);
  model_num_diff.calcDiff(data_num_diff, r);

  // Checking the partial derivatives against NumDiff
  double tol = factory.get_num_diff_modifier() * model_num_diff.get_disturbance();
  BOOST_CHECK(std::abs(data->a_value - data_num_diff->a_value) < tol);
  BOOST_CHECK((data->Ar - data_num_diff->Ar).isMuchSmallerThan(1.0, tol));

  // numerical differentiation of the Hessian is not good enough to be tested.
  // BOOST_CHECK((data->Arr - data_num_diff->Arr).isMuchSmallerThan(1.0, tol));
}

//----------------------------------------------------------------------------//

void register_unit_tests(TestTypes::Type type, test_suite& ts) {
  ts.add(BOOST_TEST_CASE(boost::bind(&test_construct_data, type)));
  ts.add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_value, type)));
  ts.add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff, type)));
}

bool init_function() {
  for (size_t i = 0; i < TestTypes::all.size(); ++i) {
    const std::string test_name = "test_" + std::to_string(i);
    test_suite* ts = BOOST_TEST_SUITE(test_name);
    register_unit_tests(TestTypes::all[i], *ts);
    framework::master_test_suite().add(ts);
  }
  return true;
}

int main(int argc, char** argv) { return ::boost::unit_test::unit_test_main(&init_function, argc, argv); }


//  Permission to copy, use, modify, sell and
//  distribute this software is granted provided this copyright notice appears
//  in all copies. This software is provided "as is" without express or implied
//  warranty, and with no claim as to its suitability for any purpose.
//  Copyright Toon Knapen and Kresimir Fresl

#ifndef BOOST_BINDINGS_BLAS_BLAS3_HPP
#define BOOST_BINDINGS_BLAS_BLAS3_HPP

#include <boost/numeric/bindings/blas/blas3_overloads.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/transpose.hpp>

namespace boost { namespace numeric { namespace bindings { namespace blas {

  // C <- alpha * op (A) * op (B) + beta * C
  // op (X) == X || X^T || X^H
  template < typename value_type, typename matrix_type_a, typename matrix_type_b, typename matrix_type_c >
  // ! CAUTION this function assumes that all matrices involved are column-major matrices
  void gemm(const char TRANSA, const char TRANSB,
	    const value_type& alpha,
	    const matrix_type_a &a,
	    const matrix_type_b &b,
	    const value_type &beta,
	    matrix_type_c &c
	    )
  {
    const int m = TRANSA == traits::NO_TRANSPOSE ? traits::matrix_size1( a ) : traits::matrix_size2( a ) ;
    const int n = TRANSB == traits::NO_TRANSPOSE ? traits::matrix_size2( b ) : traits::matrix_size1( b );
    const int k = TRANSA == traits::NO_TRANSPOSE ? traits::matrix_size2( a ) : traits::matrix_size1( a ) ;
    assert( k ==  ( TRANSB == traits::NO_TRANSPOSE ? traits::matrix_size1( b ) : traits::matrix_size2( b ) ) ) ;
    assert( m == traits::matrix_size1( c ) );
    assert( n == traits::matrix_size2( c ) );
    const int lda = traits::leading_dimension( a );
    const int ldb = traits::leading_dimension( b );
    const int ldc = traits::leading_dimension( c );

    const value_type *a_ptr = traits::matrix_storage( a ) ;
    const value_type *b_ptr = traits::matrix_storage( b ) ;
    value_type *c_ptr = traits::matrix_storage( c ) ;

    detail::gemm( TRANSA, TRANSB, m, n, k, alpha, a_ptr, lda, b_ptr, ldb, beta, c_ptr, ldc ) ;
  }

  // C <- alpha * A * B + beta * C
  template < typename value_type, typename matrix_type_a, typename matrix_type_b, typename matrix_type_c >
  void gemm(const value_type& alpha,
	    const matrix_type_a &a,
	    const matrix_type_b &b,
	    const value_type &beta,
	    matrix_type_c &c
	    )
  {
    gemm( traits::NO_TRANSPOSE, traits::NO_TRANSPOSE, alpha, a, b, beta, c ) ;
  }

  // C <- A * B
  // ! CAUTION this function assumes that all matrices involved are column-major matrices
  template <
    typename matrix_type_a, typename matrix_type_b, typename matrix_type_c
    >
  void gemm(const matrix_type_a &a, const matrix_type_b &b, matrix_type_c &c)
  {
    typedef typename traits::matrix_traits<matrix_type_c>::value_type val_t;
    gemm( traits::NO_TRANSPOSE, traits::NO_TRANSPOSE, (val_t) 1, a, b, (val_t) 0, c ) ;
  }

  // C <- alpha * A * A^T + beta * C
  // C <- alpha * A^T * A + beta * C
  template < typename value_type, typename matrix_type_a, typename matrix_type_c >
  void syrk( char uplo, char trans, const value_type& alpha, const matrix_type_a& a,
             const value_type& beta, matrix_type_c& c) {
     const int n = traits::matrix_size1( c );
     assert( n == traits::matrix_size2( c ) );
     const int k = trans == traits::NO_TRANSPOSE ? traits::matrix_size2( a ) : traits::matrix_size1( a ) ;
     assert( n == traits::NO_TRANSPOSE ? traits::matrix_size1( a ) : traits::matrix_size2( a ) );
     const int lda = traits::leading_dimension( a );
     const int ldc = traits::leading_dimension( c );

     const value_type *a_ptr = traits::matrix_storage( a ) ;
     value_type *c_ptr = traits::matrix_storage( c ) ;

     detail::syrk( uplo, trans, n, k, alpha, a_ptr, lda, beta, c_ptr, ldc );
  } // syrk()

  // C <- alpha * A * A^H + beta * C
  // C <- alpha * A^H * A + beta * C
  template < typename real_type, typename matrix_type_a, typename matrix_type_c >
  void herk( char uplo, char trans, const real_type& alpha, const matrix_type_a& a,
             const real_type& beta, matrix_type_c& c) {
     typedef typename matrix_type_c::value_type value_type ;

     const int n = traits::matrix_size1( c );
     assert( n == traits::matrix_size2( c ) );
     const int k = trans == traits::NO_TRANSPOSE ? traits::matrix_size2( a ) : traits::matrix_size1( a ) ;
     assert( n == traits::NO_TRANSPOSE ? traits::matrix_size1( a ) : traits::matrix_size2( a ) );
     const int lda = traits::leading_dimension( a );
     const int ldc = traits::leading_dimension( c );

     const value_type *a_ptr = traits::matrix_storage( a ) ;
     value_type *c_ptr = traits::matrix_storage( c ) ;

     detail::herk( uplo, trans, n, k, alpha, a_ptr, lda, beta, c_ptr, ldc );
  } // herk()

  // B <- alpha * op( A^-1 )
  // B <- alpha * B op( A^-1 )
  // op( A ) = A, A^T, A^H
  template < class T, class A, class B >
  void trsm( char side, char uplo, char transa, char diag, T const& alpha, A const& a, B& b ) {
     const int m = traits::matrix_size1( b ) ;
     const int n = traits::matrix_size2( b ) ;
     assert( ( side=='L' && m==traits::matrix_size2( a ) && m==traits::matrix_size1( a ) ) ||
             ( side=='R' && n==traits::matrix_size2( a ) && n==traits::matrix_size1( a ) ) ) ;
     assert( side=='R' || side=='L' ) ;
     assert( uplo=='U' || uplo=='L' ) ;
     assert( ( side=='L' && m==traits::matrix_size1( a ) ) || ( side=='R' && n==traits::matrix_size1( a ) ) ) ;
     detail::trsm( side, uplo, transa, diag, m, n, alpha,
                   traits::matrix_storage( a ), traits::leading_dimension( a ),
                   traits::matrix_storage( b ), traits::leading_dimension( b )
                 ) ;
  }

}}}}

#endif // BOOST_BINDINGS_BLAS_BLAS3_HPP


#include "anova.h"

// STL
#include <vector>
#include <algorithm>

// debug
#include <iostream>
#include <iomanip>

#include <boost/math/distributions/fisher_f.hpp>

template<typename T>
static T *transform(std::vector<T> &v)
{
    int size = v.size();
    T *result = new T[size];
    std::copy(v.begin(), v.end(), result);
    return result;
}

namespace stats
{

class MeanEnumer
{
public:
    MeanEnumer() = default;

    void operator()(const Sample &s)
    {
        _n += s.getCount();
        for (auto &el: s.getElements())
            _sum += el;
    }

    inline double getMean(){ return _sum / _n; }
    inline int getN(){ return _n; }
private:
    int _n = 0;
    double _sum = 0;
};

double Anova::FTest(const Samples &samples)
{
    assert(samples.size() > 1);
    if (samples.size()  <= 1)
        throw std::domain_error("count of samples must be more than 1");

    MeanEnumer enumer = std::for_each(samples.begin(), samples.end(), MeanEnumer());
    double generalMean = enumer.getMean();
    int N = enumer.getN();

    double Qa = 0;

    std::for_each(samples.begin(), samples.end(), [&Qa, &generalMean](const Sample &s){
        Qa += s.getCount() * pow(s.getMean() - generalMean, 2);
    });

    double Qoct = 0;
    std::for_each(samples.begin(), samples.end(), [&Qoct](const Sample &s){
        for (auto &el: s.getElements())
            Qoct += pow(el - s.getMean(), 2);
    });

    int sampleSize = samples.size();
    _v1 = sampleSize - 1;
    _v2 = N - sampleSize;
    _F = (Qa / _v1) / (Qoct / _v2);
    return _F;
}

double Anova::FCriticalTest(double alpha) const
{
    using namespace boost::math;
    fisher_f dist(_v1, _v2);
    return quantile(complement(dist, alpha));
}

//---------Two factor ANOVA---------------------------------------
TwoFactorAnova::Result TwoFactorAnova::FTest(const Samples &rowSamples)
{
    int m = rowSamples.size();
    assert(m != 0);
    if (m  == 0)
        throw std::domain_error("count of samples must be more than 1");

    int r = rowSamples[0].getCount();
    Samples columnSamples(r);
    Sample commonSample;
    for (int i = 0; i < m; i++)
    {
        const Sample &rowSample = rowSamples[i];
        int j = 0;
        for (auto &el: rowSample.getElements())
        {
            columnSamples[j].add(el);
            commonSample.add(el);
            j++;
        }
    }

    commonSample.refresh();
    for (auto &sample: columnSamples)
    {
        sample.refresh();
    }

    double Qa = 0.0, Qb = 0.0, Qoct = 0.0;

    for (const Sample &sample: columnSamples)
    {
        Qa += r*pow(sample.getMean() - commonSample.getMean(), 2);
    }

    double Da = Qa / (m - 1);

    for (const Sample &sample: rowSamples)
    {
        Qb += m*pow(sample.getMean() - commonSample.getMean(), 2);
    }

    double Db = Qb / (r - 1);

    for (int i = 0; i < m; i++)
    {
        for (int j = 0; j < r; j++)
        {
            Qoct += pow((rowSamples[i].getElements())[j]
                        + commonSample.getMean() - (rowSamples[i].getMean() + columnSamples[j].getMean()), 2);
        }
    }
    double Doct = Qoct / ((m - 1)*(r - 1));

    _Va = m - 1;
    _Vb = r - 1;
    _V2 = (m - 1) * (r - 1);

    Result res;
    res.FA = _F.FA =  Da / Doct;
    res.FB = _F.FB =  Db / Doct;
    res.FAB = _F.FAB = std::max(res.FA, res.FB) / std::min(res.FA, res.FB);

    return res;
}

double TwoFactorAnova::FcriticalTest(double alpha, int v1, int v2) const
{
    using namespace boost::math;
    fisher_f dist(v1, v2);
    return quantile(complement(dist, alpha));
}

double TwoFactorAnova::FCriticalTestA(double alpha) const
{
    return FcriticalTest(alpha, _Va, _V2);
}

double TwoFactorAnova::FCriticalTestB(double alpha) const
{
    return FcriticalTest(alpha, _Vb, _V2);
}

double TwoFactorAnova::FCriticalTestAB(double alpha) const
{
    return FcriticalTest(alpha, _V2, _V2);
}

}// stats


/* Copyright (c) 2020 C. Pattison
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once
#include "syk.hpp"
#include "syk_types.hpp"
#include "cuda_diagonalize.hpp"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <exception>
#include <algorithm>
#include <complex>

namespace syk {
using namespace std::complex_literals;

std::vector<double> cpu_hamiltonian_eigenvals(const MatrixType& hamiltonian) {
    auto eigensolver = Eigen::SelfAdjointEigenSolver<MatrixType>();
    eigensolver.compute(hamiltonian, false);

    if(eigensolver.info() != 0) {
        throw std::runtime_error("Eigensolver not converged");
    }
    const auto& eigenvals_vector = eigensolver.eigenvalues();
    std::vector<double> eigenvals(eigenvals_vector.size());
    std::copy_n(eigenvals_vector.data(), eigenvals_vector.size(), eigenvals.begin());
    std::sort(eigenvals.begin(), eigenvals.end());

    return eigenvals;
}

template<typename mat>
std::complex<double> closest_eigenval(mat m, std::complex<double> v) {
    std::complex<double> a, b, c, d;
    a = m(0,0);
    b = m(0,1);
    c = m(1,0);
    d = m(1,1);

    auto desc = std::sqrt(a*a + 4.0 * b * c - 2.0 * a * d + d * d);
    auto l_1 = (a + d + desc)/2.0;
    auto l_2 = (a + d - desc)/2.0;

    auto dist_1 = std::abs(l_1 - v);
    auto dist_2 = std::abs(l_2 - v);
    return dist_1 < dist_2 ? l_1 : l_2;
}

std::vector<double> QR_hamiltonian_eigenvals(const MatrixType& ham, MatrixType* Q_H,  double max_resid = 1e-7) {
    int max_iter = 1000 * ham.rows();
    assert(ham.rows() == ham.cols());
    assert(Q_H->rows() == ham.rows());
    assert(Q_H->cols() == ham.cols());

    // Eigen::HessenbergDecomposition<MatrixType> hd;
    // hd.compute(ham);
    // *Q_H = hd.matrixQ().adjoint();

    MatrixType temp(ham.rows(), ham.cols());
    MatrixType Q_curr(ham.rows(), ham.cols());

    MatrixType A = Q_H->adjoint() * ham * *Q_H;
    Eigen::HouseholderQR<MatrixType> qr(ham.rows(), ham.cols());

    // TODO: Use tridiagonal form
    int k;
    int deflate = ham.diagonal().size();
    for(k = 0; k < max_iter; ++k) {
        // // Rayleigh quotient
        // std::complex<double> shift = A(deflate-1, deflate-1);
        // Wilkinson
        auto shift = closest_eigenval(A.block(deflate-2, deflate-2, 2, 2), A(deflate-1, deflate-1));
        // QR
        qr.compute(A.topLeftCorner(deflate, deflate) - MatrixType::Identity(deflate, deflate) * shift);
        Q_curr = qr.householderQ();
        // Update Q_H with deflated matmul
        temp.topLeftCorner(deflate, deflate).noalias() = Q_H->topLeftCorner(deflate, deflate) * Q_curr;
        temp.bottomLeftCorner(Q_H->rows() - deflate, deflate).noalias() = Q_H->bottomLeftCorner(Q_H->rows() - deflate, deflate) * Q_curr;
        Q_H->topLeftCorner(Q_H->rows(), deflate) = temp.topLeftCorner(Q_H->rows(), deflate);
        // Update A = Q^* H Q
        A.noalias() = Q_H->adjoint() * ham * *Q_H; // TODO: Deflate this
        // Check convergence condition
        if(std::abs(A(deflate-1, deflate-2)) < max_resid && --deflate == 1) {
            break;
        }
    }

    double residual = 0;
    for(int j = 0; j < A.rows(); ++j) {
        for(int i = j+1; i < A.cols(); ++i) {
            residual += std::real(A(i,j) * std::conj(A(i,j)));
        }
    }
    residual = std::sqrt(residual) / A.diagonal().size();

    std::vector<double> eigenvals(A.diagonal().size());
    Eigen::Map<Eigen::VectorXd>(eigenvals.data(), eigenvals.size()) = A.diagonal().real();
    std::sort(eigenvals.begin(), eigenvals.end());

    return eigenvals;
}

std::vector<double> gpu_hamiltonian_eigenvals(const MatrixType& hamiltonian) {
    syk::GpuEigenValSolver solver;
    std::vector<double> eigenvals;
    #pragma omp critical
    eigenvals = solver.eigenvals(hamiltonian);
    return eigenvals;
}
}


//=======================================================================
// Copyright (c) 2013 Robert Rosolek
//
// 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)
//=======================================================================
/**
 * @file fractional_winner_determination_in_MUCA_test.cpp
 * @brief
 * @author Robert Rosolek
 * @version 1.0
 * @date 2014-07-24
 */

#include "test_utils/fractional_winner_determination_in_MUCA_test_utils.hpp"

#include "paal/auctions/fractional_winner_determination_in_MUCA/fractional_winner_determination_in_MUCA.hpp"
#include "paal/auctions/xor_bids.hpp"

#include <boost/mem_fn.hpp>
#include <boost/test/unit_test.hpp>

#include <functional>
#include <iterator>
#include <map>
#include <set>
#include <string>
#include <utility>
#include <vector>

BOOST_AUTO_TEST_CASE(testFracDetermineWinners)
{
   using Bidder = std::string;
   using Item = std::string;
   using Items = std::set<Item>;
   using Bid = std::pair<Items, double>;
   using Bids = std::vector<Bid>;
   const std::vector<Bidder> bidders {"John", "Bob"};
   const std::map<Bidder, Bids> bids {
      {"John", {
         {Items{}, 0},
         {{"lemon", "orange"}, 1},
         {{"apple", "ball"}, 1},
      }},
      {"Bob", {
         {{"lemon", "apple"}, 1},
         {{"orange", "ball"}, 1},
      }},
   };
   auto get_bids = [&](const Bidder& bidder) -> const Bids& { return bids.at(bidder); };
   auto get_value = boost::mem_fn(&Bid::second);
   auto get_items = boost::mem_fn(&Bid::first);
   auto get_copies_num = [](const Item& item) { return item == "lemon" ? 2 : 1; };
   Items items;
   paal::auctions::extract_items_from_xor_bids(
      bidders, get_bids, get_items, std::inserter(items, items.begin())
   );
   auto opt = 0.5 * (1 + 1 + 1 + 1);
   check_fractional_determine_winners_in_demand_query_auction(
      bidders, items, get_bids, get_value, get_items, get_copies_num, opt
   );
}


#include <stan/math/fwd/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/digamma.hpp>
#include <test/unit/math/fwd/scal/fun/nan_util.hpp>

TEST(AgradFwdDigamma, Fvar) {
  using boost::math::digamma;
  using boost::math::zeta;
  using stan::math::fvar;

  fvar<double> x(0.5, 1.0);
  fvar<double> a = digamma(x);
  EXPECT_FLOAT_EQ(digamma(0.5), a.val_);
  EXPECT_FLOAT_EQ(4.9348022005446793094, a.d_);
}

TEST(AgradFwdDigamma, FvarFvarDouble) {
  using boost::math::digamma;
  using stan::math::fvar;

  fvar<fvar<double> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<double> > a = digamma(x);

  EXPECT_FLOAT_EQ(digamma(0.5), a.val_.val_);
  EXPECT_FLOAT_EQ(4.9348022005446793094, a.val_.d_);
  EXPECT_FLOAT_EQ(0, a.d_.val_);
  EXPECT_FLOAT_EQ(0, a.d_.d_);

  fvar<fvar<double> > y;
  y.val_.val_ = 0.5;
  y.d_.val_ = 1.0;

  a = digamma(y);
  EXPECT_FLOAT_EQ(digamma(0.5), a.val_.val_);
  EXPECT_FLOAT_EQ(0, a.val_.d_);
  EXPECT_FLOAT_EQ(4.9348022005446793094, a.d_.val_);
  EXPECT_FLOAT_EQ(0, a.d_.d_);
}

struct digamma_fun {
  template <typename T0>
  inline T0 operator()(const T0& arg1) const {
    return digamma(arg1);
  }
};

TEST(AgradFwdDigamma, digamma_NaN) {
  digamma_fun digamma_;
  test_nan_fwd(digamma_, false);
}


// Author(s): Frank Stappers
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// 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)
//
/// \file list_test.cpp
/// \brief Basic regression test for quantifier expressions.

#include <boost/test/minimal.hpp>

#include "mcrl2/data/list.h"
#include "mcrl2/data/parse.h"
#include "mcrl2/data/rewriter.h"
#include "mcrl2/data/set.h"
#include "mcrl2/data/fset.h"
#include "mcrl2/data/standard.h"
#include "mcrl2/data/detail/rewrite_strategies.h"

using namespace mcrl2;
using namespace mcrl2::data;
using namespace mcrl2::data::sort_list;

void quantifier_expression_test(const std::string& expr1_text, const std::string& expr2_text, const data_specification& dataspec, const rewriter& r)
{
  data_expression expr1 = parse_data_expression(expr1_text, dataspec);
  data_expression expr2 = parse_data_expression(expr2_text, dataspec);
  std::cout << "Testing " << expr1_text << " <-> " << expr2_text << std::endl;
  if (r(expr1) != r(expr2))
  {
    std::cout << "--- ERROR ---\n";
    std::cout << " expr1    = " << expr1 << std::endl;
    std::cout << " expr2    = " << expr2 << std::endl;
    std::cout << " r(expr1) = " << r(expr1) << std::endl;
    std::cout << " r(expr2) = " << r(expr2) << std::endl;
    BOOST_CHECK(r(expr1) == r(expr2));
  }
}

void quantifier_expression_test(mcrl2::data::rewrite_strategy s)
{
  data_specification dataspec;
  rewriter r(dataspec, s);

  // tests for Bool
  quantifier_expression_test("exists x: Bool. x == false", "true", dataspec, r);
  quantifier_expression_test("forall x: Bool. x == false", "false", dataspec, r);
  quantifier_expression_test("exists x: Bool. x == true", "true", dataspec, r);
  quantifier_expression_test("forall x: Bool. x == true", "false", dataspec, r);
  quantifier_expression_test("forall x: Bool. x == true && x==false", "false", dataspec, r);
  quantifier_expression_test("exists x: Bool. x == true && x==false", "false", dataspec, r);
  quantifier_expression_test("forall x: Bool. x == true || x==false", "true", dataspec, r);
  quantifier_expression_test("exists x: Bool. x == true || x==false", "true", dataspec, r);
  quantifier_expression_test("forall x:Bool.exists y: Bool. x == y", "true", dataspec, r);
  quantifier_expression_test("exists x: Bool.forall y:Bool.x == y", "false", dataspec, r);
  quantifier_expression_test("forall b: Bool. b", "false", dataspec, r);

  // tests for Pos / Nat
  dataspec.add_context_sort(sort_nat::nat());
  dataspec.add_context_sort(sort_set::set_(sort_nat::nat()));
  r = rewriter(dataspec, s);
  quantifier_expression_test("exists x: Nat. (  x in {1,2,25,600} && 25 == x )", "true", dataspec, r);

  // This test depends on the naming of variables in the enumerator
  // quantifier_expression_test("forall x: Nat. exists y: Nat. y == x", "true", dataspec, r);

  quantifier_expression_test("forall x: Nat. x == 3", "false", dataspec, r);
  quantifier_expression_test("exists x: Nat. x == 3", "true", dataspec, r);
  quantifier_expression_test("forall x: Pos. exists y: Pos.x == y+1", "false", dataspec, r);
  quantifier_expression_test("forall x: Pos. exists y: Pos.x == y+1", "false", dataspec, r);
  /* Test 15. Test whether elimination of quantifiers also happens inside a term. */
  quantifier_expression_test("(exists x_0: Bool. false) && (forall x_0: Nat. true)", "false", dataspec, r);
  quantifier_expression_test("(forall x_0: Pos. true) || (exists x_0: Bool. false)", "true", dataspec, r);
  /* The test below is too complex for the enumerator to solve.
  quantifier_expression_test("forall x: Pos. exists y: Nat.x == y+1", "true", dataspec, r);
  */

/* These tests do not work with the new enumerator, since it does not handle data equalities.
  quantifier_expression_test("forall x:Pos.exists y1,y2:Pos.x==y1+y2", "false", dataspec, r);
  quantifier_expression_test("forall x:Nat.exists y1,y2:Nat.x==y1+y2", "true", dataspec, r);
*/

  // tests for struct / List
  dataspec = parse_data_specification("sort S = struct s1?is_s1 | s2?is_s2;"
                                      "sort T = struct t;"
                                      "sort L = List(T);");
  r = rewriter(dataspec, s);

  quantifier_expression_test("exists s: S.( is_s1(s) && is_s2(s) )", "false", dataspec, r);
  quantifier_expression_test("exists s: S.( s == s2 && is_s2(s) )", "true", dataspec, r);
  quantifier_expression_test("forall y: T. y in [t]", "true", dataspec, r);

/* Should work but takes too much time.
  dataspec = parse_data_specification(
  " sort A = struct ac( first: Bool, args: List(Bool));"
  " "
  " map COMM: List(Bool)#Bool#List(A) -> List(A);"
  "     COMM: List(Bool)#Bool#List(A)#(List(Bool)->FBag(Bool))#FBag(Bool) -> List(A);"
  "     MAC: List(Bool) -> FBag(Bool);"
  "     PART: List(A)#(List(Bool) -> FBag(Bool)) -> (List(Bool) -> FBag(Bool));"
  "     ELM: List(Bool)#Bool#List(A)#List(Bool)#List(Bool) -> List(A);"
  "     RM: A#List(A)->List(A);"
  " var func: List(Bool)->FBag(Bool);"
  "     as: List(A);"
  "     ca: Bool;"
  "     cal: List(Bool);"
  "     cal_const: List(Bool);"
  "     al: Bool;"
  "     lsa: List(A);"
  "     m: FBag(Bool);"
  "     args: List(Bool);"
  "     a, b: A;"
  " eqn PART( [] , func ) = func;"
  "     PART( a |> as , func ) = PART( as, func[ args(a) -> func(args(a)) + {first(a):1}]  );"
  "     MAC( [] ) = {:};"
  "     MAC( ca |> cal ) = {ca:1} + MAC(cal);"
  " "
  "     COMM( cal, al, lsa ) = COMM( cal, al, lsa,  PART( lsa, lambda x: List(Bool). {:} ), MAC(cal) );"
  "     COMM( cal, al, [] , func, m ) = [];"
  "     COMM( cal, al, a |> lsa, func, m ) = if( m <= func(args(a)), "
  "                                                      ELM( cal, al, a |> lsa, args(a) , cal ) , "
  "                                                      a |> COMM( cal, al, lsa, func, m)"
  "                                                    );"
  "     ELM( [] , al, lsa, args, cal_const ) = [ac(al, args)] ++ COMM( cal_const, al, lsa );"
  "     ELM( ca |> cal, al, lsa, args, cal_const ) = ELM( cal, al , RM( ac( ca ,args), lsa), args, cal_const );"
  "     RM( a, [] ) = [];"
  "     RM( a, b |> lsa ) = if(a == b , lsa,  b |> RM( a, lsa)) ;"
  );
  r = rewriter(dataspec, s);

  quantifier_expression_test("exists x_0: List(A). x_0 == [ac( false, []), ac(true, []), ac(false, [])] && [ac(true, []), ac(true, [])] == COMM([false, false], true, x_0, PART(x_0, lambda x: List(Bool). {:}), {false: 2}) ", "true", dataspec, r);
  quantifier_expression_test("exists x_0: List(A). [ac(true, []), ac(true, [])] == "
       "        COMM([false, false], true, x_0, PART(x_0, lambda x: List(Bool). {:}), {false: 2}) &&  "
       "        x_0 == [ac( false, []), ac(true, []), ac(false, [])]", "true", dataspec, r);
*/

  // tests for Set
  dataspec = parse_data_specification( "sort A = Set(Bool);");
  r = rewriter(dataspec, s);

  /* Test that exists and forall over a non enumerable sort (situation winter 2012)
     with a trivial predicate can still be reduced, by removing the variable. */
  quantifier_expression_test("exists x:Set(Bool). x==x", "true", dataspec, r);
  quantifier_expression_test("forall x:Set(Bool). x==x", "true", dataspec, r);
  quantifier_expression_test("exists x:Set(Bool). x!=x", "false", dataspec, r);
  quantifier_expression_test("forall x:Set(Bool). x!=x", "false", dataspec, r);
}

void quantifier_in_rewrite_rules_test(mcrl2::data::rewrite_strategy s)
{
  // The test below checks whether bound variables in rewrite rules are properly renamed
  // when substituting variables. In concreto, if the y in the eqn for f is substituted in
  // the body of g(x), then if y is substituted for x, the rhs of g(x) reduces to y!=y, or false.
  // The correct answer however is true, as f states taht for every boolean y there is an y' that
  // is not equal to it.
  data_specification dataspec = parse_data_specification(
                    "map f:Bool;\n"
                    "    g:Bool->Bool;\n"
                    "var x:Bool;\n"
                    "eqn f=forall y:Bool.g(y);\n"
                    "    g(x)=exists y:Bool.x!=y;\n");

  rewriter r(dataspec, s);
  quantifier_expression_test("f", "true", dataspec, r);
}

int test_main(int argc, char** argv)
{
  auto strategies = data::detail::get_test_rewrite_strategies(false);
  for (const auto& strategy: strategies)
  {
    std::clog << "  Strategy: " << strategy << std::endl;
    quantifier_expression_test(strategy);
    quantifier_in_rewrite_rules_test(strategy);
  }

  return EXIT_SUCCESS;
}


#include "tasktorrent/tasktorrent.hpp"
#ifdef USE_MKL
#include <mkl_cblas.h>
#include <mkl_lapacke.h>
#else
#include <cblas.h>
#include <lapacke.h>
#endif
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <fstream>
#include <array>
#include <random>
#include <mutex>
#include <iostream>
#include <map>
#include <memory>
#include <set>
#include <mpi.h>
#include <string>

using namespace std;
using namespace Eigen;
using namespace ttor;

typedef array<int, 2> int2;
typedef array<int, 3> int3;

/*
 * Parametrized priorities for cholesky:
 * 0. No priority, only enforces potrf>trsm>gemm
 * 1. Row-based priority, prioritize tasks with smaller row number in addition to priority 0.
 * 2. Critical path priority, prioritize tasks with longest distance to the exit task. For references, check out the paper
    Beaumont, Olivier, et al. "A Makespan Lower Bound for the Scheduling of the Tiled Cholesky Factorization based on ALAP Schedule." (2020).
 * 3. Critical path and row priority, prioritize tasks with smaller row number in addition to priority 2. We also enforces potrf>trsm>gemm
 */

enum PrioKind { no = 0, row = 1, cp = 2, cp_row = 3};

/*
* 3D Cholesky based on the algorithm from the paper
    Kalluri Eswar, et al. "On Mapping Data and Computation for Parallel Sparse Cholesky Factorization." (1995)
* However, this implementation only uses 3D mapping for gemm tasks. 1D mapping is used for potrf tasks and 2D mapping is used for trsm tasks.
*/

void cholesky3d(int n_threads, int verb, int block_size, int num_blocks, int npcols, int nprows, PrioKind prio, int log, int debug)
{
    const int rank = comm_rank();
    const int n_ranks = comm_size();
    std::atomic<long long int> potrf_us_t(0);
    std::atomic<long long int> trsm_us_t(0);
    std::atomic<long long int> gemm_us_t(0);
    std::atomic<long long int> accu_us_t(0);
    assert(npcols * nprows == n_ranks);
    int q = static_cast<int>(cbrt(n_ranks));
    if (q * q * q != n_ranks)
    {
        if (rank == 0)
        {
            cerr << "Number of processes must be a perfect cube." << endl;
        }
        MPI_Finalize();
        exit(1);
    }

    int3 rank_3d;
    int2 rank_2d;
    rank_3d[0] = rank / (q * q);
    rank_3d[1] = (rank % (q * q)) / q;
    rank_3d[2] = (rank % (q * q)) % q;
    rank_2d[0] = rank % nprows;
    rank_2d[1] = rank / nprows;

    // Number of tasks
    int n_tasks_per_rank = 2;
    struct acc_data {
        vector<std::unique_ptr<MatrixXd>> to_accumulate; // to_accumulate[k] holds matrix result of gemm(k,i,j)
    };

    auto potf_block_2_prio = [&](int j) {
        if (prio == PrioKind::cp_row) {
            return (double)(9 * (num_blocks - j) - 1) + 18 * num_blocks * num_blocks;
        }
        else if(prio == PrioKind::cp) {
            return (double)(9 * (num_blocks - j) - 1);
        }
        else if(prio == PrioKind::row) {
            return 3.0 * (double)(num_blocks - j);
        }
        else {
            return 3.0;
        }
    };

    auto trsm_block_2_prio = [&](int2 ij) {
        if (prio == PrioKind::cp_row) {
            return (double)((num_blocks - ij[0]) + num_blocks * (9.0 * num_blocks - 9.0 * ij[1] - 2.0) + 9 * num_blocks * num_blocks);
        }
        else if(prio == PrioKind::cp) {
            return (double)(9 * (num_blocks - ij[1]) - 2);
        }
        else if(prio == PrioKind::row) {
            return 2.0 * (double)(num_blocks - ij[0]);
        }
        else {
            return 2.0;
        }
    };

    auto gemm_block_2_prio = [&](int3 kij) {
        if (prio == PrioKind::cp_row) {
            return (double)(num_blocks - kij[1]) + num_blocks * (9.0 * num_blocks - 3.0 * kij[2] - 6.0 * (kij[0] / q) - 2.0);
        }
        else if(prio == PrioKind::cp) {
            return (double)(9 * num_blocks - 9 * kij[2] - 2);
        }
        else if(prio == PrioKind::row) {
            return (double)(num_blocks - kij[1]);
        }
        else {
            return 1.0;
        }
    };

    std::vector<acc_data> gemm_results(num_blocks*num_blocks);
    auto val = [&](int i, int j) { return 1.0/(double)((i + j)*(i + j) + 1) + ((i == j) ? 1.0 * block_size * num_blocks : 0.0); };
    auto rank3d21 = [&](int i, int j, int k) { return ((j % q) * q + k % q) + (i % q) * q * q;};
    auto rank2d21 = [&](int i, int j) { return (j % npcols) * nprows + (i % nprows);};
    auto rank1d21 = [&](int j) { return j % n_ranks; };
    vector<unique_ptr<MatrixXd>> blocks(num_blocks*num_blocks);

    auto bloc_2_rank = [&](int i, int j) {
        int r = (j % npcols) * nprows + (i % nprows);
        assert(r >= 0 && r < n_ranks);
        return r;
    };

    auto block_2_thread = [&](int i, int j) {
        int ii = i / nprows;
        int jj = j / npcols;
        int num_blocksit = num_blocks / nprows;
        return (ii + jj * num_blocksit) % n_threads;
    };
    {
        Eigen::MatrixXd A = Eigen::MatrixXd::Identity(256,256);
        Eigen::MatrixXd B = Eigen::MatrixXd::Identity(256,256);
        Eigen::MatrixXd C = Eigen::MatrixXd::Identity(256,256);
        for(int i = 0; i < 10; i++) {
            cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, 256, 256, 256, 1.0, A.data(), 256, B.data(), 256, 1.0, C.data(), 256);
        }
    }
    for (int ii=0; ii<num_blocks; ii++) {
        for (int jj=0; jj<num_blocks; jj++) {
            auto val_loc = [&](int i, int j) { return val(ii*block_size+i,jj*block_size+j); };
            int dest = (ii == jj) ? rank1d21(ii) : rank2d21(ii,jj);
            if(dest == rank) {
                blocks[ii+jj*num_blocks]=make_unique<MatrixXd>(block_size, block_size);
                *blocks[ii+jj*num_blocks]=MatrixXd::NullaryExpr(block_size, block_size, val_loc);
                gemm_results[ii+jj*num_blocks].to_accumulate= vector<std::unique_ptr<MatrixXd>>(q);
                for (int ll=0; ll<q; ll++) {
                    gemm_results[ii+jj*num_blocks].to_accumulate[ll]=make_unique<MatrixXd>(block_size, block_size);
                    *(gemm_results[ii+jj*num_blocks].to_accumulate[ll])=MatrixXd::Zero(block_size, block_size);
                }
            }
            else if (((ii % q) == rank_3d[0]) && ((jj % q) == rank_3d[1])) {
                blocks[ii+jj*num_blocks]=make_unique<MatrixXd>(block_size, block_size);
                *blocks[ii+jj*num_blocks]=MatrixXd::Zero(block_size, block_size);
            }
            else {
                blocks[ii+jj*num_blocks]=make_unique<MatrixXd>(block_size, block_size);
            }
        }
    }
    // Initialize the communicator structure
    Communicator comm(MPI_COMM_WORLD, verb);
    // Initialize the runtime structures
    Threadpool tp(n_threads, &comm, verb, "WkTuto_" + to_string(rank) + "_");
    Taskflow<int> potrf(&tp, verb);
    Taskflow<int2> trsm(&tp, verb);
    Taskflow<int3> gemm(&tp, verb);
    Taskflow<int3> accu(&tp, verb);
    DepsLogger dlog(1000000);
    Logger ttorlog(1000000);
    if (log)  {
        tp.set_logger(&ttorlog);
        comm.set_logger(&ttorlog);
    }
    // Create active message
    auto am_trsm = comm.make_large_active_msg(
        [&](int& j) {
                int offset = ((j + 1) / nprows + (((j + 1) % nprows) > rank_2d[0])) * nprows + rank_2d[0];
                for(int i = offset; i < num_blocks; i = i + nprows) {
                    if (debug) printf("Fulfilling trsm (%d, %d) on rank (%d, %d)\n", i, j, rank_2d[0], rank_2d[1]);
                    assert(rank2d21(i, j) == rank);
                    trsm.fulfill_promise({i,j});
                }
            },
            [&](int& j){
                return blocks[j+j*num_blocks]->data();
            },
            [&](int& j){
                return;
            });

    auto am_gemm = comm.make_large_active_msg(
        [&](int& i, int& k) {
            assert(k % q == rank_3d[2]);
            int offset_c = ((k + 1) / q + (((k + 1) % q) > rank_3d[1])) * q + rank_3d[1];
            if (i % q == rank_3d[0]) {
                for(int j = offset_c; j < i; j = j + q) {
                    if (debug) printf("TRSM (%d, %d) Fulfilling gemm (%d, %d, %d) on rank (%d, %d, %d)\n", i, k, k, i, j, rank_3d[2], rank_3d[0], rank_3d[1]);
                    assert(rank3d21(i,j,k) == rank);
                    gemm.fulfill_promise({k,i,j});
                }
            }
            int offset_r = (i / q + ((i % q) > rank_3d[0])) * q + rank_3d[0];
            if (i % q == rank_3d[1]) {
                for(int j = offset_r; j < num_blocks; j = j + q) {
                    if (debug) printf("TRSM (%d, %d) Fulfilling gemm (%d, %d, %d) on rank (%d, %d, %d)\n", i,k, k, j, i, rank_3d[2], rank_3d[0], rank_3d[1]);
                    assert(rank3d21(j,i,k) == rank);
                    gemm.fulfill_promise({k,j,i});
                }
            }
        },
        [&](int& i, int& k) {
            return blocks[i+k*num_blocks]->data();
        },
        [&](int& i, int& k) {
            return;
        });

    potrf.set_task([&](int j) {
            assert(rank1d21(j) == rank);
            timer t1 = wctime();
            int info = LAPACKE_dpotrf(LAPACK_COL_MAJOR, 'L', block_size, blocks[j+j*num_blocks]->data(), block_size);
            timer t2 = wctime();
            potrf_us_t += 1e6 * elapsed(t1, t2);
            assert(info == 0);
            if (debug) printf("Running POTRF %d on rank %d\n", j, rank);
        })
        .set_fulfill([&](int j) {
            for (int p = 0; p < nprows; p++)
            {
                int r = rank2d21(p,j);
                if (rank == r) {
                    int offset = ((j + 1) / nprows + ((j + 1) % nprows) / (rank_2d[0] + 1)) * nprows + rank_2d[0];
                    for(int i = offset; i < num_blocks; i = i + nprows) {
                        trsm.fulfill_promise({i,j});
                    }
                }
                else {
                    auto Ljjv = view<double>(blocks[j+j*num_blocks]->data(), block_size*block_size);
                    am_trsm->send_large(r, Ljjv, j);
                }
            }
        })
        .set_indegree([&](int j) {
            if (j==0) {
                return 1;
            }
            else if (j < q) {
                return j;
            }
            else {
                return q;
            }
        })
        .set_mapping([&](int j) {
            return block_2_thread(j,j);
        })
        .set_binding([&](int j) {
            return false;

        })
        .set_priority(potf_block_2_prio)
        .set_name([&](int j) {
            return "POTRF" + to_string(j) + "_" + to_string(rank);
        });

    trsm.set_task([&](int2 ij) {
            int i=ij[0];
            int j=ij[1];
            assert(rank2d21(i,j) == rank);
            timer t1 = wctime();
            cblas_dtrsm(CblasColMajor, CblasRight, CblasLower, CblasTrans, CblasNonUnit, block_size, block_size, 1.0, blocks[j + j * num_blocks]->data(),block_size, blocks[i + j * num_blocks]->data(), block_size);
            timer t2 = wctime();
            trsm_us_t += 1e6 * elapsed(t1, t2);
            if (debug) printf("Running trsm (%d, %d) on rank %d, %d\n", i, j, rank_2d[0], rank_2d[1]);
        })
        .set_fulfill([&](int2 ij) {
            int i=ij[0];
            int j=ij[1];
            for (int ri = 0; ri < q; ri++)  {
                for (int rj = 0; rj < q; rj++) {
                    int r = rank3d21(ri, rj, j);
                    if (r == rank) {
                        int offset_c = ((j + 1) / q + (((j + 1) % q) > rank_3d[1])) * q + rank_3d[1];
                        if (i % q == rank_3d[0]) {
                            for(int k = offset_c; k < i; k = k + q) {
                                gemm.fulfill_promise({j,i,k});
                            }
                        }
                        int offset_r = (i / q + ((i % q) > rank_3d[0])) * q + rank_3d[0];
                        if (i % q == rank_3d[1]) {
                            for(int k = offset_r; k < num_blocks; k = k + q) {
                                gemm.fulfill_promise({j,k,i});
                            }
                        }
                    }
                    else {
                        auto Lijv = view<double>(blocks[i + j * num_blocks]->data(), block_size*block_size);
                        am_gemm->send_large(r, Lijv, i, j);
                    }
                }
            }
        })
        .set_indegree([&](int2 ij) {
            int i=ij[0];
            int j=ij[1];
            return (j < q) ? j + 1 : q + 1;
        })
        .set_mapping([&](int2 ij) {
            int i=ij[0];
            int j=ij[1];
            return block_2_thread(i,j);
        })
        .set_binding([&](int2 ij) {
            int i=ij[0];
            int j=ij[1];
            return false;

        })
        .set_priority(trsm_block_2_prio)
        .set_name([&](int2 ij) {
            int i=ij[0];
            int j=ij[1];
            return "TRSM" + to_string(j) + "_" + to_string(i) + "_" +to_string(rank);
        });

    auto am_accu = comm.make_large_active_msg(
        [&](int& i, int& j, int& from) {
            accu.fulfill_promise({from, i, j});
        },
        [&](int& i, int& j, int& from){
            return gemm_results[i+j*num_blocks].to_accumulate[from]->data();
        },
        [&](int& i, int& j, int& from){
            return;
        });

    gemm.set_task([&](int3 kij) {
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            assert(rank3d21(i,j,k) == rank);
            timer t1 = wctime();
            if (i==j) {
                cblas_dsyrk(CblasColMajor, CblasLower, CblasNoTrans, block_size, block_size, -1.0, blocks[i+k*num_blocks]->data(), block_size, 1.0, blocks[i+j*num_blocks]->data(), block_size);
            }
            else {
                cblas_dgemm(CblasColMajor, CblasNoTrans, CblasTrans, block_size, block_size, block_size, -1.0,blocks[i+k*num_blocks]->data(), block_size, blocks[j+k*num_blocks]->data(), block_size, 1.0, blocks[i+j*num_blocks]->data(), block_size);
            }
            timer t2 = wctime();
            if (debug) printf("Running gemm (%d, %d, %d) on rank %d, %d, %d\n", k, i, j, rank_3d[2], rank_3d[0], rank_3d[1]);
            gemm_us_t += 1e6 * elapsed(t1, t2);
        })
        .set_fulfill([&](int3 kij) {
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            if (k+q<=j-1) {
                gemm.fulfill_promise({k+q, i, j});
            }
            else {
                int dest = (i == j) ? rank1d21(i) : rank2d21(i, j);
                if (dest == rank) {
                    if (debug) printf("gemm (%d, %d, %d) fulfilling accumu (%d, %d, %d) on rank %d, %d, %d\n", k, i, j, rank_3d[2], i, j, rank_3d[2], rank_3d[0], rank_3d[1]);
                    accu.fulfill_promise({rank_3d[2], i, j});
                }
                else {
                    int kk = rank_3d[2];
                    auto Lij = view<double>(blocks[i+j*num_blocks]->data(), block_size*block_size);
                    if (debug) printf("gemm (%d, %d, %d) Sending accumu (%d, %d, %d) to rank %d, %d\n", k, i, j, rank_3d[2], i, j, dest % nprows, dest / nprows);
                    am_accu->send_large(dest, Lij, i, j, kk);
                }
            }
        })
        .set_indegree([&](int3 kij) {
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            return 3 - (k/q == 0) - (i == j);
        })
        .set_mapping([&](int3 kij) {
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            return block_2_thread(i,j);
        })
        .set_binding([&](int3 kij) {
            return false;

        })
        .set_priority(gemm_block_2_prio)
        .set_name([&](int3 kij) { // This is just for debugging and profiling
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            return "gemm" + to_string(k) + "_" + to_string(i)+"_"+to_string(j)+"_"+to_string(comm_rank());
        });

    accu.set_task([&](int3 kij) {
            int k=kij[0]; // Step (gemm's pivot)
            int i=kij[1]; // Row
            int j=kij[2]; // Col
            int dest = (i == j) ? rank1d21(i) : rank2d21(i,j);
            assert(dest == rank);
            assert(j <= i);
            if (debug) printf("Running accumu (%d, %d, %d) on rank %d, %d\n", k, i, j, rank % nprows, rank / nprows);
            {
                timer t_ = wctime();
                *blocks[i+j*num_blocks] += (*gemm_results[i+j*num_blocks].to_accumulate[k]);
                timer t__ = wctime();
                accu_us_t += 1e6 * elapsed(t_, t__);
            }
        })
        .set_fulfill([&](int3 kij) {
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            assert(j <= i);
            if(i == j) {
                potrf.fulfill_promise(i);
            } else {
                trsm.fulfill_promise({i,j});
            }
        })
        .set_indegree([&](int3 kij) {
            return 1;
        })
        .set_mapping([&](int3 kij) {
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            return block_2_thread(i,j);
        })
        .set_priority(gemm_block_2_prio)
        .set_binding([&](int3 kij) {
            return true; // IMPORTANT
        })
        .set_name([&](int3 kij) { // This is just for debugging and profiling
            int k=kij[0];
            int i=kij[1];
            int j=kij[2];
            return "accumu" + to_string(k) + "_" + to_string(i)+"_"+to_string(j)+"_"+to_string(comm_rank());
        });

    MPI_Barrier(MPI_COMM_WORLD);
    timer t0 = wctime();
    if (rank == 0){
        potrf.fulfill_promise(0);
    }
    tp.join();
    MPI_Barrier(MPI_COMM_WORLD);
    timer t1 = wctime();
    MPI_Status status;
    if (rank==0) {
        cout<<"3D, Number of ranks "<<n_ranks<<", block_size "<<block_size<<", num_blocks "<<num_blocks<<", n_threads "<<n_threads<<", Priority "<<prio<<", Elapsed time: "<<elapsed(t0,t1)<<endl;
    }
    MatrixXd A;
    A = MatrixXd::NullaryExpr(block_size*num_blocks,block_size*num_blocks, val);
    MatrixXd L = A;
    for (int ii=0; ii<num_blocks; ii++) {
        for (int jj=0; jj<num_blocks; jj++) {
            if (jj<=ii)  {
                int dest = (ii == jj) ? rank1d21(ii) : rank2d21(ii,jj);
                if (rank==0 && rank!=dest) {
                    blocks[ii+jj*num_blocks]=make_unique<MatrixXd>(block_size,block_size);
                    MPI_Recv(blocks[ii+jj*num_blocks]->data(), block_size*block_size, MPI_DOUBLE, dest, 0, MPI_COMM_WORLD, &status);
                }
                else if (rank==dest && rank != 0) {
                    MPI_Send(blocks[ii+jj*num_blocks]->data(), block_size*block_size, MPI_DOUBLE, 0, 0, MPI_COMM_WORLD);
                }
            }
        }
    }
    if (rank == 0)  {
        for (int ii=0; ii<num_blocks; ii++) {
            for (int jj=0; jj<num_blocks; jj++) {
                if (jj<=ii)  {
                    L.block(ii*block_size,jj*block_size,block_size,block_size)=*blocks[ii+jj*num_blocks];
                }
            }
        }
        auto L1=L.triangularView<Lower>();
        VectorXd x = VectorXd::Random(block_size * num_blocks);
        VectorXd b = A*x;
        L1.solveInPlace(b);
        L1.transpose().solveInPlace(b);
        double error = (b - x).norm() / x.norm();
        cout << "Error solve: " << error << endl;
    }
    if (log)  {
        std::ofstream logfile;
        string filename = "ttor_3Dcholesky_Priority_"+to_string(block_size)+"_"+to_string(num_blocks)+"_"+ to_string(n_threads)+"_"+ to_string(n_ranks)+"_"+ to_string(prio)+".log."+to_string(rank);
        logfile.open(filename);
        logfile << ttorlog;
        logfile.close();
    }
}

int main(int argc, char **argv)
{
    int req = MPI_THREAD_FUNNELED;
    int prov = -1;
    MPI_Init_thread(NULL, NULL, req, &prov);
    assert(prov == req);
    int n_threads = 2;
    int verb = 0; // Can be changed to vary the verbosity of the messages
    int block_size = 5;
    int num_blocks = 10;
    int npcols = 1;
    int nprows = ttor::comm_size();
    PrioKind prio = PrioKind::no;
    int log = 0;
    int debug = 0;
    if (argc >= 2)
    {
        block_size = atoi(argv[1]);
        assert(block_size > 0);
    }
    if (argc >= 3)
    {
        num_blocks = atoi(argv[2]);
        assert(num_blocks > 0);
    }
    if (argc >= 5) {
        n_threads=atoi(argv[3]);
        assert(n_threads > 0);
        verb=atoi(argv[4]);
        assert(verb >= 0);
    }
    if (argc >= 7) {
        npcols=atoi(argv[5]);
        assert(npcols > 0);
        nprows=atoi(argv[6]);
        assert(nprows > 0);
    }
    if (argc >= 8) {
        prio=(PrioKind)atoi(argv[7]);
        assert(prio >= 0 && prio < 4);
    }
    if (argc >= 9) {
        log = atoi(argv[9]);
        assert(log == 0 || log == 1);
    }
    if (argc >= 10) {
        debug = atoi(argv[10]);
        assert(debug == 0 || debug == 1);
    }
    if(comm_rank() == 0) printf("Usage: ./3d_cholesky block_size num_blocks n_threads verb nprows npcols priority log debug\n");
    cholesky3d(n_threads, verb, block_size, num_blocks, npcols, nprows, prio, log, debug);
    MPI_Finalize();
}


#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <json.hpp>

using namespace Eigen;
using namespace std;
using json = nlohmann::json;

// Converts an std::vector of std::vector to Eigen Matrix
template <typename T>
Matrix<T, Dynamic, Dynamic> vv_to_matrix(vector<vector<T>> vv){
    Matrix <T, Dynamic, Dynamic>  m (vv.size(), vv[0].size());
    for (int i = 0; i< vv.size(); i++){
        for (int j = 0; j < vv[i].size(); j++){
            m(i,j)= vv[i][j];
        }
    }
    return m;
}

// Converts a std::vector to Eigen Vector
template <typename T>
Vector<T,Dynamic> std_vector_to_eigen(vector <T> v){
    Vector<T,Dynamic> ev (v.size());
    for (int i = 0; i< v.size(); i++){
        ev(i)= v[i];
    }
    return ev;
}

VectorXd ReLu(VectorXd x){
    for (int i = 0; i < x.size(); i++)
        if (x(i) < 0) x(i) = 0;
    return x;
}

class DoubleLayerMLP{
    private:
        MatrixXd w1;
        VectorXd b1;
        MatrixXd w2;
        VectorXd b2;
    public:

        DoubleLayerMLP(MatrixXd w1, VectorXd b1, MatrixXd w2, VectorXd b2){
            this->w1 = w1;
            this->b1 = b1;
            this->w2 = w2;
            this->b2 = b2;
        }

        DoubleLayerMLP(const DoubleLayerMLP & nn){
            this->w1 = nn.w1;
            this->b1 = nn.b1;
            this->w2 = nn.w2;
            this->b2 = nn.b2;
        }

        DoubleLayerMLP(){
            this->w1 = MatrixXd();
            this->b1 = VectorXd();
            this->w2 = MatrixXd();
            this->b2 = VectorXd();
        }

        DoubleLayerMLP(json json_data, string s){
            vector<vector<double>> vv1 = json_data[s]["w1"].get<vector<vector<double>>>();
            MatrixXd w1 = vv_to_matrix(vv1);

            vector<vector<double>> vv2 = json_data[s]["w2"].get<vector<vector<double>>>();
            MatrixXd w2 = vv_to_matrix(vv2);

            vector<double> v1 = json_data[s]["b1"].get<vector<double>>();
            VectorXd b1 = std_vector_to_eigen(v1);

            vector<double> v2 = json_data[s]["b2"].get<vector<double>>();
            VectorXd b2 = std_vector_to_eigen(v2);

            this->w1 = w1;
            this->b1 = b1;
            this->w2 = w2;
            this->b2 = b2;
        }

        VectorXd forward(VectorXd x){
            //cout<< endl << x << endl;
            //cout<< endl << w1 << endl;
            //cout<< endl << w1 * x << endl;

            return w2*ReLu(w1 * x + b1) + b2;
        }
};

class MPNN{
    private:
        int V_attributes;
        int E_attributes;
        DoubleLayerMLP edge_update_nn;
        DoubleLayerMLP vertice_update_nn;
        DoubleLayerMLP output_update_nn;
        int V_hidden;
        int E_hidden;
    public:
        MPNN(int V_attributes, int E_attributes, DoubleLayerMLP edge_update_nn, DoubleLayerMLP vertice_update_nn, DoubleLayerMLP output_update_nn, int V_hidden = 0, int E_hidden = 0){
                this -> V_attributes = V_attributes;
                this -> E_attributes = E_attributes;
                this -> edge_update_nn = edge_update_nn;
                this -> vertice_update_nn = vertice_update_nn;
                this -> output_update_nn = output_update_nn;
                this -> V_hidden = V_hidden ? V_hidden : V_attributes;
                this -> E_hidden = E_hidden ? E_hidden : E_attributes;
            }
        tuple<MatrixXd, MatrixXd, VectorXd>  forward (MatrixXd E, MatrixXi E_V, MatrixXd V){
            int E_n = E.rows();
            int V_n = V.rows();

            MatrixXd E_new = MatrixXd::Zero (E_n, this->E_hidden);
            MatrixXd V_new = MatrixXd::Zero (V_n, this->V_hidden);

            for (int i = 0; i < E_n; i++){
                VectorXd E_concat(V.cols() + V.cols() + E.cols());
                E_concat << V.row(E_V(i,0)).transpose(), V.row(E_V(i,1)).transpose(), E.row(i).transpose();

                //cout<<V.row(E_V(i,0)) <<endl;
                E_new.row(i) = this->edge_update_nn.forward(E_concat);
            }

            VectorXd V_agregated = VectorXd::Zero(this->V_hidden);

            for (int i = 0; i < V_n; i++){
                VectorXd E_agregated = VectorXd::Zero(this->E_hidden);
                for (int j = 0; j < E_n; j++){
                    if (E(j,1) == i)
                        E_agregated += E_new.row(i);
                }
                VectorXd V_concat(E_agregated.size() + V.cols());
                V_concat << E_agregated, V.row(i).transpose();

                V_new.row(i) = this->vertice_update_nn.forward(V_concat);
                V_agregated += V_new.row(i);
            }

            tuple<MatrixXd, MatrixXd, VectorXd> res = tuple<MatrixXd, MatrixXd, VectorXd> (E_new, V_new, this->output_update_nn.forward(V_agregated));
            return res;
        }
};

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

    std::ifstream ifs ("weights.json", std::ifstream::in);
    json json_data = json::parse(ifs);

    DoubleLayerMLP edge_update_nn = DoubleLayerMLP(json_data, "edge_update");
    DoubleLayerMLP vertice_update_nn = DoubleLayerMLP(json_data, "vertice_update");
    DoubleLayerMLP output_update_nn = DoubleLayerMLP(json_data, "output_update");
    MPNN model = MPNN(3, 3, edge_update_nn, vertice_update_nn, output_update_nn);
    std::ifstream graphifstream ("test_graph.json", std::ifstream::in);
    json json_graph = json::parse(graphifstream);

    vector<vector<double>> E_vv = json_graph["E"].get<vector<vector<double>>>();
    vector<vector<int>> V_E_vv = json_graph["V_E"].get<vector<vector<int>>>();
    vector<vector<double>> V_vv = json_graph["V"].get<vector<vector<double>>>();

    MatrixXd E = vv_to_matrix<double>(E_vv);
    MatrixXi V_E = vv_to_matrix<int> (V_E_vv);
    MatrixXd V = vv_to_matrix<double>(V_vv);

    tuple<MatrixXd, MatrixXd, VectorXd> res = model.forward(E,V_E,V);
    MatrixXd new_E = get<0> (res);
    MatrixXd new_V = get<1> (res);
    MatrixXd new_u = get<2> (res);
    cout << "Edge attributes: "<< endl << new_E  << endl;
    cout << "Vertice attributes: "<< endl << new_V << endl;
    cout << "Global attributes: "<< endl << new_u << endl;
}


/* p_integrand.c */
#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <boost/math/special_functions/gamma.hpp>
extern "C"
{
/* Survival function for R^phi*W */
int RW_marginal_C(double *xval, double phi, double gamma, int n_xval, double *result){
    double tmp2 = pow(gamma/2, phi)/boost::math::tgamma(0.5);
    double tmp1, tmp0, a;
    a = 0.5-phi;

    for(int i=0; i<n_xval; i++){
        tmp1 = gamma/(2*pow(xval[i],1/phi));
        tmp0 = tmp2/(a*xval[i]);
        result[i] = boost::math::gamma_p(0.5L,tmp1) + boost::math::tgamma((long double)(a+1),tmp1)*tmp0-pow(tmp1,a)*exp(-tmp1)*tmp0;
    }
    return 1;
}

/* Marginal distribution function for R^phi*W + epsilon */
int pRW_me_interp_C(double *xval, double *xp, double *surv_p, double tau_sqd, double phi, double gamma, int n_xval, int n_grid, double *result){
    bool tau_bool = (tau_sqd > 0.05);
    double tp[n_grid];
    double integrand_p[n_grid];
    double tmp, tmp_res; /* temporary constant */
    double tmp_sum = 0; /* temporary trapesoid sum */
    double sd = sqrt(tau_sqd);
    double sd_const = sqrt(2)*sd;
    double sd_const_pi =sqrt(2*M_PI)*sd;
    int i,j, tmp_int; /* iterative constants */

    for (i = 0; i < n_xval; i++) {
        if(tau_bool & (xval[i]<820)){
            /* Calculate integrand on a grid */
            for(j=0; j<n_grid;j++){
                tmp = xval[i]-xp[j];
                tp[j] = tmp;
                integrand_p[j] = exp(-tmp*tmp/(2*tau_sqd)) * surv_p[j];
            }

            /* Numerical integral using the trapesoid method */
            for(j=0; j<(n_grid-1);j++){
                tmp_sum+= (tp[j+1]-tp[j])*(integrand_p[j] + integrand_p[j+1])/2;
            }
            tmp_res = 0.5*erfc(-xval[i]/sd_const)-tmp_sum/sd_const_pi;
            tmp_sum = 0;

            /* CDF value must be greater than 0 */
            if(tmp_res < 0){
                tmp_res = 0;
            }
            result[i] = tmp_res;
        }
        else{
            tmp_int = RW_marginal_C(&xval[i], phi, gamma, 1, &tmp_res);
            result[i] = 1-tmp_res;
        }
    }

    return 1;
}

/* Get the quantile range for certain probability levels */
int find_xrange_pRW_me_C(double min_p, double max_p, double min_x, double max_x, double *xp, double *surv_p, double tau_sqd, double phi, double gamma, int n_grid, double *x_range){
    if (min_x >= max_x){
        printf("Initial value of mix_x must be smaller than max_x.\n");
        exit(EXIT_FAILURE);
    }

    /* First the min */
    double p_min_x;
    int tmp_int;
    tmp_int = pRW_me_interp_C(&min_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_min_x);
    while (p_min_x > min_p){
        min_x = min_x-40/phi;
        tmp_int = pRW_me_interp_C(&min_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_min_x);
    }

    x_range[0] = min_x;

    /* Now the max */
    double p_max_x;
    tmp_int = pRW_me_interp_C(&max_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_max_x);
    while (p_max_x < max_p){
        max_x = max_x*2; /* Upper will set to 20 initially */
        tmp_int = pRW_me_interp_C(&max_x, xp, surv_p, tau_sqd, phi, gamma, 1, n_grid, &p_max_x);
    }

    x_range[1] = max_x;
    return 1;
}

/* Density function for R^phi*W */
int RW_density_C(double *xval, double phi, double gamma, int n_xval, double *result){
    double tmp2 = pow(gamma/2, phi)/boost::math::tgamma(0.5);
    double tmp1, tmp0, a;
    a = 0.5-phi;

    for(int i=0; i<n_xval; i++){
        tmp1 = gamma/(2*pow(xval[i],1/phi));
        tmp0 = tmp2/(a*pow(xval[i],2));
        result[i] = (boost::math::tgamma((long double)(a+1),tmp1)-pow(tmp1,a)*exp(-tmp1))*tmp0;
    }
    return 1;
}

/* Marginal density function for R^phi*W + epsilon */
int dRW_me_interp_C(double *xval, double *xp, double *den_p, double tau_sqd, double phi, double gamma, int n_xval, int n_grid, double *result){
    double thresh_large = 820;
    if(tau_sqd < 1) {
        thresh_large = 50;
    }
    bool tau_bool = (tau_sqd > 0.05);

    double tp[n_grid];
    double integrand_p[n_grid];
    double tmp, tmp_res; /* temporary constant */
    double tmp_sum = 0; /* temporary trapesoid sum */
    double sd = sqrt(tau_sqd);
    double sd_const_pi =sqrt(2*M_PI)*sd;
    int i,j, tmp_int; /* iterative constants */

    for (i = 0; i < n_xval; i++) {
        if(tau_bool & (xval[i]<thresh_large)){
            /* Calculate integrand on a grid */
            for(j=0; j<n_grid;j++){
                tmp = xval[i]-xp[j];
                tp[j] = tmp;
                integrand_p[j] = exp(-tmp*tmp/(2*tau_sqd)) * den_p[j];
            }

            /* Numerical integral using the trapesoid method */
            for(j=0; j<(n_grid-1);j++){
                tmp_sum+= (tp[j+1]-tp[j])*(integrand_p[j] + integrand_p[j+1])/2;
            }
            tmp_res = tmp_sum/sd_const_pi;
            tmp_sum = 0;
            result[i] = tmp_res;
        }else if((tau_bool & (xval[i]>=thresh_large))|(!tau_bool & (xval[i]>0))){
            tmp_int = RW_density_C(&xval[i], phi, gamma, 1, &tmp_res);
            result[i] = tmp_res;
        }else{
            result[i] = 0;
        }
    }

    return 1;
}

}


/**
 * @file iqn_test.cpp
 * @author Marcus Edel
 *
 * Test file for IQN (incremental Quasi-Newton).
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/optimizers/iqn/iqn.hpp>
#include <mlpack/methods/logistic_regression/logistic_regression.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack::optimization;
using namespace mlpack::distribution;
using namespace mlpack::regression;
using namespace mlpack;

BOOST_AUTO_TEST_SUITE(IQNTest);

/**
 * Run IQN on logistic regression and make sure the results are acceptable.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionTest)
{
  // Generate a two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 1.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("9.0 9.0 9.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(3, 1000);
  arma::Row<size_t> responses(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    data.col(i) = g1.Random();
    responses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    data.col(i) = g2.Random();
    responses[i] = 1;
  }

  // Shuffle the dataset.
  arma::uvec indices = arma::shuffle(arma::linspace<arma::uvec>(0,
      data.n_cols - 1, data.n_cols));
  arma::mat shuffledData(3, 1000);
  arma::Row<size_t> shuffledResponses(1000);
  for (size_t i = 0; i < data.n_cols; ++i)
  {
    shuffledData.col(i) = data.col(indices[i]);
    shuffledResponses[i] = responses[indices[i]];
  }

  // Create a test set.
  arma::mat testData(3, 1000);
  arma::Row<size_t> testResponses(1000);
  for (size_t i = 0; i < 500; ++i)
  {
    testData.col(i) = g1.Random();
    testResponses[i] = 0;
  }
  for (size_t i = 500; i < 1000; ++i)
  {
    testData.col(i) = g2.Random();
    testResponses[i] = 1;
  }

  // Now run SGDR with snapshot ensembles on a couple of batch sizes.
  for (size_t batchSize = 1; batchSize < 9; batchSize += 4)
  {
    IQN iqn(0.01, batchSize, 5000, 1e-3);
    LogisticRegression<> lr(shuffledData, shuffledResponses, iqn, 0.5);

    // Ensure that the error is close to zero.
    const double acc = lr.ComputeAccuracy(data, responses);
    BOOST_REQUIRE_CLOSE(acc, 100.0, 1.3); // 1.3% error tolerance.

    const double testAcc = lr.ComputeAccuracy(testData, testResponses);
    BOOST_REQUIRE_CLOSE(testAcc, 100.0, 1.6); // 1.6% error tolerance.
  }
}

BOOST_AUTO_TEST_SUITE_END();