#include <boost/program_options.hpp>
#include <cmath>
#include <map>
#include <random>
#include <set>

#include "../tools/linalg.hpp"
#include "../tools/util.hpp"
#include "bilinear_form.hpp"
#include "initial_triangulation.hpp"
#include "linear_form.hpp"
#include "operators.hpp"
#include "triangulation.hpp"
#include "triangulation_view.hpp"

using namespace space;
using namespace datastructures;
namespace po = boost::program_options;

Eigen::VectorXd RandomVector(const TriangulationView &triang,
                             bool dirichlet_boundary = true) {
  Eigen::VectorXd vec(triang.V);
  vec.setRandom();
  if (dirichlet_boundary)
    for (int v = 0; v < triang.V; v++)
      if (triang.OnBoundary(v)) vec[v] = 0.0;
  return vec;
}

void UniformRefine(TreeVector<HierarchicalBasisFn> &tree_vec) {
  for (auto nv : tree_vec.Bfs()) {
    if (!nv->node()->is_full()) nv->node()->Refine();
    if (!nv->template is_full<0>()) nv->Refine();
  }
}

auto Mark(TreeVector<HierarchicalBasisFn> &residual, double theta = 0.8) {
  auto nodes = residual.Bfs();
  std::sort(nodes.begin(), nodes.end(), [](auto n1, auto n2) {
    return std::abs(n1->value()) > std::abs(n2->value());
  });
  double sq_norm = 0.0;
  for (auto node : nodes) sq_norm += node->value() * node->value();
  double cur_sq_norm = 0.0;
  size_t last_idx = 0;
  for (; last_idx < nodes.size(); last_idx++) {
    cur_sq_norm += nodes[last_idx]->value() * nodes[last_idx]->value();
    if (cur_sq_norm >= theta * theta * sq_norm) {
      nodes.resize(last_idx + 1);
      break;
    }
  }
  return nodes;
}

auto Now() { return std::chrono::steady_clock::now(); }
double Duration(std::chrono::steady_clock::time_point start) {
  return std::chrono::duration<double>(Now() - start).count();
}

constexpr size_t mg_cycles = 4;
constexpr size_t mg_inner_cycles = 5;

int main(int argc, char *argv[]) {
  double mark_theta = 0.5;
  bool print_mesh = false;
  bool print_mesh_info = true;
  bool time_mg_cycles = true;
  int max_iter = 4;
  boost::program_options::options_description adapt_optdesc("Adaptive options");
  adapt_optdesc.add_options()("mark_theta", po::value<double>(&mark_theta))(
      "print_mesh", po::value<bool>(&print_mesh))("max_iters",
                                                  po::value<int>(&max_iter))(
      "print_mesh_info", po::value<bool>(&print_mesh_info))(
      "time_mg_cycles", po::value<bool>(&time_mg_cycles));
  boost::program_options::options_description cmdline_options;
  cmdline_options.add(adapt_optdesc);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(cmdline_options).run(),
            vm);
  po::notify(vm);
  std::cout << "Adaptive options:" << std::endl;
  std::cout << "\ttheta: " << mark_theta << std::endl;
  std::cout << "\tmax_iter: " << max_iter << std::endl;

  auto T = InitialTriangulation::LShape();
  auto f = [](double x, double y) { return 1; };

  auto x_d = TreeVector<HierarchicalBasisFn>(T.hierarch_basis_meta_root);
  x_d.DeepRefine();
  auto solution = x_d.ToVector();

  auto lf = LinearForm(f, /* apply_quadrature*/ true, /*order*/ 0,
                       /* dirichlet_boundary */ true);
  space::OperatorOptions space_opts(
      {.build_mat = false, .mg_cycles = mg_cycles});
  size_t iter = 0;
  auto start_algorithm = std::chrono::steady_clock::now();
  while (true) {
    auto start_iteration = std::chrono::steady_clock::now();
    std::cout << "iter: " << ++iter << "\n\tXDelta-size: " << x_d.Bfs().size()
              << "\n\ttotal-memory-kB: " << getmem() << std::flush;

    if (print_mesh_info) {
      auto time_start = std::chrono::steady_clock::now();
      TriangulationView t_view(x_d.Bfs());
      std::vector<int> hist;
      for (auto &&[elem, _] : t_view.element_leaves()) {
        if (elem->level() >= hist.size()) hist.resize(elem->level() + 1, 0);
        hist[elem->level()]++;
      }
      std::cout << "\n\ttime-tview-XDelta: " << Duration(time_start);
      std::cout << "\n\tmesh-info: [";
      for (auto val : hist) std::cout << val << ",";
      std::cout << "]" << std::flush;
    }

    if (print_mesh) {
      std::cout << "\n\tmesh: [";
      for (auto nv : x_d.Bfs())
        std::cout << "[" << nv->node()->center().first << ","
                  << nv->node()->center().second << "],";
      std::cout << "]" << std::flush;
    }

    // Create linform + bilform + precond.
    auto bilform = CreateBilinearForm<StiffnessOperator>(x_d, x_d, space_opts);
    auto precond =
        CreateBilinearForm<MultigridPreconditioner<StiffnessOperator>>(
            x_d, x_d, space_opts);
    std::cout << "\n\ttime-stiff-create: " << bilform.TimeCreate()
              << "\n\ttime-MG(" << mg_cycles
              << ")-create: " << precond.TimeCreate();

    // Calculate rhs.
    auto time_start = std::chrono::steady_clock::now();
    lf.Apply(x_d.root());
    auto rhs = x_d.ToVector();
    std::cout << "\n\ttime-rhs: " << Duration(time_start) << std::flush;

    // Solve.
    time_start = std::chrono::steady_clock::now();
    auto [new_solution, pcg_data] =
        tools::linalg::PCG(bilform, rhs, precond, solution, max_iter, 1e-16);
    std::cout << "\n\ttime-solve: " << Duration(time_start)
              << "\n\tsolve-PCG-steps: " << pcg_data.iterations
              << "\n\tsolve-PCG-relative-residual: "
              << pcg_data.relative_residual
              << "\n\tsolve-PCG-algebraic-error: " << pcg_data.algebraic_error
              << "\n\tsolve-memory: " << getmem()
              << "\n\ttime-stiff-per-apply: " << bilform.TimePerApply()
              << "\n\ttime-MG(" << mg_cycles
              << ")-per-apply: " << precond.TimePerApply();

    // Time preconds apply time.
    if (time_mg_cycles) {
      time_start = std::chrono::steady_clock::now();
      for (size_t mg = 1; mg < mg_cycles; ++mg) {
        auto mg_bilform =
            CreateBilinearForm<MultigridPreconditioner<StiffnessOperator>>(
                x_d, x_d, {.build_mat = false, .mg_cycles = mg});
        for (int i = 0; i < mg_inner_cycles; i++) mg_bilform.Apply();
        std::cout << "\n\ttime-MG(" << mg
                  << ")-create: " << mg_bilform.TimeCreate() << std::flush;
        std::cout << "\n\ttime-MG(" << mg
                  << ")-per-apply: " << mg_bilform.TimePerApply() << std::flush;
      }
      std::cout << "\n\ttime-mg-test: " << Duration(time_start);
    }

    // Estimate.
    time_start = std::chrono::steady_clock::now();
    auto x_dd = x_d.DeepCopy();
    x_dd.FromVector(new_solution);
    UniformRefine(x_dd);
    std::cout << "\n\tXDeltaDelta-size: " << x_dd.Bfs().size();
    bilform = CreateBilinearForm<StiffnessOperator>(x_dd, x_dd, space_opts);
    bilform.Apply();
    auto residual = x_dd.ToVector();
    lf.Apply(x_dd.root());
    residual -= x_dd.ToVector();
    std::cout << "\n\tresidual-norm: " << residual.norm() << std::flush;
    std::cout << "\n\ttime-estimate: " << Duration(time_start)
              << "\n\testimate-memory: " << getmem() << std::flush;

    // Mark.
    time_start = std::chrono::steady_clock::now();
    x_dd.FromVector(residual);
    auto marked_nodes = Mark(x_dd, mark_theta);
    std::cout << "\n\tmarked-nodes: " << marked_nodes.size()
              << "\n\ttime-mark: " << Duration(time_start) << std::flush;

    // Refine.
    time_start = std::chrono::steady_clock::now();
    x_d.FromVector(new_solution);
    x_d.ConformingRefinement(x_dd, marked_nodes);
    solution = x_d.ToVector();
    std::cout << "\n\ttime-refine: " << Duration(time_start)
              << "\n\ttime-iteration: " << Duration(start_iteration)
              << "\n\ttime-total-algorithm: " << Duration(start_algorithm)
              << std::endl;
  }
  return 0;
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2016 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "swept_volume_signed_distance.h"
#include "LinSpaced.h"
#include "flood_fill.h"
#include "signed_distance.h"
#include "AABB.h"
#include "pseudonormal_test.h"
#include "per_face_normals.h"
#include "per_vertex_normals.h"
#include "per_edge_normals.h"
#include <Eigen/Geometry>

IGL_INLINE void igl::swept_volume_signed_distance(
  const Eigen::MatrixXd & V,
  const Eigen::MatrixXi & F,
  const std::function<Eigen::Affine3d(const double t)> & transform,
  const size_t & steps,
  const Eigen::MatrixXd & GV,
  const Eigen::RowVector3i & res,
  const double h,
  const double isolevel,
  const Eigen::VectorXd & S0,
  Eigen::VectorXd & S)
{
  using namespace std;
  using namespace igl;
  using namespace Eigen;
  S = S0;
  const VectorXd t = igl::LinSpaced<VectorXd >(steps,0,1);
  const bool finite_iso = isfinite(isolevel);
  const double extension = (finite_iso ? isolevel : 0) + sqrt(3.0)*h;
  Eigen::AlignedBox3d box(
    V.colwise().minCoeff().array()-extension,
    V.colwise().maxCoeff().array()+extension);
  // Precomputation
  Eigen::MatrixXd FN,VN,EN;
  Eigen::MatrixXi E;
  Eigen::VectorXi EMAP;
  per_face_normals(V,F,FN);
  per_vertex_normals(V,F,PER_VERTEX_NORMALS_WEIGHTING_TYPE_ANGLE,FN,VN);
  per_edge_normals(
    V,F,PER_EDGE_NORMALS_WEIGHTING_TYPE_UNIFORM,FN,EN,E,EMAP);
  AABB<MatrixXd,3> tree;
  tree.init(V,F);
  for(int ti = 0;ti<t.size();ti++)
  {
    const Affine3d At = transform(t(ti));
    for(int g = 0;g<GV.rows();g++)
    {
      // Don't bother finding out how deep inside points are.
      if(finite_iso && S(g)==S(g) && S(g)<isolevel-sqrt(3.0)*h)
      {
        continue;
      }
      const RowVector3d gv =
        (GV.row(g) - At.translation().transpose())*At.linear();
      // If outside of extended box, then consider it "far away enough"
      if(finite_iso && !box.contains(gv.transpose()))
      {
        continue;
      }
      RowVector3d c,n;
      int i;
      double sqrd,s;
      //signed_distance_pseudonormal(tree,V,F,FN,VN,EN,EMAP,gv,s,sqrd,i,c,n);
      const double min_sqrd =
        finite_iso ?
        pow(sqrt(3.)*h+isolevel,2) :
        numeric_limits<double>::infinity();
      sqrd = tree.squared_distance(V,F,gv,min_sqrd,i,c);
      if(sqrd<min_sqrd)
      {
        pseudonormal_test(V,F,FN,VN,EN,EMAP,gv,i,c,s,n);
        if(S(g) == S(g))
        {
          S(g) = std::min(S(g),s*sqrt(sqrd));
        }else
        {
          S(g) = s*sqrt(sqrd);
        }
      }
    }
  }

  if(finite_iso)
  {
    flood_fill(res,S);
  }else
  {
#ifndef NDEBUG
    // Check for nans
    std::for_each(S.data(),S.data()+S.size(),[](const double s){assert(s==s);});
#endif
  }
}

IGL_INLINE void igl::swept_volume_signed_distance(
  const Eigen::MatrixXd & V,
  const Eigen::MatrixXi & F,
  const std::function<Eigen::Affine3d(const double t)> & transform,
  const size_t & steps,
  const Eigen::MatrixXd & GV,
  const Eigen::RowVector3i & res,
  const double h,
  const double isolevel,
  Eigen::VectorXd & S)
{
  using namespace std;
  using namespace igl;
  using namespace Eigen;
  S = VectorXd::Constant(GV.rows(),1,numeric_limits<double>::quiet_NaN());
  return
    swept_volume_signed_distance(V,F,transform,steps,GV,res,h,isolevel,S,S);
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2018 John Maddock
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef BOOST_MATH_HYPERGEOMETRIC_1F1_ADDITION_THEOREMS_ON_Z_HPP
#define BOOST_MATH_HYPERGEOMETRIC_1F1_ADDITION_THEOREMS_ON_Z_HPP

#include <boost/math/tools/series.hpp>

//
// This file implements the addition theorems for 1F1 on z, specifically
// each function returns 1F1[a, b, z + k] for some integer k - there's
// no particular reason why k needs to be an integer, but no reason why
// it shouldn't be either.
//
// The functions are named hypergeometric_1f1_recurrence_on_z_[plus|minus|zero]_[plus|minus|zero]
// where a "plus" indicates forward recurrence, minus backwards recurrence, and zero no recurrence.
// So for example hypergeometric_1f1_recurrence_on_z_zero_plus uses forward recurrence on b and
// hypergeometric_1f1_recurrence_on_z_minus_minus uses backwards recurrence on both a and b.
//
// See https://dlmf.nist.gov/13.13
//

  namespace boost { namespace math { namespace detail {

     //
     // This works moderately well for a < 0, but has some very strange behaviour with
     // strings of values of the same sign followed by a sign switch then another
     // series all the same sign and so on.... doesn't converge smoothly either
     // but rises and falls in wave-like behaviour.... very slow to converge...
     //
     template <class T, class Policy>
     struct hypergeometric_1f1_recurrence_on_z_minus_zero_series
     {
        typedef T result_type;

        hypergeometric_1f1_recurrence_on_z_minus_zero_series(const T& a, const T& b, const T& z, int k_, const Policy& pol)
           : term(1), b_minus_a_plus_n(b - a), a_(a), b_(b), z_(z), n(0), k(k_)
        {
           BOOST_MATH_STD_USING
           int scale1(0), scale2(0);
           M = boost::math::detail::hypergeometric_1F1_imp(a, b, z, pol, scale1);
           M_next = boost::math::detail::hypergeometric_1F1_imp(T(a - 1), b, z, pol, scale2);
           if (scale1 != scale2)
              M_next *= exp(scale2 - scale1);
           if (M > 1e10f)
           {
              // rescale:
              int rescale = itrunc(log(fabs(M)));
              M *= exp(T(-rescale));
              M_next *= exp(T(-rescale));
              scale1 += rescale;
           }
           scaling = scale1;
        }
        T operator()()
        {
           T result = term * M;
           term *= b_minus_a_plus_n * k / ((z_ + k) * ++n);
           b_minus_a_plus_n += 1;
           T M2 = -((2 * (a_ - n) - b_ + z_) * M_next - (a_ - n) * M) / (b_ - (a_ - n));
           M = M_next;
           M_next = M2;

           return result;
        }
        int scale()const { return scaling; }
     private:
        T term, b_minus_a_plus_n, M, M_next, a_, b_, z_;
        int n, k, scaling;
     };

     template <class T, class Policy>
     T hypergeometric_1f1_recurrence_on_z_minus_zero(const T& a, const T& b, const T& z, int k, const Policy& pol, int& log_scaling)
     {
        BOOST_MATH_STD_USING
           BOOST_ASSERT((z + k) / z > 0.5f);
        hypergeometric_1f1_recurrence_on_z_minus_zero_series<T, Policy> s(a, b, z, k, pol);
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
        log_scaling += s.scale();
        boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1f1_recurrence_on_z_plus_plus<%1%>(%1%,%1%,%1%)", max_iter, pol);
        return result * exp(T(k)) * pow(z / (z + k), b - a);
     }

#if 0

     //
     // These are commented out as they are currently unused, but may find use in the future:
     //

     template <class T, class Policy>
     struct hypergeometric_1f1_recurrence_on_z_plus_plus_series
     {
        typedef T result_type;

        hypergeometric_1f1_recurrence_on_z_plus_plus_series(const T& a, const T& b, const T& z, int k_, const Policy& pol)
           : term(1), a_plus_n(a), b_plus_n(b), z_(z), n(0), k(k_)
        {
           M = boost::math::detail::hypergeometric_1F1_imp(a, b, z, pol);
           M_next = boost::math::detail::hypergeometric_1F1_imp(a + 1, b + 1, z, pol);
        }
        T operator()()
        {
           T result = term * M;
           term *= a_plus_n * k / (b_plus_n * ++n);
           a_plus_n += 1;
           b_plus_n += 1;
           // The a_plus_n == 0 case below isn't actually correct, but doesn't matter as that term will be zero
           // anyway, we just need to not divide by zero and end up with a NaN in the result.
           T M2 = (a_plus_n == -1) ? 1 : (a_plus_n == 0) ? 0 : (M_next * b_plus_n * (1 - b_plus_n + z_) + b_plus_n * (b_plus_n - 1) * M) / (a_plus_n * z_);
           M = M_next;
           M_next = M2;
           return result;
        }
        T term, a_plus_n, b_plus_n, M, M_next, z_;
        int n, k;
     };

     template <class T, class Policy>
     T hypergeometric_1f1_recurrence_on_z_plus_plus(const T& a, const T& b, const T& z, int k, const Policy& pol)
     {
        hypergeometric_1f1_recurrence_on_z_plus_plus_series<T, Policy> s(a, b, z, k, pol);
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
        boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1f1_recurrence_on_z_plus_plus<%1%>(%1%,%1%,%1%)", max_iter, pol);
        return result;
     }

     template <class T, class Policy>
     struct hypergeometric_1f1_recurrence_on_z_zero_minus_series
     {
        typedef T result_type;

        hypergeometric_1f1_recurrence_on_z_zero_minus_series(const T& a, const T& b, const T& z, int k_, const Policy& pol)
           : term(1), b_pochhammer(1 - b), x_k_power(-k_ / z), b_minus_n(b), a_(a), z_(z), b_(b), n(0), k(k_)
        {
           M = boost::math::detail::hypergeometric_1F1_imp(a, b, z, pol);
           M_next = boost::math::detail::hypergeometric_1F1_imp(a, b - 1, z, pol);
        }
        T operator()()
        {
           BOOST_MATH_STD_USING
           T result = term * M;
           term *= b_pochhammer * x_k_power / ++n;
           b_pochhammer += 1;
           b_minus_n -= 1;
           T M2 = (M_next * b_minus_n * (1 - b_minus_n - z_) + z_ * (b_minus_n - a_) * M) / (-b_minus_n * (b_minus_n - 1));
           M = M_next;
           M_next = M2;
           return result;
        }
        T term, b_pochhammer, x_k_power, M, M_next, b_minus_n, a_, z_, b_;
        int n, k;
     };

     template <class T, class Policy>
     T hypergeometric_1f1_recurrence_on_z_zero_minus(const T& a, const T& b, const T& z, int k, const Policy& pol)
     {
        BOOST_MATH_STD_USING
           BOOST_ASSERT(abs(k) < fabs(z));
        hypergeometric_1f1_recurrence_on_z_zero_minus_series<T, Policy> s(a, b, z, k, pol);
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
        boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1f1_recurrence_on_z_plus_plus<%1%>(%1%,%1%,%1%)", max_iter, pol);
        return result * pow((z + k) / z, 1 - b);
     }

     template <class T, class Policy>
     struct hypergeometric_1f1_recurrence_on_z_plus_zero_series
     {
        typedef T result_type;

        hypergeometric_1f1_recurrence_on_z_plus_zero_series(const T& a, const T& b, const T& z, int k_, const Policy& pol)
           : term(1), a_pochhammer(a), z_plus_k(z + k_), b_(b), a_(a), z_(z), n(0), k(k_)
        {
           M = boost::math::detail::hypergeometric_1F1_imp(a, b, z, pol);
           M_next = boost::math::detail::hypergeometric_1F1_imp(a + 1, b, z, pol);
        }
        T operator()()
        {
           T result = term * M;
           term *= a_pochhammer * k / (++n * z_plus_k);
           a_pochhammer += 1;
           T M2 = (a_pochhammer == -1) ? 1 : (a_pochhammer == 0) ? 0 : (M_next * (2 * a_pochhammer - b_ + z_) + (b_ - a_pochhammer) * M) / a_pochhammer;
           M = M_next;
           M_next = M2;

           return result;
        }
        T term, a_pochhammer, z_plus_k, M, M_next, b_minus_n, a_, b_, z_;
        int n, k;
     };

     template <class T, class Policy>
     T hypergeometric_1f1_recurrence_on_z_plus_zero(const T& a, const T& b, const T& z, int k, const Policy& pol)
     {
        BOOST_MATH_STD_USING
           BOOST_ASSERT(k / z > -0.5f);
        //BOOST_ASSERT(floor(a) != a || a > 0);
        hypergeometric_1f1_recurrence_on_z_plus_zero_series<T, Policy> s(a, b, z, k, pol);
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
        boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1f1_recurrence_on_z_plus_plus<%1%>(%1%,%1%,%1%)", max_iter, pol);
        return result * pow(z / (z + k), a);
     }

     template <class T, class Policy>
     struct hypergeometric_1f1_recurrence_on_z_zero_plus_series
     {
        typedef T result_type;

        hypergeometric_1f1_recurrence_on_z_zero_plus_series(const T& a, const T& b, const T& z, int k_, const Policy& pol)
           : term(1), b_minus_a_plus_n(b - a), b_plus_n(b), a_(a), z_(z), n(0), k(k_)
        {
           M = boost::math::detail::hypergeometric_1F1_imp(a, b, z, pol);
           M_next = boost::math::detail::hypergeometric_1F1_imp(a, b + 1, z, pol);
        }
        T operator()()
        {
           T result = term * M;
           term *= b_minus_a_plus_n * -k / (b_plus_n * ++n);
           b_minus_a_plus_n += 1;
           b_plus_n += 1;
           T M2 = (b_plus_n * (b_plus_n - 1) * M + b_plus_n * (1 - b_plus_n - z_) * M_next) / (-z_ * b_minus_a_plus_n);
           M = M_next;
           M_next = M2;

           return result;
        }
        T term, b_minus_a_plus_n, M, M_next, b_minus_n, a_, b_plus_n, z_;
        int n, k;
     };

     template <class T, class Policy>
     T hypergeometric_1f1_recurrence_on_z_zero_plus(const T& a, const T& b, const T& z, int k, const Policy& pol)
     {
        BOOST_MATH_STD_USING
           hypergeometric_1f1_recurrence_on_z_zero_plus_series<T, Policy> s(a, b, z, k, pol);
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
        boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1f1_recurrence_on_z_plus_plus<%1%>(%1%,%1%,%1%)", max_iter, pol);
        return result * exp(T(k));
     }
     //
     // I'm unable to find any situation where this series isn't divergent and therefore
     // is probably quite useless:
     //
     template <class T, class Policy>
     struct hypergeometric_1f1_recurrence_on_z_minus_minus_series
     {
        typedef T result_type;

        hypergeometric_1f1_recurrence_on_z_minus_minus_series(const T& a, const T& b, const T& z, int k_, const Policy& pol)
           : term(1), one_minus_b_plus_n(1 - b), a_(a), b_(b), z_(z), n(0), k(k_)
        {
           M = boost::math::detail::hypergeometric_1F1_imp(a, b, z, pol);
           M_next = boost::math::detail::hypergeometric_1F1_imp(a - 1, b - 1, z, pol);
        }
        T operator()()
        {
           T result = term * M;
           term *= one_minus_b_plus_n * k / (z_ * ++n);
           one_minus_b_plus_n += 1;
           T M2 = -((b_ - n) * (1 - b_ + n + z_) * M_next - (a_ - n) * z_ * M) / ((b_ - n) * (b_ - n - 1));
           M = M_next;
           M_next = M2;

           return result;
        }
        T term, one_minus_b_plus_n, M, M_next, a_, b_, z_;
        int n, k;
     };

     template <class T, class Policy>
     T hypergeometric_1f1_recurrence_on_z_minus_minus(const T& a, const T& b, const T& z, int k, const Policy& pol)
     {
        BOOST_MATH_STD_USING
           hypergeometric_1f1_recurrence_on_z_minus_minus_series<T, Policy> s(a, b, z, k, pol);
        boost::uintmax_t max_iter = boost::math::policies::get_max_series_iterations<Policy>();
        T result = boost::math::tools::sum_series(s, boost::math::policies::get_epsilon<T, Policy>(), max_iter);
        boost::math::policies::check_series_iterations<T>("boost::math::hypergeometric_1f1_recurrence_on_z_plus_plus<%1%>(%1%,%1%,%1%)", max_iter, pol);
        return result * exp(T(k)) * pow((z + k) / z, 1 - b);
     }
#endif

  } } } // namespaces

#endif // BOOST_MATH_HYPERGEOMETRIC_1F1_ADDITION_THEOREMS_ON_Z_HPP


//  Copyright John Maddock 2009.
//  Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)

#include <boost/integer/static_min_max.hpp> // must be the only #include!

int main()
{
   boost::static_min_max_signed_type m = boost::static_signed_min<2, 3>::value + boost::static_signed_max<2, 3>::value;
   (void)m;
   boost::static_min_max_unsigned_type u = boost::static_unsigned_min<2, 3>::value + boost::static_unsigned_max<2, 3>::value;
   (void)u;
}


// PGASGraph
#include <pgas-graph/graph-utilities.hpp>
#include <pgas-graph/pgas-graph.h>

// STL
#include <chrono>
#include <iosfwd>
#include <iostream>
#include <map>
#include <random>
#include <set>
#include <string>

// Boost
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/small_world_generator.hpp>
#include <boost/program_options.hpp>
#include <boost/random/linear_congruential.hpp>

using MyBundledVertex = PGASGraph::Graph<std::string, size_t>::Vertex;
using MyBundledEdge =
    PGASGraph::Graph<std::string, size_t>::Vertex::weight_node_t;

std::ostream &operator<<(std::ostream &os, const MyBundledVertex &e) noexcept {
  os << e.id;
  return os;
}

std::ostream &operator<<(std::ostream &os, const MyBundledEdge &e) noexcept {
  os << e.first;
  return os;
}

bool operator==(const MyBundledVertex &lhs,
                const MyBundledVertex &rhs) noexcept {
  return lhs.id == rhs.id;
}

bool operator!=(const MyBundledVertex &lhs,
                const MyBundledVertex &rhs) noexcept {
  return !(lhs == rhs);
}

template <typename Graph, typename BundledVertex>
typename boost::graph_traits<Graph>::vertex_descriptor
add_bundled_vertex(const BundledVertex &v, Graph &g) noexcept {
  static_assert(!std::is_const<Graph>::value, "graph cannot be const");
  const auto vd = boost::add_vertex(g);
  g[vd] = v;
  return vd;
}

template <typename Graph> void set_my_bundled_vertexes(Graph &g) {
  static_assert(!std::is_const<Graph>::value, "graph cannot be const");
  const auto vip = boost::vertices(g);
  const auto j = vip.second;

  PGASGraph::Id id{0};
  for (auto i = vip.first; i != j; ++i) {
    g[*i] = MyBundledVertex(id++);
  }
}

template <typename Graph, typename Function>
void for_each_bundled_vertex(Graph &g, Function f) {
  static_assert(!std::is_const<Graph>::value, "graph cannot be const");
  const auto vip = boost::vertices(g);
  const auto j = vip.second;

  PGASGraph::Id id{0};
  for (auto i = vip.first; i != j; ++i) {
    f(g[*i]);
  }
}

int main(int argc, char *argv[]) {
  size_t totalNumberVertices{200};
  GraphUtilities::BoostGraph boostGraph{
      GraphUtilities::generateRandomConnectedGraph<GraphUtilities::BoostGraph>(totalNumberVertices, 30.0)};

  // upcxx::init();
  // using GT = boost::adjacency_list<boost::vecS, boost::vecS,
  // boost::undirectedS,
  //                                  MyBundledVertex, MyBundledEdge>;
  // using ERGen = boost::erdos_renyi_iterator<boost::minstd_rand, GT>;

  // // Generate the graph.
  // boost::minstd_rand gen;
  // const size_t vertexCount{63};
  // GT g(ERGen(gen, vertexCount, 0.05), ERGen(), 100);

  // // Assign ids to the vertices.
  // set_my_bundled_vertexes(g);

  // // Assign weights to the edges.
  // auto edges = boost::edges(g);
  // int weight{0};
  // for (auto it = edges.first; it != edges.second; ++it) {
  //   g[*it].first = weight++;
  // }

  // // Convert to the PGASGraph.
  // const auto verticesPerRank{(vertexCount + upcxx::rank_n() - 1) /
  //                            upcxx::rank_n()};
  // PGASGraph::logMsg("vertexCount=" + std::to_string(vertexCount));
  // PGASGraph::logMsg("verticesPerRank=" + std::to_string(verticesPerRank));
  // PGASGraph::Graph<std::string, int> pgasGraph(vertexCount, verticesPerRank);
  // auto vs = boost::vertices(g);
  // weight = 0;
  // for (auto vit = vs.first; vit != vs.second; ++vit) {
  //   auto neighbors = boost::adjacent_vertices(*vit, g);
  //   for (auto nit = neighbors.first; nit != neighbors.second; ++nit) {
  //     pgasGraph.AddEdge({g[*vit].id, g[*nit].id, weight++});
  //   }
  // }

  // // auto start = std::chrono::steady_clock::now();
  // // pgasGraph.MST();
  // // auto end = std::chrono::steady_clock::now();

  // // PGASGraph::logMsg(
  // //     "MST Elapsed time: " +
  // //     std::to_string(std::chrono::duration<double>(end - start).count()));

  // upcxx::finalize();
  return 0;
};


/**
 * @file TransBeliefHarness.cpp
 * Test Harness for dec_brl::TransBelief class
 * @author Luke Teacy
 */

#include <dec_brl/TransBelief.h>
#include "register.h"
#include <iostream>
#include <boost/random/mersenne_twister.hpp>
#include <cmath>

/**
 * Private module namespace.
 */
namespace {

    /**
     * Function used for testing drawn Transition probability matrices.
     */
    int testSampledCPT
    (
     dec_brl::TransBelief& paramDist,
     dec_brl::SampledTransProb& cpt
    )
    {
        //**********************************************************************
        //  Initialise previous and next states. Note that we're assuming the
        //  test variables and domain sizes as in the main function:
        //  condition variables: 1,2,3 with sizes 2,3,4.
        //  domain variables: 1,2 with sizes 2,3.
        //  Thus, when sampling, we expect 1 and 2 to change, but 3 to stay the
        //  same.
        //**********************************************************************
        Eigen::Vector4i prevStates, nxtStates, sizes; // first element not used
        prevStates << 0,0,0,0;
        nxtStates << 0,0,0,0;
        sizes << 1,2,3,4;

        //**********************************************************************
        //  Set up random number generator used for sampling
        //**********************************************************************
        boost::mt19937 randGenerator; // generates uniform random numbers

        //**********************************************************************
        //  Check that we can change the CPT by resampling
        //**********************************************************************
        Eigen::ArrayXXd oldCPT = cpt.getCPT();
        Eigen::ArrayXXd newCPT = cpt.getCPT();

        if(!oldCPT.isApprox(newCPT))
        {
            std::cout << "inconsistent CPT before redraw" << std::endl;
            return EXIT_FAILURE;
        }

        cpt.drawNewCPT(randGenerator);
        newCPT = cpt.getCPT();

        if(oldCPT.isApprox(newCPT))
        {
            std::cout << "No change in CPT after redraw" << std::endl;
            return EXIT_FAILURE;
        }

        //**********************************************************************
        //  Set up matrix to hold counts for each possible state value
        //**********************************************************************
        Eigen::ArrayXXd counts;
        counts.setConstant(newCPT.rows(),newCPT.cols(),0);

        //**********************************************************************
        //  Draw a lot of samples
        //**********************************************************************
        const int N_SAMPLES = 100000;
        for(int k=0; k<N_SAMPLES; ++k)
        {
            //******************************************************************
            //  Draw new states
            //******************************************************************
            prevStates.swap(nxtStates); // last new states become prev states
            cpt.drawNextStates(randGenerator, prevStates, nxtStates);

            //******************************************************************
            //  Check that cond variable that is not in domain stays the same
            //******************************************************************
            if(nxtStates[3]!=0)
            {
                std::cout << "detected change in non-domain variable: "
                << std::endl;
                std::cout << "prev: " << prevStates << std::endl;
                std::cout << "nxt: " << nxtStates << std::endl;
                return EXIT_FAILURE;
            }

            //******************************************************************
            //  Check that the maximum values for each variable is not exceeded
            //******************************************************************
            bool tooLow = (nxtStates.array()<0).any();
            bool tooHigh = (nxtStates.array()>=sizes.array()).any();
            if( tooLow || tooHigh )
            {
                std::cout << "sampled state out of bounds: "
                << nxtStates << std::endl;
                return EXIT_FAILURE;
            }

            //******************************************************************
            //  Update count of observed values
            //******************************************************************
            int condInd = maxsum::sub2ind(sizes.begin(), sizes.end(),
                                          prevStates.begin(), prevStates.end());

            int domainInd = maxsum::sub2ind(sizes.begin(), sizes.end()-1,
                                            nxtStates.begin(),
                                            nxtStates.end()-1);

            counts(domainInd,condInd) += 1;

        } // for loop

        //**********************************************************************
        //  Check count against expected result
        //**********************************************************************
        Eigen::ArrayXXd expCounts = newCPT.rowwise() * counts.colwise().sum();
        std::cout << "Expected number of samples:" << std::endl;
        std::cout << expCounts << std::endl;
        std::cout << "Actual number of samples:" << std::endl;
        std::cout << counts << std::endl;
        std::cout << "diff: " << std::endl;
        std::cout << (expCounts-counts).array().abs() << std::endl;

        if(!expCounts.matrix().isApprox(counts.matrix()))
        {
            std::cout << "COUNTS NOT EQUAL WITHIN PRECISION" << std::endl;
            return EXIT_FAILURE;
        }
        else
        {
            std::cout << "COUNTS OK" << std::endl;
            return EXIT_SUCCESS;
        }

    } // testSampledCPT

} // module namespace

/**
 * Main function.
 */
int main()
{
    using namespace dec_brl;

    //**************************************************************************
    // Register some test variables with the maxsum library
    //**************************************************************************
    std::cout << "Registering maxsum variables..." << std::endl;
    Eigen::Vector3i vars, sizes;
    vars << 1,2,3;
    sizes << 2,3,4;

    maxsum::registerVariables(vars.begin(), vars.end(),
                              sizes.begin(), sizes.end());

    //**************************************************************************
    // Create belief distribution for transition probability over variables 1
    // and 2, given 1,2 and 3.
    //**************************************************************************
    std::cout << "Constructing beliefs..." << std::endl;
    dec_brl::TransBelief beliefs(vars,vars.head(2));

    std::cout << "Beliefs:" << std::endl;
    std::cout << beliefs << std::endl;

    //**************************************************************************
    // Check that the reported domain sizes are correct.
    // Should be the product of the domain sizes for corresponding variables
    //**************************************************************************
    const int CORRECT_COND_SIZE = sizes[0]*sizes[1]*sizes[2];
    const int CORRECT_DOMAIN_SIZE = sizes[0]*sizes[1];
    if(beliefs.condSize()!=CORRECT_COND_SIZE)
    {
        std::cout << "Incorrect conditional size: " << beliefs.condSize()
        << " should be " << (2*3*4) << std::endl;
        return EXIT_FAILURE;
    }

    if(beliefs.domainSize() != CORRECT_DOMAIN_SIZE)
    {
        std::cout << "Incorrect domain size: " << beliefs.domainSize()
        << " should be " << (2*3) << std::endl;
        return EXIT_FAILURE;
    }
    std::cout << "Domain sizes: OK" << std::endl;

    //**************************************************************************
    //  Ensure that is equal to prior
    //**************************************************************************
    if(!beliefs.getAlpha().isApproxToConstant(TransBelief::DEFAULT_ALPHA))
    {
        std::cout << "Hyperparameters incorrect - should all be "
        << TransBelief::DEFAULT_ALPHA << std::endl;
        return EXIT_FAILURE;
    }
    std::cout << "Prior OK" << std::endl;

    //**************************************************************************
    //  Try to set hyperparameters to some constant scalar
    //**************************************************************************
    const double NEW_PRIOR = 2.5;
    beliefs.setAlpha(NEW_PRIOR);

    std::cout << "Beliefs:" << std::endl;
    std::cout << beliefs << std::endl;

    if(!beliefs.getAlpha().isApproxToConstant(NEW_PRIOR))
    {
        std::cout << "Hyperparameters incorrect after set - should all be "
        << NEW_PRIOR << std::endl;
        return EXIT_FAILURE;
    }

    std::cout << "Prior reset OK" << std::endl;

    //**************************************************************************
    //  Try to observe a few different values based on linear indices
    //**************************************************************************
    beliefs.setAlpha(1);
    int condInd = beliefs.condSize() / 2; // select middle value to update
    int domainInd = beliefs.domainSize() / 2;
    beliefs.observeByInd(condInd, domainInd);

    Eigen::Matrix<double, 2*3, 2*3*4> correctValue;
    correctValue.setConstant(1);
    correctValue(domainInd,condInd) += 1;

    if(beliefs.getAlpha() != correctValue)
    {
        std::cout << "Incorrect posterior hyperparameters" << std::endl;
        std::cout << beliefs.getAlpha() << std::endl;
        std::cout << "BUT SHOULD BE" << std::endl;
        std::cout << correctValue << std::endl;
        return EXIT_FAILURE;
    }

    std::cout << "Linear Observation OK" << std::endl;

    //**************************************************************************
    // Try to observe a few different values based on sub indices
    //**************************************************************************
    Eigen::Vector3i condVals;
    Eigen::Vector2i domainVals;
    condVals << 1, 0, 2;
    domainVals << 0, 1;

    condInd = maxsum::sub2ind(sizes.begin(), sizes.end(),
                              condVals.begin(), condVals.end());

    domainInd = maxsum::sub2ind(sizes.begin(), sizes.end()-1,
                                domainVals.begin(), domainVals.end());

    correctValue(domainInd,condInd) += 1;
    beliefs.observeByVec(condVals.begin(), condVals.end(), domainVals.begin(), domainVals.end());

    if(beliefs.getAlpha() != correctValue)
    {
        std::cout << "Incorrect posterior hyperparameters" << std::endl;
        std::cout << beliefs.getAlpha() << std::endl;
        std::cout << "BUT SHOULD BE" << std::endl;
        std::cout << correctValue << std::endl;
        return EXIT_FAILURE;
    }

    std::cout << "Vector Observation OK" << std::endl;

    //**************************************************************************
    //  Try to observe a few different values based on mapped indices
    //**************************************************************************
    Eigen::Vector4i condMap;    // using vectors as maps
    Eigen::Vector3i domainMap;  // e.g. condMap[var] = val
    condMap << 0, 1, 1, 0;
    domainMap << 0, 1, 2;

    condInd = maxsum::sub2ind(sizes.begin(), sizes.end(),
                              condMap.begin()+1, condMap.end());

    domainInd = maxsum::sub2ind(sizes.begin(), sizes.end()-1,
                                domainMap.begin()+1, domainMap.end());

    correctValue(domainInd,condInd) += 2; // observe something twice
    beliefs.observeByMap(condMap, domainMap);
    beliefs.observeByMap(condMap, domainMap);

    if(beliefs.getAlpha() != correctValue)
    {
        std::cout << "Incorrect posterior hyperparameters" << std::endl;
        std::cout << beliefs.getAlpha() << std::endl;
        std::cout << "BUT SHOULD BE" << std::endl;
        std::cout << correctValue << std::endl;
        return EXIT_FAILURE;
    }

    std::cout << "Map Observation OK" << std::endl;

    std::cout << "Posterior:" << std::endl;
    std::cout << beliefs << std::endl;

    //**************************************************************************
    // Manually Calculate the correct expected CPT for comparision.
    //**************************************************************************
    std::cout << "CORRECT EXPECTED CPT CALCUATION" << std::endl;
    std::cout << correctValue << std::endl;
    std::cout << "DIVIDED BY" << std::endl;

    Eigen::RowVectorXd totals(correctValue.cols());
    totals = correctValue.colwise().sum();

    std::cout << totals << std::endl;
    std::cout << "EQUALS" << std::endl;

    correctValue.array().rowwise() /= totals.array();

    std::cout << correctValue << std::endl;

    std::cout << "MARGINALS SHOULD BE ONE" << std::endl;
    totals = correctValue.colwise().sum();

    std::cout << totals << std::endl;

    if(!totals.isApproxToConstant(1))
    {
        std::cout << "INCORRECT MARGINALS IN TEST HARNESS CODE" << std::endl;
        return EXIT_FAILURE;
    }

    //**************************************************************************
    // Check that the mean function works
    //**************************************************************************
    std::cout << "PERFORMING CHECK..." << std::endl;
    Eigen::MatrixXd expCPT;
    beliefs.getMean(expCPT);

    if( (correctValue.rows()!=expCPT.rows()) ||
        (correctValue.cols()!=expCPT.cols()) ||
        (!expCPT.isApprox(correctValue))     )
    {
        std::cout << "Incorrect expected CPT" << std::endl;
        std::cout << expCPT << std::endl;
        std::cout << "BUT SHOULD BE" << std::endl;
        std::cout << correctValue << std::endl;
        return EXIT_FAILURE;
    }

    std::cout << "expCPT is" << std::endl;
    std::cout << expCPT << std::endl << "ALL OK" << std::endl;

    //**************************************************************************
    //  Try to get the expCPT for a specific set of conditions
    //**************************************************************************
    std::cout << "Checking specific expected CPT..." << std::endl;
    Eigen::VectorXd secondCPT;
    beliefs.getMeanByMap(secondCPT,condMap);

    std::cout << secondCPT.transpose() << std::endl;
    std::cout << expCPT.col(condInd).transpose() << std::endl;

    if( (secondCPT.rows()!=expCPT.rows()) || (secondCPT.cols()!=1) ||
        (!secondCPT.isApprox(expCPT.col(condInd))) )
    {
        std::cout << "Inconsistent expectations" << std::endl;
        std::cout << "max difference: " << std::endl;
        Eigen::VectorXd diff = secondCPT-expCPT.col(condInd);
        std::cout << diff.transpose().array().abs() << std::endl;
        return EXIT_FAILURE;
    }

    //**************************************************************************
    //  Check that the random generator works
    //**************************************************************************
    std::cout << "Trying to sample some CPTs" << std::endl;
    const int NUM_SAMPLES = 100;
    boost::mt19937 randGenerator; // generates uniform random numbers
    Eigen::MatrixXd oldSample(expCPT);
    Eigen::MatrixXd newSample(expCPT);
    Eigen::MatrixXd sampleMean(expCPT.rows(),expCPT.cols());
    for(int k=0; k<NUM_SAMPLES; ++k)
    {
        //*********************************************************************
        //  Get a sampled CPT and update sum for future reference
        //*********************************************************************
        beliefs.sample(randGenerator,newSample);
        sampleMean += newSample;

        //*********************************************************************
        //  Check that its not the same as the previous sample, which would be
        //  odd.
        //*********************************************************************
        if(oldSample.isApprox(newSample))
        {
            std::cout << "this sample matches the last sample - VERY UNLIKELY";
            std::cout << std::endl;
            return EXIT_FAILURE;
        }

        //*********************************************************************
        //  Check that this is a valid CPT (each column sums to one)
        //*********************************************************************
        Eigen::RowVectorXd sampleTotals = newSample.colwise().sum();
        if(!sampleTotals.isApproxToConstant(1))
        {
            std::cout << "Sampled totals do not all sum to one:" << std::endl;
            std::cout << sampleTotals << std::endl;
            return EXIT_FAILURE;
        }

        //*********************************************************************
        //  Store this sample for comparsion in the next iteration
        //*********************************************************************
        newSample.swap(oldSample);

    } // for loop

    //**************************************************************************
    //  Ensure that sample mean is reasonably close to the actual mean
    //**************************************************************************
    std::cout << "Mean Sampled CPT" << std::endl;
    sampleMean /= NUM_SAMPLES;
    const double VAR_UPPER_BOUND = 0.02; // based on quick Diriclet calculation
    const double SAMPLE_STDERR = std::sqrt(VAR_UPPER_BOUND/NUM_SAMPLES);
    const double ACCEPT_PRECISION = SAMPLE_STDERR*8; // no more than 8 std dev
    std::cout << sampleMean << std::endl;

    if(!sampleMean.isApprox(expCPT,ACCEPT_PRECISION))
    {
        std::cout << "Some sample means differ by more than expected\n";
        std::cout << "DIFFERENCE: (threshold " << ACCEPT_PRECISION << ")\n";
        std::cout << (sampleMean-expCPT).array().abs() << std::endl;
        return EXIT_FAILURE;
    }
    std::cout << "Sample mean within acceptable precision: ";
    std::cout << ACCEPT_PRECISION << std::endl;

    //**************************************************************************
    //  Try drawing from a sampled CPT
    //**************************************************************************
    SampledTransProb transProb(beliefs,randGenerator);
    if(EXIT_SUCCESS!=testSampledCPT(beliefs, transProb))
    {
        return EXIT_FAILURE;
    }

    //**************************************************************************
    // If we get this far, everything passed.
    //**************************************************************************
    std::cout << "All TransBelief tests passed" << std::endl;
    return EXIT_SUCCESS;

} // function main


/*******************************************************************************
Copyright(c) 2015-2020 Parker Hannifin Corp. All rights reserved.

MIT Licensed. See the included LICENSE.txt for a copy of the full MIT License.
*******************************************************************************/
#include "mscl/MicroStrain/Matrix.h"
#include "mscl/MicroStrain/ByteStream.h"

#include <boost/test/unit_test.hpp>

using namespace mscl;

BOOST_AUTO_TEST_SUITE(Matrix_Test)

BOOST_AUTO_TEST_CASE(Matrix_defaultConstructor)
{
    Matrix m;

    //check the matrix initialized correctly
    BOOST_CHECK_EQUAL(m.rows(), 0);
    BOOST_CHECK_EQUAL(m.columns(), 0);
}

BOOST_AUTO_TEST_CASE(Matrix_valuesType)
{
    ByteStream b;
    Matrix m(0, 0, valueType_float, b);

    //check that the value type is correct
    BOOST_CHECK_EQUAL(m.valuesType(), valueType_float);

    Matrix m2(0, 0, valueType_int32, b);

    //check that the value type is correct
    BOOST_CHECK_EQUAL(m2.valuesType(), valueType_int32);
}

BOOST_AUTO_TEST_CASE(Matrix_rows_columns)
{
    ByteStream b;
    Matrix m(10, 12, valueType_float, b);

    //check that the number of rows is correct
    BOOST_CHECK_EQUAL(m.rows(), 10);

    //check that the number of columns is correct
    BOOST_CHECK_EQUAL(m.columns(), 12);
}

BOOST_AUTO_TEST_CASE(Matrix_atFloat)
{
    //add 6 floats to the Matrix
    ByteStream b;
    b.append_float(1.0f);    //row 0, col 0
    b.append_float(2.0f);    //row 0, col 1
    b.append_float(3.0f);    //row 0, col 2
    b.append_float(5.0f);    //row 1, col 0
    b.append_float(6.0f);    //row 1, col 1
    b.append_float(1.234f);    //row 1, col 2

    //create a 2x3 Matrix
    Matrix m(2, 3, valueType_float, b);

    //check that the values are correct at the given indices
    BOOST_CHECK_CLOSE(m.as_floatAt(0, 0), 1.0, 0.001);
    BOOST_CHECK_CLOSE(m.as_floatAt(0, 1), 2.0, 0.001);
    BOOST_CHECK_CLOSE(m.as_floatAt(0, 2), 3.0, 0.001);
    BOOST_CHECK_CLOSE(m.as_floatAt(1, 0), 5.0, 0.001);
    BOOST_CHECK_CLOSE(m.as_floatAt(1, 1), 6.0, 0.001);
    BOOST_CHECK_CLOSE(m.as_floatAt(1, 2), 1.234, 0.001);

    //check that asking for an invalid index throws an exception
    BOOST_CHECK_THROW(m.as_floatAt(0, 3), std::out_of_range);
    BOOST_CHECK_THROW(m.as_floatAt(2, 0), std::out_of_range);
}

BOOST_AUTO_TEST_CASE(Matrix_atUint16)
{
    //add 8 values to the Matrix
    ByteStream b;
    b.append_uint16(1);        //row 0, col 0
    b.append_uint16(2);        //row 0, col 1
    b.append_uint16(3);        //row 1, col 0
    b.append_uint16(4);        //row 1, col 1
    b.append_uint16(5);        //row 2, col 0
    b.append_uint16(6);        //row 2, col 1
    b.append_uint16(7);        //row 3, col 0
    b.append_uint16(12345);    //row 3, col 1

    //create a 4x2 Matrix
    Matrix m(4, 2, valueType_uint16, b);

    //check that the values are correct at the given indices
    BOOST_CHECK_EQUAL(m.as_uint16At(0, 0), 1);
    BOOST_CHECK_EQUAL(m.as_uint16At(0, 1), 2);
    BOOST_CHECK_EQUAL(m.as_uint16At(1, 0), 3);
    BOOST_CHECK_EQUAL(m.as_uint16At(1, 1), 4);
    BOOST_CHECK_EQUAL(m.as_uint16At(2, 0), 5);
    BOOST_CHECK_EQUAL(m.as_uint16At(2, 1), 6);
    BOOST_CHECK_EQUAL(m.as_uint16At(3, 0), 7);
    BOOST_CHECK_EQUAL(m.as_uint16At(3, 1), 12345);

    //check that asking for an invalid index throws an exception
    BOOST_CHECK_THROW(m.as_uint16At(0, 2), std::out_of_range);
    BOOST_CHECK_THROW(m.as_uint16At(4, 0), std::out_of_range);
}

BOOST_AUTO_TEST_CASE(Matrix_as_uint8At)
{
    //add 8 values to the Matrix
    ByteStream b;
    b.append_uint8(1);        //row 0, col 0
    b.append_uint8(2);        //row 0, col 1
    b.append_uint8(3);        //row 1, col 0
    b.append_uint8(4);        //row 1, col 1
    b.append_uint8(5);        //row 2, col 0
    b.append_uint8(6);        //row 2, col 1
    b.append_uint8(7);        //row 3, col 0
    b.append_uint8(8);        //row 3, col 1

    //create a 4x2 Matrix
    Matrix m(4, 2, valueType_uint8, b);

    //check that the values are correct at the given indices
    BOOST_CHECK_EQUAL(m.as_uint8At(0, 0), 1);
    BOOST_CHECK_EQUAL(m.as_uint8At(0, 1), 2);
    BOOST_CHECK_EQUAL(m.as_uint8At(1, 0), 3);
    BOOST_CHECK_EQUAL(m.as_uint8At(1, 1), 4);
    BOOST_CHECK_EQUAL(m.as_uint8At(2, 0), 5);
    BOOST_CHECK_EQUAL(m.as_uint8At(2, 1), 6);
    BOOST_CHECK_EQUAL(m.as_uint8At(3, 0), 7);
    BOOST_CHECK_EQUAL(m.as_uint8At(3, 1), 8);

    //check that asking for an invalid index throws an exception
    BOOST_CHECK_THROW(m.as_uint8At(0, 2), std::out_of_range);
    BOOST_CHECK_THROW(m.as_uint8At(4, 0), std::out_of_range);
}

BOOST_AUTO_TEST_SUITE_END()


#include "GAGPL/SPECTRUM/IsotopicDistribution.h"
#include <time.h>
#include <boost/filesystem.hpp>

#pragma warning(disable : 4996)

using namespace std;
using namespace gag;
using namespace boost::filesystem;

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

		vector<pair<Composition, int> > compo_vec;
		//Composition compo1("C50H71N12O13");
    Composition compo1("C44H64N3O12S4");
    Composition compo2("C23832H37816N6528O7031S170");
		compo_vec.push_back(std::make_pair(compo1, 11));
		compo_vec.push_back(std::make_pair(compo2, 1325));

		clock_t t;

		// Parse the input path and generate output file name.
		for(vector<pair<Composition, int> >::iterator iter = compo_vec.begin(); iter != compo_vec.end(); iter++)
		{
			// Initial time.
			t = clock();

			IsotopicDistribution iso_dist(iter->first, iter->second);
			AggregatedIsotopicVariants peakset = iso_dist.getAggregatedIsotopicVariants();

			// Ending time.
			t = clock()-t;
			cout << "It took " << ((double)t)/CLOCKS_PER_SEC << " seconds to calculate " << iter->first.getCompositionString() << endl;

			cout << "Composition: " << iter->first.getCompositionString() << endl;
			cout << "Monoisotopic mass: " << iter->first.getMass() << endl;
			cout << "Average mass: " << iter->first.getAverageMass() << endl;
			cout << "Estimated average mass: " << iso_dist.getAverageMass() << endl;
			peakset.printPeakList<peak_mz>(cout);

		}
	}
	catch(std::exception& e)
	{
		cout << "Exception: " << e.what() << endl;
	}

	cout << "The End!" << endl;
	//cin.get();

	return 0;

}


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

#include <cap/resistor_capacitor.h>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/filesystem.hpp>
#include <cmath>
#include <fstream>
#include <stdexcept>

namespace cap
{

REGISTER_ENERGY_STORAGE_DEVICE(SeriesRC)
REGISTER_ENERGY_STORAGE_DEVICE(ParallelRC)

void ParallelRC::inspect(EnergyStorageDeviceInspector *inspector)
{
  inspector->inspect(this);
}

void SeriesRC::inspect(EnergyStorageDeviceInspector *inspector)
{
  inspector->inspect(this);
}

SeriesRC::SeriesRC(boost::property_tree::ptree const &ptree,
                   boost::mpi::communicator const &comm)
    : EnergyStorageDevice(comm), R(ptree.get<double>("series_resistance")),
      C(ptree.get<double>("capacitance")),
      U_C(ptree.get<double>("initial_voltage", 0.0)), U(U_C), I(0.0),
      _comm(comm)
{
}

void SeriesRC::evolve_one_time_step_constant_load(double const delta_t,
                                                  double const load)
{
  U_C *= std::exp(-delta_t / ((R + load) * C));
  I = -U_C / (R + load);
  U = U_C + R * I;
}

void SeriesRC::evolve_one_time_step_constant_power(double const delta_t,
                                                   double const power)
{
  evolve_one_time_step_constant_power(delta_t, power, "NEWTON");
}

void SeriesRC::evolve_one_time_step_constant_current(double const delta_t,
                                                     double const current)
{
  U_C += current * delta_t / C;
  I = current;
  U = R * I + U_C;
}

void SeriesRC::evolve_one_time_step_linear_current(double const delta_t,
                                                   double const current)
{
  U_C += I * delta_t / C;
  U_C += (current - I) * 0.5 * delta_t / C;
  I = current;
  U = R * I + U_C;
}

void SeriesRC::evolve_one_time_step_constant_voltage(double const delta_t,
                                                     double const voltage)
{
  U_C -= (voltage - U_C) * std::expm1(-delta_t / (R * C));
  U = voltage;
  I = (U - U_C) / R;
}

void SeriesRC::evolve_one_time_step_linear_voltage(double const delta_t,
                                                   double const voltage)
{
  U_C -= (U - U_C) * std::expm1(-delta_t / (R * C));
  U_C += (voltage - U) / delta_t *
         (delta_t + R * C * std::expm1(-delta_t / (R * C)));
  U = voltage;
  I = (U - U_C) / R;
}

void SeriesRC::evolve_one_time_step_linear_power(double const delta_t,
                                                 double const power)
{
  std::ignore = delta_t;
  std::ignore = power;

  throw std::runtime_error("This function is not implemented.");
}

void SeriesRC::evolve_one_time_step_linear_load(double const delta_t,
                                                double const load)
{
  std::ignore = delta_t;
  std::ignore = load;

  throw std::runtime_error("This function is not implemented.");
}

std::size_t SeriesRC::evolve_one_time_step_constant_power(
    double const delta_t, double const power, std::string const &method)
{
  // TODO: if P is zero do constant current 0
  double const P = power;
  double const ATOL = 1.0e-14;
  double const RTOL = 1.0e-14;
  std::size_t const MAXIT = 30;
  double const TOL = std::abs(P) * RTOL + ATOL;
  std::size_t k = 0;
  while (true)
  {
    ++k;
    I = P / U;
    if (method.compare("FIXED_POINT") == 0)
      U = (R + delta_t / C) * I + U_C;
    else if (method.compare("NEWTON") == 0)
      U += ((R + delta_t / C) * P / U - U + U_C) /
           ((R + delta_t / C) * P / (U * U) + 1.0);
    else
      throw std::runtime_error("invalid method " + method);
    if (std::abs(P - U * I) < TOL)
      break;
    if (k >= MAXIT)
      throw std::runtime_error(method + " fail to converge within " +
                               std::to_string(MAXIT) + " iterations");
  }
  U_C += I * delta_t / C;
  return k;
}

void SeriesRC::save(const std::string &filename) const
{
  if (_comm.rank() == 0)
  {
    std::ofstream ofs(filename);
    boost::archive::text_oarchive oa(ofs);
    oa << *this;
  }
}

void SeriesRC::load(const std::string &filename)
{
  if (_comm.rank() == 0)
  {
    // Check that the file exist
    if (boost::filesystem::exists(filename) == false)
      throw std::runtime_error("The file " + filename + " does not exists.");

    std::ifstream ifs(filename);
    if (ifs.good() == false)
      throw std::runtime_error("Error while opening file " + filename);
    boost::archive::text_iarchive ia(ifs);
    ia >> *this;
  }
}

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

ParallelRC::ParallelRC(boost::property_tree::ptree const &ptree,
                       boost::mpi::communicator const &comm)
    : EnergyStorageDevice(comm),
      R_series(ptree.get<double>("series_resistance")),
      R_parallel(ptree.get<double>("parallel_resistance")),
      C(ptree.get<double>("capacitance")),
      U_C(ptree.get<double>("initial_voltage", 0.0)),
      U((R_series + R_parallel) / R_parallel * U_C),
      I(U / (R_series + R_parallel)), _comm(comm)
{
}

void ParallelRC::evolve_one_time_step_constant_current(double const delta_t,
                                                       double const current)
{
  U_C = R_parallel * current +
        (U_C - R_parallel * current) * std::exp(-delta_t / (R_parallel * C));
  I = current;
  U = R_series * I + U_C;
}

void ParallelRC::evolve_one_time_step_linear_current(double const delta_t,
                                                     double const current)
{
  U_C = R_parallel * I +
        (U_C - R_parallel * I) * std::exp(-delta_t / (R_parallel * C));
  U_C += R_parallel * (current - I) / delta_t *
         (delta_t + (R_parallel * C) * std::expm1(-delta_t / (R_parallel * C)));
  I = current;
  U = R_series * I + U_C;
}

void ParallelRC::evolve_one_time_step_constant_voltage(double const delta_t,
                                                       double const voltage)
{
  U_C -= (voltage * R_parallel / (R_series + R_parallel) - U_C) *
         std::expm1(-delta_t * (R_series + R_parallel) /
                    (R_series * R_parallel * C));
  U = voltage;
  I = (U - U_C) / R_series;
}

void ParallelRC::evolve_one_time_step_linear_voltage(double const delta_t,
                                                     double const voltage)
{
  U_C -= (U * R_parallel / (R_series + R_parallel) - U_C) *
         std::expm1(-delta_t * (R_series + R_parallel) /
                    (R_series * R_parallel * C));
  U_C += (voltage - U) / delta_t * R_parallel / (R_series + R_parallel) *
         (delta_t +
          (R_series * R_parallel * C) / (R_series + R_parallel) *
              std::expm1(-delta_t * (R_series + R_parallel) /
                         (R_series * R_parallel * C)));
  U = voltage;
  I = (U - U_C) / R_series;
}

void ParallelRC::evolve_one_time_step_linear_power(double const delta_t,
                                                   double const power)
{
  std::ignore = delta_t;
  std::ignore = power;

  throw std::runtime_error("This function is not implemented.");
}

void ParallelRC::evolve_one_time_step_linear_load(double const delta_t,
                                                  double const load)
{
  std::ignore = delta_t;
  std::ignore = load;

  throw std::runtime_error("This function is not implemented.");
}

void ParallelRC::evolve_one_time_step_constant_load(double const delta_t,
                                                    double const load)
{
  U_C *= std::exp(-delta_t * (1.0 + (R_series + load) / R_parallel) /
                  ((R_series + load) * C));
  I = -U_C / (R_series + load);
  U = U_C + R_series * I;
}

void ParallelRC::evolve_one_time_step_constant_power(double const delta_t,
                                                     double const power)
{
  evolve_one_time_step_constant_power(delta_t, power, "NEWTON");
}

std::size_t ParallelRC::evolve_one_time_step_constant_power(
    double const delta_t, double const power, std::string const &method)
{
  // TODO: if P is zero do constant current 0
  double const P = power;
  double const ATOL = 1.0e-14;
  double const RTOL = 1.0e-14;
  std::size_t const MAXIT = 30;
  double const TOL = std::abs(P) * RTOL + ATOL;
  std::size_t k = 0;
  while (true)
  {
    ++k;
    I = P / U;
    if (method.compare("FIXED_POINT") == 0)
      U = (R_series + R_parallel) * I +
          (U_C - R_parallel * I) * std::exp(-delta_t / (R_parallel * C));
    else if (method.compare("NEWTON") == 0)
      U += ((R_series +
             R_parallel * (1.0 - std::exp(-delta_t / (R_parallel * C)))) *
                P / U -
            U + U_C * std::exp(-delta_t / (R_parallel * C))) /
           ((R_series +
             R_parallel * (1.0 - std::exp(-delta_t / (R_parallel * C)))) *
                P / (U * U) +
            1.0);
    else
      throw std::runtime_error("invalid method " + method);
    if (std::abs(P - U * I) < TOL)
      break;
    if (k >= MAXIT)
      throw std::runtime_error(method + " fail to converge within " +
                               std::to_string(MAXIT) + " iterations");
  }
  U_C = U - R_series * I;
  return k;
}

void ParallelRC::save(const std::string &filename) const
{
  if (_comm.rank() == 0)
  {
    std::ofstream ofs(filename);
    boost::archive::text_oarchive oa(ofs);
    oa << *this;
  }
}

void ParallelRC::load(const std::string &filename)
{
  if (_comm.rank() == 0)
  {
    // Check that the file exist
    if (boost::filesystem::exists(filename) == false)
      throw std::runtime_error("The file " + filename + " does not exists.");

    std::ifstream ifs(filename);
    if (ifs.good() == false)
      throw std::runtime_error("Error while opening file " + filename);
    boost::archive::text_iarchive ia(ifs);
    ia >> *this;
  }
}

} // end namespace


//==============================================================================
//         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
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_BITINCREMENT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_CONSTANT_CONSTANTS_BITINCREMENT_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/sdk/meta/float.hpp>
#include <boost/simd/sdk/meta/int_c.hpp>
#include <boost/simd/sdk/meta/double.hpp>
#include <boost/simd/sdk/constant/constant.hpp>

/*!
 * \ingroup boost_simd_constant
 * \defgroup boost_simd_constant_bitincrement Bitincrement
 *
 * \par Description
 * Constant Bitincrement : represents 0x00000001 bitwise casts in the
 * the corresponding type.
 * \par
 * The value of this constant is type dependant. This means that for different
 * types it does not represent the same mathematical number.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/bitincrement.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::bitincrement_(A0)>::type
 *     Bitincrement();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Bitincrement
 *
 * \return type T value
 *
 *
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /*!
     * \brief Define the tag Bitincrement of functor Bitincrement
     *        in namespace boost::simd::tag for toolbox boost.simd.constant
    **/

    struct Bitincrement : ext::pure_constant_<Bitincrement>
    {
      template<class Target, class Dummy=void>
      struct apply : meta::int_c<typename Target::type,1> {};
    };

    template<class T, class Dummy>
    struct  Bitincrement::apply<boost::dispatch::meta::single_<T>,Dummy>
          : meta::single_<1> {};

    template<class T, class Dummy>
    struct  Bitincrement::apply<boost::dispatch::meta::double_<T>,Dummy>
          : meta::double_<1> {};
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION( boost::simd::tag::Bitincrement, Bitincrement)
} }

#include <boost/simd/sdk/constant/common.hpp>

#endif