#pragma once
/* This code is a stripped down version of 'google/spherical-harmonics' on
 * github to focus on spherical harmonics rotations using Eigen. This part
 * of the code is release using the Apache license V2.0 (see [here][license])
 *
 * Modifications:
 *  + I added a new Apply interface to the Rotation class that takes Eigen
 *    vectors as input and outputs to perform efficient Matrix products.
 *    See Rotation::Apply(const Eigen::VectorXf&, Eigen::VectorXf&) const.
 *  + I changed all Eigen type to be 32b float instead of 64b.
 *
 * [license]: https://github.com/google/spherical-harmonics/blob/master/LICENSE
 */

// Include Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>

// Include STL
#include <vector>
#include <memory>

template <class T>
using VectorX = Eigen::Matrix<T, Eigen::Dynamic, 1>;

// Get the total number of coefficients for a function represented by
// all spherical harmonic basis of degree <= @order (it is a point of
// confusion that the order of an SH refers to its degree and not the order).
constexpr int GetCoefficientCount(int order) {
  return (order + 1) * (order + 1);
}

// Get the one dimensional index associated with a particular degree @l
// and order @m. This is the index that can be used to access the Coeffs
// returned by SHSolver.
constexpr int GetIndex(int l, int m) {
  return l * (l + 1) + m;
}

// Usage: CHECK(bool, string message);
// Note that it must end a semi-colon, making it look like a
// valid C++ statement (hence the awkward do() while(false)).
#ifndef NDEBUG
# define CHECK(condition, message) \
  do { \
    if (!(condition)) { \
      std::cerr << "Check failed (" #condition ") in " << __FILE__ \
        << ":" << __LINE__ << ", message: " << message << std::endl; \
      std::exit(EXIT_FAILURE); \
    } \
  } while(false)
#else
# define ASSERT(condition, message) do {} while(false)
#endif

// Return true if the first value is within epsilon of the second value.
bool NearByMargin(double actual, double expected) {
  double diff = actual - expected;
  if (diff < 0.0) {
    diff = -diff;
  }
  // 5 bits of error in mantissa (source of '32 *')
  return diff < 32 * std::numeric_limits<double>::epsilon();
}

// ---- The following functions are used to implement SH rotation computations
//      based on the recursive approach described in [1, 4]. The names of the
//      functions correspond with the notation used in [1, 4].

// See http://en.wikipedia.org/wiki/Kronecker_delta
double KroneckerDelta(int i, int j) {
  if (i == j) {
    return 1.0;
  } else {
    return 0.0;
  }
}

// [4] uses an odd convention of referring to the rows and columns using
// centered indices, so the middle row and column are (0, 0) and the upper
// left would have negative coordinates.
//
// This is a convenience function to allow us to access an Eigen::MatrixXf
// in the same manner, assuming r is a (2l+1)x(2l+1) matrix.
double GetCenteredElement(const Eigen::MatrixXf& r, int i, int j) {
  // The shift to go from [-l, l] to [0, 2l] is (rows - 1) / 2 = l,
  // (since the matrix is assumed to be square, rows == cols).
  int offset = (r.rows() - 1) / 2;
  return r(i + offset, j + offset);
}

// P is a helper function defined in [4] that is used by the functions U, V, W.
// This should not be called on its own, as U, V, and W (and their coefficients)
// select the appropriate matrix elements to access (arguments @a and @b).
double P(int i, int a, int b, int l, const std::vector<Eigen::MatrixXf>& r) {
  if (b == l) {
    return GetCenteredElement(r[1], i, 1) *
        GetCenteredElement(r[l - 1], a, l - 1) -
        GetCenteredElement(r[1], i, -1) *
        GetCenteredElement(r[l - 1], a, -l + 1);
  } else if (b == -l) {
    return GetCenteredElement(r[1], i, 1) *
        GetCenteredElement(r[l - 1], a, -l + 1) +
        GetCenteredElement(r[1], i, -1) *
        GetCenteredElement(r[l - 1], a, l - 1);
  } else {
    return GetCenteredElement(r[1], i, 0) * GetCenteredElement(r[l - 1], a, b);
  }
}

// The functions U, V, and W should only be called if the correspondingly
// named coefficient u, v, w from the function ComputeUVWCoeff() is non-zero.
// When the coefficient is 0, these would attempt to access matrix elements that
// are out of bounds. The list of rotations, @r, must have the @l - 1
// previously completed band rotations. These functions are valid for l >= 2.

double U(int m, int n, int l, const std::vector<Eigen::MatrixXf>& r) {
  // Although [1, 4] split U into three cases for m == 0, m < 0, m > 0
  // the actual values are the same for all three cases
  return P(0, m, n, l, r);
}

double V(int m, int n, int l, const std::vector<Eigen::MatrixXf>& r) {
  if (m == 0) {
    return P(1, 1, n, l, r) + P(-1, -1, n, l, r);
  } else if (m > 0) {
    return P(1, m - 1, n, l, r) * sqrt(1 + KroneckerDelta(m, 1)) -
        P(-1, -m + 1, n, l, r) * (1 - KroneckerDelta(m, 1));
  } else {
    // Note there is apparent errata in [1,4,4b] dealing with this particular
    // case. [4b] writes it should be P*(1-d)+P*(1-d)^0.5
    // [1] writes it as P*(1+d)+P*(1-d)^0.5, but going through the math by hand,
    // you must have it as P*(1-d)+P*(1+d)^0.5 to form a 2^.5 term, which
    // parallels the case where m > 0.
    return P(1, m + 1, n, l, r) * (1 - KroneckerDelta(m, -1)) +
        P(-1, -m - 1, n, l, r) * sqrt(1 + KroneckerDelta(m, -1));
  }
}

double W(int m, int n, int l, const std::vector<Eigen::MatrixXf>& r) {
  if (m == 0) {
    // whenever this happens, w is also 0 so W can be anything
    return 0.0;
  } else if (m > 0) {
    return P(1, m + 1, n, l, r) + P(-1, -m - 1, n, l, r);
  } else {
    return P(1, m - 1, n, l, r) - P(-1, -m + 1, n, l, r);
  }
}

// Calculate the coefficients applied to the U, V, and W functions. Because
// their equations share many common terms they are computed simultaneously.
void ComputeUVWCoeff(int m, int n, int l, double* u, double* v, double* w) {
  double d = KroneckerDelta(m, 0);
  double denom = (abs(n) == l ? 2.0 * l * (2.0 * l - 1) : (l + n) * (l - n));

  *u = sqrt((l + m) * (l - m) / denom);
  *v = 0.5 * sqrt((1 + d) * (l + abs(m) - 1.0) * (l + abs(m)) / denom)
      * (1 - 2 * d);
  *w = -0.5 * sqrt((l - abs(m) - 1) * (l - abs(m)) / denom) * (1 - d);
}

// Calculate the (2l+1)x(2l+1) rotation matrix for the band @l.
// This uses the matrices computed for band 1 and band l-1 to compute the
// matrix for band l. @rotations must contain the previously computed l-1
// rotation matrices, and the new matrix for band l will be appended to it.
//
// This implementation comes from p. 5 (6346), Table 1 and 2 in [4] taking
// into account the corrections from [4b].
void ComputeBandRotation(int l, std::vector<Eigen::MatrixXf>* rotations) {
  // The band's rotation matrix has rows and columns equal to the number of
  // coefficients within that band (-l <= m <= l implies 2l + 1 coefficients).
  Eigen::MatrixXf rotation(2 * l + 1, 2 * l + 1);
  for (int m = -l; m <= l; m++) {
    for (int n = -l; n <= l; n++) {
      double u, v, w;
      ComputeUVWCoeff(m, n, l, &u, &v, &w);

      // The functions U, V, W are only safe to call if the coefficients
      // u, v, w are not zero
      if (!NearByMargin(u, 0.0))
          u *= U(m, n, l, *rotations);
      if (!NearByMargin(v, 0.0))
          v *= V(m, n, l, *rotations);
      if (!NearByMargin(w, 0.0))
          w *= W(m, n, l, *rotations);

      rotation(m + l, n + l) = (u + v + w);
    }
  }

  rotations->push_back(rotation);
}

class Rotation {
 public:
/*
  // Create a new Rotation that can applies @rotation to sets of coefficients
  // for the given @order. @order must be at least 0.
  static std::unique_ptr<Rotation> Create(int order,
                                          const Eigen::Quaternionf& rotation);

  // Create a new Rotation that applies the same rotation as @rotation. This
  // can be used to efficiently calculate the matrices for the same 3x3
  // transform when a new order is necessary.
  static std::unique_ptr<Rotation> Create(int order, const Rotation& rotation);
*/
  // Transform the SH basis coefficients in @coeff by this rotation and store
  // them into @result. These may be the same vector. The @result vector will
  // be resized if necessary, but @coeffs must have its size equal to
  // GetCoefficientCount(order()).
  //
  // This rotation transformation produces a set of coefficients that are equal
  // to the coefficients found by projecting the original function rotated by
  // the same rotation matrix.
  //
  // There are explicit instantiations for double, float, and Array3f.
  template <typename T>
  void Apply(const std::vector<T>& coeffs,  std::vector<T>* result) const;
  void Apply(const Eigen::MatrixXf& coeffs, Eigen::MatrixXf& result) const;
  void Apply(const Eigen::VectorXf& coeffs, Eigen::VectorXf& result) const;

  // The order (0-based) that the rotation was constructed with. It can only
  // transform coefficient vectors that were fit using the same order.
  int order() const;

  // Return the rotation that is effectively applied to the inputs of the
  // original function.
  Eigen::Quaternionf rotation() const;

  // Return the (2l+1)x(2l+1) matrix for transforming the coefficients within
  // band @l by the rotation. @l must be at least 0 and less than or equal to
  // the order this rotation was initially constructed with.
  const Eigen::MatrixXf& band_rotation(int l) const;

  Rotation(int order, const Eigen::Quaternionf& rotation);

 private:
  const int order_;
  const Eigen::Quaternionf rotation_;

  std::vector<Eigen::MatrixXf> band_rotations_;
};

Rotation::Rotation(int order, const Eigen::Quaternionf& rotation)
    : order_(order), rotation_(rotation) {
  band_rotations_.reserve(GetCoefficientCount(order));

  // Order 0 (first band) is simply the 1x1 identity since the SH basis
  // function is a simple sphere.
  Eigen::MatrixXf r(1, 1);
  r(0, 0) = 1.0;
  band_rotations_.push_back(r);

  r.resize(3, 3);
  // The second band's transformation is simply a permutation of the
  // rotation matrix's elements, provided in Appendix 1 of [1], updated to
  // include the Condon-Shortely phase. The recursive method in
  // ComputeBandRotation preserves the proper phases as high bands are computed.
  Eigen::Matrix3f rotation_mat = rotation.toRotationMatrix();
  r(0, 0) = rotation_mat(1, 1);
  r(0, 1) = -rotation_mat(1, 2);
  r(0, 2) = rotation_mat(1, 0);
  r(1, 0) = -rotation_mat(2, 1);
  r(1, 1) = rotation_mat(2, 2);
  r(1, 2) = -rotation_mat(2, 0);
  r(2, 0) = rotation_mat(0, 1);
  r(2, 1) = -rotation_mat(0, 2);
  r(2, 2) = rotation_mat(0, 0);
  band_rotations_.push_back(r);

  // Recursively build the remaining band rotations, using the equations
  // provided in [4, 4b].
  for (int l = 2; l <= order; l++) {
    ComputeBandRotation(l, &band_rotations_);
  }
}

/*
std::unique_ptr<Rotation> Rotation::Create(
    int order, const Eigen::Quaternionf& rotation) {
#ifndef NDEBUG
  CHECK(order >= 0, "Order must be at least 0.");
  CHECK(NearByMargin(rotation.squaredNorm(), 1.0),
        "Rotation must be normalized.");
#endif

  std::unique_ptr<Rotation> sh_rot(new Rotation(order, rotation));

  // Order 0 (first band) is simply the 1x1 identity since the SH basis
  // function is a simple sphere.
  Eigen::MatrixXf r(1, 1);
  r(0, 0) = 1.0;
  sh_rot->band_rotations_.push_back(r);

  r.resize(3, 3);
  // The second band's transformation is simply a permutation of the
  // rotation matrix's elements, provided in Appendix 1 of [1], updated to
  // include the Condon-Shortely phase. The recursive method in
  // ComputeBandRotation preserves the proper phases as high bands are computed.
  Eigen::Matrix3f rotation_mat = rotation.toRotationMatrix();
  r(0, 0) = rotation_mat(1, 1);
  r(0, 1) = -rotation_mat(1, 2);
  r(0, 2) = rotation_mat(1, 0);
  r(1, 0) = -rotation_mat(2, 1);
  r(1, 1) = rotation_mat(2, 2);
  r(1, 2) = -rotation_mat(2, 0);
  r(2, 0) = rotation_mat(0, 1);
  r(2, 1) = -rotation_mat(0, 2);
  r(2, 2) = rotation_mat(0, 0);
  sh_rot->band_rotations_.push_back(r);

  // Recursively build the remaining band rotations, using the equations
  // provided in [4, 4b].
  for (int l = 2; l <= order; l++) {
    ComputeBandRotation(l, &(sh_rot->band_rotations_));
  }

  return sh_rot;
}

std::unique_ptr<Rotation> Rotation::Create(int order,
                                           const Rotation& rotation) {
#ifndef NDEBUG
  CHECK(order >= 0, "Order must be at least 0.");
#endif

  std::unique_ptr<Rotation> sh_rot(new Rotation(order, rotation.rotation_));

  // Copy up to min(order, rotation.order_) band rotations into the new
  // SHRotation. For shared orders, they are the same. If the new order is
  // higher than already calculated then the remainder will be computed next.
  for (int l = 0; l <= std::min(order, rotation.order_); l++) {
    sh_rot->band_rotations_.push_back(rotation.band_rotations_[l]);
  }

  // Calculate remaining bands (automatically skipped if there are no more).
  for (int l = rotation.order_ + 1; l <= order; l++) {
    ComputeBandRotation(l, &(sh_rot->band_rotations_));
  }

  return sh_rot;
}
*/

int Rotation::order() const { return order_; }

Eigen::Quaternionf Rotation::rotation() const { return rotation_; }

const Eigen::MatrixXf& Rotation::band_rotation(int l) const {
  return band_rotations_[l];
}

template <typename T>
void Rotation::Apply(const std::vector<T>& coeff,
                     std::vector<T>* result) const {
#ifndef NDEBUG
  CHECK(coeff.size() == GetCoefficientCount(order_),
        "Incorrect number of coefficients provided.");
#endif

  // Resize to the required number of coefficients.
  // If result is already the same size as coeff, there's no need to zero out
  // its values since each index will be written explicitly later.
  if (result->size() != coeff.size()) {
    result->assign(coeff.size(), T());
  }

  // Because of orthogonality, the coefficients outside of each band do not
  // interact with one another. By separating them into band-specific matrices,
  // we take advantage of that sparsity.

  for (int l = 0; l <= order_; l++) {
    VectorX<T> band_coeff(2 * l + 1);

    // Fill band_coeff from the subset of @coeff that's relevant.
    for (int m = -l; m <= l; m++) {
      // Offset by l to get the appropiate vector component (0-based instead
      // of starting at -l).
      band_coeff(m + l) = coeff[GetIndex(l, m)];
    }

    band_coeff = band_rotations_[l].cast<T>() * band_coeff;

    // Copy rotated coefficients back into the appropriate subset into @result.
    for (int m = -l; m <= l; m++) {
      (*result)[GetIndex(l, m)] = band_coeff(m + l);
    }
  }
}

void Rotation::Apply(const Eigen::MatrixXf& coeffs,
                     Eigen::MatrixXf& result) const {
   const int rows = coeffs.cols();
   for(int l=0; l<=order_; ++l) {
      const int i = l*l;
      const int n = 2*l+1;
      result.block(i, 0, n, rows) = band_rotations_[l] * coeffs.block(i, 0, n, rows);
   }
}

void Rotation::Apply(const Eigen::VectorXf& coeffs,
                     Eigen::VectorXf& result) const {
   for(int l=0; l<=order_; ++l) {
      const int i = l*l;
      const int n = 2*l+1;
      result.segment(i, n) = band_rotations_[l] * coeffs.segment(i, n);
   }
}


#include <cassert>
#include <iostream>
#include <boost/timer.hpp>

template <typename T>
class vector
{
    void check_size(int that_size) const { assert(my_size == that_size); }
    void check_index(int i) const { assert(i >= 0 && i < my_size); }
  public:
    explicit vector(int size)
      : my_size(size), data( new T[my_size] )
    {}

    vector()
      : my_size(0), data(0)
    {}

    vector( const vector& that )
      : my_size(that.my_size), data( new T[my_size] )
    {
	for (int i= 0; i < my_size; ++i)
	    data[i]= that.data[i];
    }

    ~vector() { if (data) delete [] data ; }

    vector& operator=( const vector& that )
    {
	check_size(that.my_size);
	for (int i= 0; i < my_size; ++i)
	    data[i]= that.data[i];
    }

    int size() const { return my_size ; }

    const T& operator[]( int i ) const
    {
	check_index(i);
	return data[i];
    }

    T& operator[]( int i )
    {
	check_index(i);
	return data[i] ;
    }

  private:
    int   my_size ;
    T*    data ;
};

template <typename T>
std::ostream& operator<<(std::ostream& os, const vector<T>& v)
{
  os << '[';
  for (int i= 0; i < v.size(); ++i) os << v[i] << ',';
  os << ']';
  return os ;
}

template <unsigned Offset, unsigned Max>
struct my_axpy_ftor
{
    template <typename U, typename V, typename W>
    void operator()(U& u, const V& v, const W& w, unsigned i)
    {
	u[i+Offset]= 3.0f * v[i+Offset] + w[i+Offset];
	my_axpy_ftor<Offset+1, Max>()(u, v, w, i);
    }
};

template <unsigned Max>
struct my_axpy_ftor<Max, Max>
{
    template <typename U, typename V, typename W>
    void operator()(U& u, const V& v, const W& w, unsigned i) {}
};

template <unsigned BSize, typename U, typename V, typename W>
void my_axpy(U& u, const V& v, const W& w)
{
    assert(u.size() == v.size() && v.size() == w.size());
    unsigned s= u.size(), sb= s / BSize * BSize;

    for (unsigned i= 0; i < sb; i+= BSize)
	my_axpy_ftor<0, BSize>()(u, v, w, i);

    for (unsigned i= sb; i < s; i++)
	u[i]= 3.0f * v[i] + w[i];
}

int main(int argc, char** argv)
{
    /* const */ unsigned s= 1000;
    // if (argc > 1) s= atoi(argv[1]);
    vector<float> u(s), v(s), w(s);

    for (unsigned i= 0; i < s; i++) {
	v[i]= float(i);
	w[i]= float(2*i + 15);
    }

    for (unsigned j= 0; j < 3; j++)
	for (unsigned i= 0; i < s; i++)
	    u[i]= 3.0f * v[i] + w[i];

    const unsigned rep= 2000000; // 1000000;

    boost::timer native;
    // asm ( "# start here native" );
    for (unsigned j= 0; j < rep; j++)
	for (unsigned i= 0; i < s; i++)
	    u[i]= 3.0f * v[i] + w[i];
    // asm ( "# end here native" );
    std::cout << "Compute time native loop is " << 1000000.0 * native.elapsed() / double(rep) << " mmicros.\n";
    std::cout << "u[0] is " << u[0] << '\n';

    boost::timer unrolled;
    // asm ( "# start here unrolled" );
    for (unsigned j= 0; j < rep; j++) {
	unsigned sb= s / 4 * 4;
	for (unsigned i= 0; i < sb; i+= 4) {
	    u[i]=   3.0f * v[i]   + w[i];
	    u[i+1]= 3.0f * v[i+1] + w[i+1];
	    u[i+2]= 3.0f * v[i+2] + w[i+2];
	    u[i+3]= 3.0f * v[i+3] + w[i+3];
	}
	for (unsigned i= sb; i < s; i++)
	    u[i]= 3.0f * v[i] + w[i];
    }
    // asm ( "# end here unrolled" );
    std::cout << "Compute time unrolled loop is " << 1000000.0 * unrolled.elapsed() / double(rep) << " mmicros.\n";
    std::cout << "u[0] is " << u[0] << '\n';

    boost::timer unrolled2;
    for (unsigned j= 0; j < rep; j++)
	my_axpy<2>(u, v, w);
    std::cout << "Compute time unrolled<2> loop is " << 1000000.0 * unrolled2.elapsed() / double(rep) << " mmicros.\n";
    std::cout << "u[0] is " << u[0] << '\n';

    boost::timer unrolled4;
    for (unsigned j= 0; j < rep; j++)
	my_axpy<4>(u, v, w);
    std::cout << "Compute time unrolled<4> loop is " << 1000000.0 * unrolled4.elapsed() / double(rep) << " mmicros.\n";
    std::cout << "u[0] is " << u[0] << '\n';

    boost::timer unrolled6;
    for (unsigned j= 0; j < rep; j++)
	my_axpy<6>(u, v, w);
    std::cout << "Compute time unrolled<6> loop is " << 1000000.0 * unrolled6.elapsed() / double(rep) << " mmicros.\n";
    std::cout << "u[0] is " << u[0] << '\n';

    boost::timer unrolled8;
    for (unsigned j= 0; j < rep; j++)
	my_axpy<8>(u, v, w);
    std::cout << "Compute time unrolled<8> loop is " << 1000000.0 * unrolled8.elapsed() / double(rep) << " mmicros.\n";
    std::cout << "u[0] is " << u[0] << '\n';

    return 0;
}


//=======================================================================
// Copyright 2009 Trustees of Indiana University.
// Authors: Michael Hansen
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <iostream>
#include <map>
#include <vector>
#include <ctime>
#include <boost/config.hpp>

#ifdef BOOST_MSVC
// Without disabling this we get hard errors about initialialized pointers:
#pragma warning(disable : 4703)
#endif

#include <boost/lexical_cast.hpp>
#include <boost/random.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/random.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/graph/iteration_macros.hpp>

#define INITIALIZE_VERTEX 0
#define DISCOVER_VERTEX 1
#define EXAMINE_VERTEX 2
#define EXAMINE_EDGE 3
#define EDGE_RELAXED 4
#define EDGE_NOT_RELAXED 5
#define FINISH_VERTEX 6

template < typename Graph > void run_dijkstra_test(const Graph& graph)
{
    using namespace boost;

    // Set up property maps
    typedef typename graph_traits< Graph >::vertex_descriptor vertex_t;

    typedef typename std::map< vertex_t, vertex_t > vertex_map_t;
    typedef associative_property_map< vertex_map_t > predecessor_map_t;
    vertex_map_t default_vertex_map, no_color_map_vertex_map;
    predecessor_map_t default_predecessor_map(default_vertex_map),
        no_color_map_predecessor_map(no_color_map_vertex_map);

    typedef typename std::map< vertex_t, double > vertex_double_map_t;
    typedef associative_property_map< vertex_double_map_t > distance_map_t;
    vertex_double_map_t default_vertex_double_map,
        no_color_map_vertex_double_map;
    distance_map_t default_distance_map(default_vertex_double_map),
        no_color_map_distance_map(no_color_map_vertex_double_map);

    // Run dijkstra algoirthms
    dijkstra_shortest_paths(graph, vertex(0, graph),
        predecessor_map(default_predecessor_map)
            .distance_map(default_distance_map));

    dijkstra_shortest_paths_no_color_map(graph, vertex(0, graph),
        predecessor_map(no_color_map_predecessor_map)
            .distance_map(no_color_map_distance_map));

    // Verify that predecessor maps are equal
    BOOST_TEST(std::equal(default_vertex_map.begin(), default_vertex_map.end(),
        no_color_map_vertex_map.begin()));

    // Verify that distance maps are equal
    BOOST_TEST(std::equal(default_vertex_double_map.begin(),
        default_vertex_double_map.end(),
        no_color_map_vertex_double_map.begin()));
}

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

    int vertices_to_create = 10;
    int edges_to_create = 500;
    std::size_t random_seed = std::time(0);

    if (argc > 1)
    {
        vertices_to_create = lexical_cast< int >(argv[1]);
    }

    if (argc > 2)
    {
        edges_to_create = lexical_cast< int >(argv[2]);
    }

    if (argc > 3)
    {
        random_seed = lexical_cast< std::size_t >(argv[3]);
    }

    minstd_rand generator(random_seed);

    // Set up graph
    typedef adjacency_list< listS, listS, directedS,
        property< vertex_index_t, int >, property< edge_weight_t, double > >
        graph_t;

    graph_t graph;
    generate_random_graph(
        graph, vertices_to_create, edges_to_create, generator);

    // Set up property maps
    typedef property_map< graph_t, vertex_index_t >::type index_map_t;
    index_map_t index_map = get(vertex_index, graph);
    int vertex_index = 0;

    BGL_FORALL_VERTICES(current_vertex, graph, graph_t)
    {
        put(index_map, current_vertex, vertex_index++);
    }

    randomize_property< edge_weight_t >(graph, generator);

    // Run comparison test with original dijkstra_shortest_paths
    std::cout << "Running dijkstra shortest paths test with "
              << num_vertices(graph) << " vertices and " << num_edges(graph)
              << " edges " << std::endl;

    run_dijkstra_test(graph);

    return boost::report_errors();
}


#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>
#include <cxxopts.hpp>

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, const char **argv)
{

    int req = MPI_THREAD_FUNNELED;
    int prov = -1;

    MPI_Init_thread(NULL, NULL, req, &prov);

    assert(prov == req);

    std::stringstream sstr;
    sstr << comm_size();
    const std::string comm_size_str = sstr.str();

    cxxopts::Options options("2d_cholesky", "2D dense cholesky using TaskTorrent");
    options.add_options()
        ("help", "Print help")
        ("n_threads", "Number of threads", cxxopts::value<int>()->default_value("2"))
        ("verb", "Verbosity level", cxxopts::value<int>()->default_value("0"))
        ("block_size", "Block size", cxxopts::value<int>()->default_value("5"))
        ("num_blocks", "Number of blocks", cxxopts::value<int>()->default_value("10"))
        ("nprows", "Number of processors accross rows", cxxopts::value<int>()->default_value("1"))
        ("npcols", "Number of processors accross columns", cxxopts::value<int>()->default_value(comm_size_str.c_str()))
        ("kind", "Priority kind", cxxopts::value<int>()->default_value("0"))
        ("log", "Enable logging", cxxopts::value<bool>()->default_value("false"))
        ("debug", "Debug or not", cxxopts::value<bool>()->default_value("false"));
    auto result = options.parse(argc, argv);

    const int n_threads = result["n_threads"].as<int>();
    const int verb = result["verb"].as<int>();
    const int block_size = result["block_size"].as<int>();
    const int num_blocks = result["num_blocks"].as<int>();
    const int nprows = result["nprows"].as<int>();
    const int npcols = result["npcols"].as<int>();
    const PrioKind prio = (PrioKind)(result["kind"].as<int>());
    const bool log = result["log"].as<bool>();
    const bool debug = result["debug"].as<bool>();

    assert(block_size > 0);
    assert(num_blocks > 0);
    assert(n_threads > 0);
    assert(verb >= 0);
    assert(nprows >= 0);
    assert(npcols >= 0);

    if (result.count("help")) {
        std::cout << options.help({"", "Group"}) << endl;
        exit(0);
    }
    if(comm_rank() == 0) printf("Arguments: block_size (size of blocks) %d\nnum_blocks (# of blocks) %d\nn_threads %d\nverb %d\nnprows %d\nnpcols %d\nprio %d\nlog %d\ndebug %d\n",
        block_size, num_blocks, n_threads, verb, nprows, npcols, (int)prio, log, debug);

    cholesky3d(n_threads, verb, block_size, num_blocks, npcols, nprows, prio, log, debug);
    MPI_Finalize();
}


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include "test_envelope.hpp"

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

template <typename P>
void test_2d()
{
    /*test_envelope<bg::model::multi_point<P> >(
            "MULTIPOINT((1 1),(1 0),(1 2))", 1, 1, 0, 2);
    test_envelope<bg::model::multi_linestring<bg::model::linestring<P> > >(
            "MULTILINESTRING((0 0,1 1),(1 1,2 2),(2 2,3 3))", 0, 3, 0, 3);
*/
    test_envelope<bg::model::multi_polygon<bg::model::polygon<P> > >(
            "MULTIPOLYGON(((1 1,1 3,3 3,3 1,1 1)),((4 4,4 6,6 6,6 4,4 4)))", 1, 6, 1, 6);
}

template <typename P>
void test_3d()
{
    //typedef bg::model::multi_point<P> mp;
}

int test_main( int , char* [] )
{
    test_2d<boost::tuple<float, float> >();
    test_2d<bg::model::d2::point_xy<float> >();
    test_2d<bg::model::d2::point_xy<double> >();

    test_3d<boost::tuple<float, float, float> >();
    test_3d<bg::model::point<double, 3, bg::cs::cartesian> >();

    return 0;
}


#ifndef CPP_EVALUATOR_UTILS_HH
#define CPP_EVALUATOR_UTILS_HH

#include "Evaluator.hh"

#include <array>
#include <iterator>

#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/algorithm/max_element.hpp>
#include <boost/range/algorithm/transform.hpp>
#include <boost/range/numeric.hpp>

namespace tl
{

/**
 * Linear tensor field expressed in barycentric coordinates
 */
using TensorInterp = TensorProductBezierTriangle<Mat3d, double, 1>;

/**
 * Compute the derivatives of a TensorProductBezierTriangle along two
 * orthogonal directions on the triangle in the space indicated by @a D.
 *
 * @param poly The polynomial.
 * @tparam D The space (0-sizeof...(Degrees)) in which to perform the derivative
 */
template <std::size_t D,
          typename TPBT,
          typename T,
          typename C,
          std::size_t... Degrees>
auto derivatives(
        const TensorProductBezierTriangleBase<TPBT, T, C, Degrees...>& poly)
        -> std::array<
                typename TensorProductDerivativeType<D, T, C, Degrees...>::type,
                2>
{
    static_assert(D < sizeof...(Degrees), "Invalid index");
    auto d0 = poly.template derivative<D>(0);
    auto d1 = poly.template derivative<D>(1);
    auto d2 = poly.template derivative<D>(2);

    auto da = (d1 - d0) / std::sqrt(2);

    auto db = (2 * d2 - d0 - d1) / std::sqrt(6);

    return {da, db};
}

/**
 * @brief Compute an upper bound for the magnitude of the function value.
 * @details Finds the coefficient with the maximum absolute value.
 *
 * @param poly The polynomial
 * @return An upper bound for std::abs(poly(x)) on the triangles
 */
template <typename TPBT,
          typename T,
          typename C,
          std::size_t... Degrees>
double abs_upper_bound(
        const TensorProductBezierTriangleBase<TPBT, T, C, Degrees...>& poly)
{
    return boost::accumulate(poly.coefficients(), 0., MaxAbs{});
}

/**
 * @brief Compute an upper bound for the magnitude of a number of polynomials.
 * @details Finds the coefficient with the maximum absolute value of all
 *     polynomials.
 *
 * @param polys A sequence of polynomials
 * @return An upper bound for std::abs(poly(x)) over all polynomials in the
 *     sequence
 */
template <typename TPBTSeq>
double abs_max_upper_bound(const TPBTSeq& polys)
{
    using namespace boost;
    using namespace boost::adaptors;
    return accumulate(polys | transformed([](const auto& f) {
                          return abs_upper_bound(f);
                      }),
                      0.,
                      Max{});
}

/**
 * @brief Compute an upper bound for the gradient magnitude of the polynomial in
 *      the space indicated by @a D.
 *
 * @param poly The polynomial
 * @tparam D The space (0-sizeof...(Degrees)) in which to perform the derivative
 * @return an estimate for the upper bound of the gradient magnitude of poly(x)
 *      on the triangles
 */
template <std::size_t D,
          typename TPBT,
          typename T,
          typename C,
          std::size_t... Degrees>
double derivatives_upper_bound(
        const TensorProductBezierTriangleBase<TPBT, T, C, Degrees...>& poly)
{
    static_assert(D < sizeof...(Degrees), "Invalid index");
    auto upper_bound = typename TPBT::Coeffs{};
    // Estimate upper bound of gradient magnitude by L1 norm of control points
    // todo: Can we do better? (Represent gradient magnitude as polynomial)
    auto abssum = [](double v1, double v2) {
        return std::abs(v1) + std::abs(v2);
    };
    auto derivs = derivatives<D>(poly);
    boost::transform(derivs[0].coefficients(),
                     derivs[1].coefficients(),
                     std::begin(upper_bound),
                     abssum);
    return *boost::max_element(upper_bound);
}

/**
 * @brief Compute an upper bound for the gradient magnitude of a sequence of
 *      polynomials in the space indicated by @a D.
 *
 * @param polys A sequence (range) of polynomials
 * @tparam D The space in which to perform the derivatives
 * @return Upper bound for the gradient magnitude on the triangles
 */
template <std::size_t D, typename TPBTSeq>
double derivatives_max_upper_bound(const TPBTSeq& polys)
{
    using namespace boost;
    using namespace boost::adaptors;
    return accumulate(polys | transformed([](const auto& f) {
                          return derivatives_upper_bound<D>(f);
                      }),
                      0.,
                      Max{});
}

/**
 * @brief Compute an upper bound for the norm of a vector of values from
 *     multiple polynomials.
 * @details Finds an upper bound for sqrt(poly_1(x)^2 + poly_2(x)^2 + ...)
 *      where poly_i are the elements of the sequence @a polys.
 *
 * @param polys A sequence (range) of polynomials
 * @return Upper bound for the norm of the vector od polynomials on the
 *     triangles
 */
template <typename TPBTSeq>
double upper_bound_norm(const TPBTSeq& polys)
{
    using namespace boost;
    using namespace boost::adaptors;
    return std::sqrt(accumulate(polys | transformed([](const auto& f) {
                                    return std::pow(
                                            *max_element(f.coefficients()), 2);
                                }),
                                0.));
}

} // namespace tl

#endif


#include "prime_factors.h"
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <iostream>

BOOST_AUTO_TEST_CASE(_1_yields_empty)
{
    const std::vector<int> expected{};

    const std::vector<int> actual{prime_factors::of(1)};

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expected.begin(), expected.end(), actual.begin(), actual.end());
}

BOOST_AUTO_TEST_CASE(_2_yields_2)
{
    const std::vector<int> expected{2};

    const std::vector<int> actual{prime_factors::of(2)};

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expected.begin(), expected.end(), actual.begin(), actual.end());
}

BOOST_AUTO_TEST_CASE(_3_yields_3)
{
    const std::vector<int> expected{3};

    const std::vector<int> actual{prime_factors::of(3)};

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expected.begin(), expected.end(), actual.begin(), actual.end());
}

BOOST_AUTO_TEST_CASE(_4_yields_2_2)
{
    const std::vector<int> expected{2, 2};

    const std::vector<int> actual{prime_factors::of(4)};

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expected.begin(), expected.end(), actual.begin(), actual.end());
}

BOOST_AUTO_TEST_CASE(_6_yields_2_3)
{
    const std::vector<int> expected{2, 3};

    const std::vector<int> actual{prime_factors::of(6)};

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expected.begin(), expected.end(), actual.begin(), actual.end());
}

BOOST_AUTO_TEST_CASE(_8_yields_2_2_2)
{
    const std::vector<int> expected{2, 2, 2};

    const std::vector<int> actual{prime_factors::of(8)};

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expected.begin(), expected.end(), actual.begin(), actual.end());
}

BOOST_AUTO_TEST_CASE(_9_yields_3_3)
{
    const std::vector<int> expected{3, 3};

    const std::vector<int> actual{prime_factors::of(9)};

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expected.begin(), expected.end(), actual.begin(), actual.end());
}

BOOST_AUTO_TEST_CASE(_27_yields_3_3_3)
{
    const std::vector<int> expected{3, 3, 3};

    const std::vector<int> actual{prime_factors::of(27)};

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expected.begin(), expected.end(), actual.begin(), actual.end());
}

BOOST_AUTO_TEST_CASE(_625_yields_5_5_5_5)
{
    const std::vector<int> expected{5, 5, 5, 5};

    const std::vector<int> actual{prime_factors::of(625)};

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expected.begin(), expected.end(), actual.begin(), actual.end());
}

BOOST_AUTO_TEST_CASE(_901255_yields_5_17_23_461)
{
    const std::vector<int> expected{5, 17, 23, 461};

    const std::vector<int> actual{prime_factors::of(901255)};

    BOOST_REQUIRE_EQUAL_COLLECTIONS(expected.begin(), expected.end(), actual.begin(), actual.end());
}
#if defined(EXERCISM_RUN_ALL_TESTS)
#endif


#include <vtkFloatArray.h>
#include <vtkInformation.h>
#include <vtkMath.h>
#include <vtkPointData.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkRansacPlaneModel.h>
#include <vtkSmartPointer.h>
#include <vtkUnsignedIntArray.h>

#include <iostream>
#include <iomanip>

#include <Eigen/Dense>

int main(int argc, char* argv[])
{
    srand((unsigned int) time(0));

    int N = 1000;
    int N_OUTLIERS = 10;
    float noise_sigma = 0.1;
    Eigen::MatrixXf angles = Eigen::MatrixXf::Random(1, N);
    angles.array() += 1;
    angles.array() *= 3.1415;
    Eigen::MatrixXf radius = Eigen::MatrixXf::Random(1, N);
    radius.array() *= 10;
    Eigen::Matrix3Xf pts(3, N);

    // add noise on z
    Eigen::MatrixXf noisy_z = Eigen::MatrixXf::Random(1, N);
    noisy_z.array() *= noise_sigma;

    // add outliers
    for (int i = N - N_OUTLIERS; i < N; ++i)
    {
        auto v = noisy_z(i);
        if (v < 0) v = 10 * v - 5;
        else v = 10 * v + 5;
        noisy_z(i) = v;
    }
    pts << radius.array().cwiseProduct(angles.array().cos()), radius.array().cwiseProduct(angles.array().sin()), noisy_z;

    // transform the initial plane
    float initial_rot_X = static_cast<float>(std::rand() % 100) / 10.0;
    float initial_rot_Y = static_cast<float>(std::rand() % 100) / 10.0;
    float initial_z_translation = -static_cast<float>(std::rand() % 100) / 10.0;
    Eigen::AngleAxisf rotX(vtkMath::RadiansFromDegrees(initial_rot_X), Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf rotY(vtkMath::RadiansFromDegrees(initial_rot_Y), Eigen::Vector3f::UnitY());
    Eigen::Translation3f trans(0, 0, initial_z_translation);
    Eigen::Affine3f T = rotX * rotY * trans;
    pts = T * pts;

    // build polydata
    auto polydata = vtkSmartPointer<vtkPolyData>::New();
    auto array = vtkSmartPointer<vtkFloatArray>::New();
    array->SetNumberOfComponents(3);
    array->SetNumberOfTuples(N);
    float temp_f[3];
    for (int i = 0; i < N; ++i)
    {
        temp_f[0] = pts.col(i).x();
        temp_f[1] = pts.col(i).y();
        temp_f[2] = pts.col(i).z();
        array->SetTuple(i, temp_f);
    }
    auto new_points = vtkSmartPointer<vtkPoints>::New();
    new_points->SetData(array);
    polydata->SetPoints(new_points);

    // apply filter ransac plane model
    auto filter = vtkSmartPointer<vtkRansacPlaneModel>::New();
    filter->SetAlignOutput(1);
    filter->SetInputData(polydata);
    filter->Update();
    auto output = filter->GetOutput();

    // check that the points are correctly aligned to XY
    double temp_d[3];
    double max_z_threshold = 1.0;       // threshold on z for points with small noise
    for (int i = 0; i < N - N_OUTLIERS; ++i)
    {
        output->GetPoint(i, temp_d);
        if (std::abs(temp_d[2]) > max_z_threshold)
        {
            std::cout << "Error: point " << i << " has z = " << temp_d[2] << std::endl;
            return -1;
        }

    }

    // check that outliers are detected as outliers
    vtkSmartPointer<vtkUnsignedIntArray> inliers_array =
    vtkUnsignedIntArray::SafeDownCast(output->GetPointData()->GetArray("ransac_plane_inliers"));
    for (int i =  N - N_OUTLIERS; i < N; ++i)
    {
        if (inliers_array->GetValue(i) == 1)
        {
            std::cout << "Error: Point " << i << " should be classified as outliers" << std::endl;
            return -1;
        }
    }

    return 0;
}


/*
	Copyright 2005-2007 Adobe Systems Incorporated
	Distributed under the MIT License (see accompanying file LICENSE_1_0_0.txt
	or a copy at http://stlab.adobe.com/licenses.html)
*/

/*************************************************************************************************/

#include <adobe/config.hpp>

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <adobe/enum_ops.hpp>

enum Number {
    num_neg_one=-1,
    num_0 = 0,
    num_1 = 1,
    num_2 = 2,
    num_3 = 3,
    num_4 = 4,
    num_5 = 5,
    num_6 = 6,
    num_7 = 7
};

ADOBE_DEFINE_BITSET_OPS(Number)

BOOST_AUTO_TEST_CASE(enum_bitset_ops) {
    Number x;

// or
    x = num_1 | num_2;
    BOOST_CHECK(x == 3);

// and
    x = num_3 & num_1;
    BOOST_CHECK(x == 1);

// xor
    x = num_2 ^ num_3;
    BOOST_CHECK(x == 1);

// not
    x = num_7 & (~ num_5);
    BOOST_CHECK(x == 2);

// or=
    x = num_1;
    x |= num_2;
    BOOST_CHECK(x == 3);

// and=
    x = num_3;
    x &= num_1;
    BOOST_CHECK(x == 1);

// xor
    x = num_2;
    x ^= num_3;
    BOOST_CHECK(x == 1);
}

ADOBE_DEFINE_ARITHMETIC_OPS(Number)
BOOST_AUTO_TEST_CASE(enum_arith_ops) {
    Number x;

// +
    x = num_1 + num_2;
    BOOST_CHECK(x == 3);

// -
    x = num_3 - num_1;
    BOOST_CHECK(x == 2);

// - (unary)
    x = -num_neg_one;
    BOOST_CHECK(x == 1);

// *
    x = num_2 * num_3;
    BOOST_CHECK(x == 6);

// /
    x = num_5 / num_2;
    BOOST_CHECK(x == 2);

// %
    x = num_5 % num_2;
    BOOST_CHECK(x == 1);

// +=
    x = num_1;
    x+= num_2;
    BOOST_CHECK(x == 3);

// -=
    x = num_3;
    x -= num_1;
    BOOST_CHECK(x == 2);

// *=
    x = num_2;
    x *= num_3;
    BOOST_CHECK(x == 6);

// /=
    x = num_5;
    x /= num_2;
    BOOST_CHECK(x == 2);

// %=
    x = num_5;
    x %= num_2;
    BOOST_CHECK(x == 1);
}


// Author: Francesco Regazzoni - MOX, Politecnico di Milano
// Email:  francesco.regazzoni@polimi.it
// Date:   2020

#ifndef MODEL_RDQ20_SE_HPP
#define MODEL_RDQ20_SE_HPP

#include <vector>

#include <Eigen/Dense>

#include "sarcomere.hpp"

class model_RDQ20_SE : public sarcomere {

  //    Class implementing the spatially-explicit ODE model (SE-ODE) for
  //    cardiomyocytes force generation presented in [1, 2].
  //
  //    References:
  //
  //    [1] F. Regazzoni "Mathematical modeling and Machine Learning for the
  //        numerical simulation of cardiac electromechanics", PhD Thesis -
  //        Politecnico di Milano (2020)
  //        http://hdl.handle.net/10589/152617
  //    [2] F. Regazzoni, L. Dede', A. Quarteroni "Biophysically detailed
  //        mathematical models of multiscale cardiac active mechanics",
  //        PLOS Computational Biology (2020)
  //        https://doi.org/10.1371/journal.pcbi.1008294

public:
  // Constructor.
  model_RDQ20_SE(
      std::string parameters_file =
          "../../params/params_RDQ20-SE_human_body-temperature.json");

  // Solve one time step, updating the state passed as reference.
  void solve_time_step(std::vector<double> &state, const double &calcium,
                       const double &sarcomere_length, const double &dSL_dt,
                       const double &dt);

  // Compute the active tension.
  double get_active_tension(const std::vector<double> &state,
                            const double &sarcomere_length);

  // Compute the permissivity.
  double get_permissivity(const std::vector<double> &state);

  // Compute the active stiffness.
  double get_active_stiffness(const std::vector<double> &state,
                              const double &sarcomere_length);

private:
  // Allocate memory to store variables
  void allocate_variables();

  // Initialize transition rates.
  void initialize_rates();

  // Deserialize the model state into state_RU and state_XB.
  void deserialize_state(const std::vector<double> &state);

  // Serialize the model state from state_RU and state_XB.
  void serialize_state(std::vector<double> &state);

  // Update the RU transition rates.
  void RU_update_rates(const double &calcium, const double &sarcomere_length);

  // Update the RU state variables.
  void RU_update_state(const double &dt);

  // Update the XB state variables.
  void XB_update_state(const double &sarcomere_length, const double &dSL_dt,
                       const double &dt);

  // Coordinate of the i-th RU.
  inline double y_j(const unsigned int &i_RU) {
    return prm_LA * (i_RU + 0.5) / prm_n_RU;
  }

  // Coordinate of the left end of the AF.
  inline double yLA(const double &sarcomere_length) {
    return 2 * prm_LA - sarcomere_length;
  }

  // First coordinate of the MF.
  inline double yM0(const double &sarcomere_length) {
    return (2 * prm_LA - sarcomere_length + prm_LB) * 0.5;
  }

  // Second coordinate of the MF.
  inline double yM1(const double &sarcomere_length) {
    return (2 * prm_LA - sarcomere_length + prm_LM) * 0.5;
  }

  // Smoothed indicator function of the MF.
  inline double ChiMF(const double &sarcomere_length,
                      const unsigned int &i_RU) {
    return 0.5 * std::tanh((y_j(i_RU) - yM0(sarcomere_length)) / prm_Lsmooth) +
           0.5 * std::tanh(-(y_j(i_RU) - yM1(sarcomere_length)) / prm_Lsmooth);
  }

  // Smoothed indicator function of the single-overlap region.
  inline double ChiSF(const double &sarcomere_length,
                      const unsigned int &i_RU) {
    return 0.5 +
           0.5 * std::tanh((y_j(i_RU) - yLA(sarcomere_length)) / prm_Lsmooth);
  }

  // Model Parameters.
  unsigned int prm_n_RU; // [-]
  double prm_LA;         // [micro m]
  double prm_LM;         // [micro m]
  double prm_LB;         // [micro m]
  double prm_SL0;        // [micro m]
  double prm_Lsmooth;    // [micro m]
  double prm_Q;          // [-]
  double prm_Kd0;        // [micro M]
  double prm_alphaKd;    // [micro M / micro m]
  double prm_mu;         // [-]
  double prm_gamma;      // [-]
  double prm_Koff;       // [s^-1]
  double prm_Kbasic;     // [s^-1]
  double prm_r0;         // [s^-1]
  double prm_alpha;      // [-]
  double prm_mu0_fP;     // [s^-1]
  double prm_mu1_fP;     // [s^-1]
  double prm_a_XB;       // [kPa]

  // Numerical Parameters.
  double prm_time_step_update_RU_state =
      2.5e-5; // Time step used to update the RU state. [s]

  // Additional variables.
  std::vector<std::array<std::array<std::array<double, 4>, 4>, 4>>
      state_RU;                                // State variables of RU.
  std::vector<std::array<double, 4>> state_XB; // State variables of XB.

  std::vector<
      std::array<std::array<std::array<std::array<double, 4>, 4>, 4>, 4>>
      rates_RU; // RU rates.

  int permissivity_of_state[4] = {0, 0, 1,
                                  1}; // Permissivity state (0 = N, 1 = P)
                                      // associated with the basic RU states.

  std::vector<
      std::array<std::array<std::array<std::array<double, 4>, 4>, 4>, 4>>
      flux_RU_L; // Probability flux of left-ward RUs of the triplets.
  std::vector<
      std::array<std::array<std::array<std::array<double, 4>, 4>, 4>, 4>>
      flux_RU_C; // Probability flux of central RUs of the triplets.
  std::vector<
      std::array<std::array<std::array<std::array<double, 4>, 4>, 4>, 4>>
      flux_RU_R; // Probability flux of right-ward RUs of the triplets.

  Eigen::Matrix<double, 4, 4> XB_A;   // Matrix defining the local XB system.
  Eigen::Matrix<double, 4, 1> XB_rhs; // Right-hand side of the local XB system.
  Eigen::Matrix<double, 4, 1>
      XB_sol; // Vector used to store the solution of the local XB system.

  unsigned int n_states_RU; // Number of RU states.
  unsigned int n_states_XB; // Number of XB states.
  unsigned int i_RU;        // Index of regulatory_unit.
  unsigned int RU_L;        // Index of left regulatory unit.
  unsigned int RU_C;        // Index of central regulatory unit.
  unsigned int RU_R;        // Index of right regulatory unit.
  unsigned int RU_new;      // Index of regulatory unit (new state).
  unsigned int RU_dummy;    // Index of regulatory unit external to the
                            // considered triplet.
  unsigned int i_XB;        // Index of XB state.
};

#endif /* MODEL_RDQ20_SE_HPP */


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_ELLIPTIC_FUNCTIONS_SCALAR_ELLIE_HPP_INCLUDED
#define NT2_ELLIPTIC_FUNCTIONS_SCALAR_ELLIE_HPP_INCLUDED
#include <nt2/elliptic/functions/ellie.hpp>
#include <boost/math/special_functions.hpp>
#include <nt2/include/functions/scalar/is_ltz.hpp>
#include <nt2/include/functions/scalar/sin.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/sin.hpp>
#include <nt2/include/functions/scalar/sqr.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/include/functions/scalar/oneminus.hpp>
#include <nt2/include/functions/scalar/oneplus.hpp>
#include <nt2/include/functions/scalar/tan.hpp>
#include <nt2/include/functions/scalar/atan.hpp>
#include <nt2/include/functions/scalar/average.hpp>
#include <nt2/include/functions/scalar/ellpe.hpp>
#include <nt2/include/functions/scalar/ellpk.hpp>
#include <nt2/include/functions/scalar/ceil.hpp>
#include <nt2/sdk/error/policies.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/eps.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ellie_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                              (scalar_< arithmetic_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      return nt2::ellie(result_type(a0), result_type(a1));
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ellie_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                              (scalar_< double_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      typedef result_type type;
      if (a1>nt2::One<A0>()||(nt2::is_ltz(a1))) return Nan<type>();
      if (nt2::is_eqz(a1))  return type(a0);
      return boost::math::ellint_2(nt2::sqrt(type(a1)), type(a0), nt2_policy());
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ellie_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                              (scalar_< single_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      typedef result_type type;
      if (a1>nt2::One<A0>()||(nt2::is_ltz(a1))) return nt2::Nan<type>();
      else if (nt2::is_eqz(a1))
        return a0;
      else if (a1 == nt2::One<A0>())
        return nt2::sin(a0);
      else
      {
        type lphi = nt2::abs(a0);
        type m   =  a1;
        type a = nt2::One<type>();
        type b = nt2::sqrt(nt2::oneminus(m));
        type c = nt2::sqrt(m);
        type d = nt2::One<type>();
        type e = nt2::Zero<type>();
        type t = nt2::tan( lphi );
        int mod = toint((lphi+nt2::Pio_2<type>())/nt2::Pi<type>());
        while( nt2::abs(c) > nt2::Eps<type>()*nt2::abs(a) )
        {
          type temp = b/a;
          lphi = lphi + nt2::atan(t*temp) + mod * nt2::Pi<type>();
          mod = nt2::toint((lphi+nt2::Pio_2<type>())/Pi<type>());
          t *= nt2::oneplus(temp)/( nt2::oneminus(temp * nt2::sqr(t)));
          c = nt2::average(a,-b);
          temp = nt2::sqrt(a*b);
          a = nt2::average(a,b);
          b = temp;
          d += d;
          e += c*nt2::sin(lphi);
        }

        b = nt2::oneminus(m);
        type temp = nt2::ellpe(b)/nt2::ellpk(b);
        temp *= (nt2::atan(t) + mod * nt2::Pi<type>())/(d * a);
        temp += e;
        if(nt2::is_ltz(a0))  temp = -temp;
        return temp ;
      }
      }
  };
} }

#endif


#ifndef UTIL_RANDOM_HPP
#define UTIL_RANDOM_HPP

#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>

namespace util{

template <class RNG>
inline double random_01(RNG &rng)
{
    static boost::uniform_real<double> dist(0.0, 1.0);
    return dist(rng);
}

template <class RNG>
inline int random_int(RNG &rng, int N)
{
    return static_cast<int>( N * random_01(rng));
}

template <class RNG>
inline double random_gauss(RNG &rng, double sigma=1.0)
{
    static boost::normal_distribution<double> dist(0.0, 1.0);
    return sigma*dist(rng);
}

}

#endif


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

#include <Testing/Utils/SCIRunUnitTests.h>

#include <fstream>
#include <boost/filesystem.hpp>
#include <Core/Algorithms/Math/LinearSystem/SolveLinearSystemAlgo.h>
#include <Core/Algorithms/DataIO/ReadMatrix.h>
#include <Core/Algorithms/DataIO/WriteMatrix.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/MatrixComparison.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Core/Datatypes/MatrixIO.h>
#include <Core/Algorithms/Base/AlgorithmVariableNames.h>
#include <Testing/Utils/MatrixTestUtilities.h>

using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms::Math;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::DataIO;
using namespace SCIRun::TestUtils;
using namespace SCIRun;
using namespace ::testing;

void CanSolveDarrellWithMethod(const std::string& method, double solutionError)
{
  auto Afile = TestResources::rootDir() / "CGDarrell" / "A.mat";
  auto rhsFile = TestResources::rootDir() / "CGDarrell" / "RHS.mat";
  if (!boost::filesystem::exists(Afile) || !boost::filesystem::exists(rhsFile))
  {
    FAIL() << "TODO: Issue #142 will standardize these file locations other than being on Dan's hard drive." << std::endl
      << "Once that issue is done however, this will be a user setup error." << std::endl;
    return;
  }

  ReadMatrixAlgorithm reader;
  SparseRowMatrixHandle A;
  {
    ScopedTimer t("reading sparse matrix");
    A = castMatrix::toSparse(reader.run(Afile.string()));
  }
  ASSERT_TRUE(A.get() != nullptr);
  EXPECT_EQ(428931, A->nrows());
  EXPECT_EQ(428931, A->ncols());

  DenseMatrixHandle rhs;
  {
    ScopedTimer t("reading rhs");
    rhs = castMatrix::toDense(reader.run(rhsFile.string()));
  }
  ASSERT_TRUE(rhs.get() != nullptr);
  EXPECT_EQ(428931, rhs->nrows());
  EXPECT_EQ(1, rhs->ncols());

  DenseColumnMatrixHandle x0;
  ASSERT_FALSE(x0); // algo object will initialize x0 to the zero vector

  SolveLinearSystemAlgo algo;
  algo.set(Variables::MaxIterations, 500);
  algo.set(Variables::TargetError, 7e-4);
  algo.setOption(Variables::Method, method);
  algo.setUpdaterFunc([](double x) {});

  DenseColumnMatrixHandle solution;
  {
    ScopedTimer t("Running solver");
    ASSERT_TRUE(algo.run(A, convertMatrix::toColumn(rhs), x0, solution));
  }
  ASSERT_TRUE(solution.get() != nullptr);
  EXPECT_EQ(428931, solution->nrows());
  EXPECT_EQ(1, solution->ncols());

  auto scirun4solutionFile = TestResources::rootDir() / "CGDarrell" / ("dan_sol_" + method + ".mat");
  auto scirun4solution = reader.run(scirun4solutionFile.string());
  ASSERT_TRUE(scirun4solution.get() != nullptr);
  DenseColumnMatrixHandle expected = convertMatrix::toColumn(scirun4solution);

  EXPECT_COLUMN_MATRIX_EQ_BY_TWO_NORM(*expected, *solution, solutionError);

  WriteMatrixAlgorithm writer;
  auto portedSolutionFile = TestResources::rootDir() / "CGDarrell" / ("portedSolution_" + method + ".txt");
  writer.run(solution, portedSolutionFile.string());

  auto diff = *expected - *solution;
  auto maxDiff = diff.maxCoeff();
  std::cout << "max diff is: " << maxDiff << std::endl;
}

/// todo: switch these disabled tests to nightly mode. They are overly long for normal continuous builds.

TEST(SolveLinearSystemTests, DISABLED_CanSolveDarrell_CG)
{
  double solutionError;
  /// @todo: investigate this significant difference
#ifdef WIN32
  solutionError = 0.15;
#else
  solutionError = 0.23;
#endif
  CanSolveDarrellWithMethod("cg", solutionError);
}

TEST(SolveLinearSystemTests, DISABLED_CanSolveDarrell_BICG)
{
  double solutionError = 0.001;
  CanSolveDarrellWithMethod("bicg", solutionError);
}

TEST(SolveLinearSystemTests, DISABLED_CanSolveDarrell_Jacobi)
{
  /// @todo: doesn't converge for this system. Problem?
  double solutionError = 105;
  CanSolveDarrellWithMethod("jacobi", solutionError);
}

TEST(SolveLinearSystemTests, DISABLED_CanSolveDarrell_MINRES)
{
  /// @todo: converges but not as accurate.
  double solutionError = 2.4;
  CanSolveDarrellWithMethod("minres", solutionError);
}