#include <stdlib.h>

#include <boost/program_options.hpp>
#include <chrono>
#include <limits>

#include "../space/initial_triangulation.hpp"
#include "../time/basis.hpp"
#include "../tools/util.hpp"
#include "adaptive_heat_equation.hpp"
#include "problems.hpp"

using applications::AdaptiveHeatEquation;
using datastructures::DoubleTreeView;
using space::HierarchicalBasisFn;
using Time::OrthonormalWaveletFn;
using Time::ThreePointWaveletFn;

using namespace applications;
namespace po = boost::program_options;

namespace applications {
std::istream& operator>>(std::istream& in,
                         HeatEquationOptions::SpaceInverse& inverse_type) {
  std::string token;
  in >> token;
  if (token == "DirectInverse" || token == "di")
    inverse_type = HeatEquationOptions::SpaceInverse::DirectInverse;
  else if (token == "Multigrid" || token == "mg")
    inverse_type = HeatEquationOptions::SpaceInverse::Multigrid;
  else
    in.setstate(std::ios_base::failbit);
  return in;
}

void PrintTimeSliceSS(double t, AdaptiveHeatEquation::TypeXVector* solution) {
  auto time_slice = spacetime::Trace(t, *solution);

  // Calculate the triangulation corresponding to this space mesh.
  space::TriangulationView triang(time_slice.Bfs());
  std::cerr << "triang{";
  for (auto [elem, vertices] : triang.element_leaves())
    std::cerr << "(" << vertices[0] << ", " << vertices[1] << ", "
              << vertices[2] << ");";
  std::cerr << "}\t";

  // Calculate the single scale representation
  space::MassOperator op(triang);
  Eigen::VectorXd u_SS = time_slice.ToVector();
  assert(op.FeasibleVector(u_SS));
  op.ApplyHierarchToSingle(u_SS);
  assert(op.FeasibleVector(u_SS));

  // Print the data in single scale.
  time_slice.FromVector(u_SS);
  std::cerr << "vertices{";
  for (auto nv : time_slice.Bfs())
    std::cerr << "(" << nv->node()->center().first << ","
              << nv->node()->center().second << ") : " << nv->value() << ";";
  std::cerr << "}";
}

// Compile time constants.
constexpr size_t N_t = 20;
constexpr size_t N_x = 197;
constexpr size_t N_y = 199;

std::vector<std::tuple<float, float, float, double>> PrintSampling(
    AdaptiveHeatEquation::TypeXVector* solution) {
  int cnt[N_t + 1][N_x + 1][N_y + 1] = {0};
  double h_t = 1.0 / N_t;
  double h_x = 1.0 / N_x;
  double h_y = 1.0 / N_y;
  for (auto dblnode : solution->Bfs())
    for (int t = 0; t <= N_t; t++) {
      if (dblnode->node_0()->Eval(t * h_t) == 0) continue;
      for (int x = 0; x <= N_x; x++) {
        if (!dblnode->node_1()->Contains(x * h_x,
                                         dblnode->node_1()->center().second))
          continue;
        for (int y = 0; y <= N_y; y++) {
          if (dblnode->node_1()->Eval(x * h_x, y * h_y) == 0) continue;
          cnt[t][x][y]++;
        }
      }
    }

  std::vector<std::tuple<float, float, float, double>> result;
  for (int t = 0; t <= N_t; t++)
    for (int x = 0; x <= N_x; x++)
      for (int y = 0; y <= N_y; y++) {
        result.emplace_back(t * h_t, x * h_x, y * h_y, cnt[t][x][y]);
      }

  return result;
}

space::InitialTriangulation InitialTriangulation(std::string domain,
                                                 size_t initial_refines) {
  if (domain == "square" || domain == "unit-square")
    return space::InitialTriangulation::UnitSquare(initial_refines);
  else if (domain == "lshape" || domain == "l-shape")
    return space::InitialTriangulation::LShape(initial_refines);
  else if (domain == "pacman")
    return space::InitialTriangulation::Pacman(initial_refines);
  else {
    std::cout << "domain not recognized :-(" << std::endl;
    exit(1);
  }
}
}  // namespace applications

int main(int argc, char* argv[]) {
  std::string problem, domain;
  size_t initial_refines = 0;
  size_t max_dofs = 0;
  bool calculate_condition_numbers = false;
  bool print_centers = false;
  bool print_sampling = false;
  bool print_time_apply = false;
  std::vector<double> print_time_slices;
  boost::program_options::options_description problem_optdesc(
      "Problem options");
  problem_optdesc.add_options()(
      "problem", po::value<std::string>(&problem)->default_value("singular"))(
      "domain", po::value<std::string>(&domain)->default_value("square"))(
      "initial_refines", po::value<size_t>(&initial_refines))(
      "max_dofs", po::value<size_t>(&max_dofs)->default_value(
                      std::numeric_limits<std::size_t>::max()))(
      "calculate_condition_numbers",
      po::value<bool>(&calculate_condition_numbers))(
      "print_centers", po::value<bool>(&print_centers))(
      "print_sampling", po::value<bool>(&print_sampling))(
      "print_time_slices",
      po::value<std::vector<double>>(&print_time_slices)->multitoken())(
      "print_time_apply", po::value<bool>(&print_time_apply));

  std::sort(print_time_slices.begin(), print_time_slices.end());

  AdaptiveHeatEquationOptions adapt_opts;
  boost::program_options::options_description adapt_optdesc(
      "AdaptiveHeatEquation options");
  adapt_optdesc.add_options()("use_cache",
                              po::value<bool>(&adapt_opts.use_cache))(
      "build_space_mats", po::value<bool>(&adapt_opts.build_space_mats))(
      "solve_factor", po::value<double>(&adapt_opts.solve_factor))(
      "solve_xi", po::value<double>(&adapt_opts.solve_xi))(
      "solve_maxit", po::value<size_t>(&adapt_opts.solve_maxit))(
      "estimate_saturation_layers",
      po::value<size_t>(&adapt_opts.estimate_saturation_layers))(
      "estimate_mean_zero", po::value<bool>(&adapt_opts.estimate_mean_zero))(
      "mark_theta", po::value<double>(&adapt_opts.mark_theta))(
      "PX_alpha", po::value<double>(&adapt_opts.PX_alpha))(
      "PX_inv",
      po::value<HeatEquationOptions::SpaceInverse>(&adapt_opts.PX_inv))(
      "PY_inv",
      po::value<HeatEquationOptions::SpaceInverse>(&adapt_opts.PY_inv))(
      "PXY_mg_build", po::value<bool>(&adapt_opts.PXY_mg_build))(
      "PX_mg_cycles", po::value<size_t>(&adapt_opts.PX_mg_cycles))(
      "PY_mg_cycles", po::value<size_t>(&adapt_opts.PY_mg_cycles));
  boost::program_options::options_description cmdline_options;
  cmdline_options.add(problem_optdesc).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 << "Problem options:" << std::endl;
  std::cout << "\tProblem: " << problem << std::endl;
  std::cout << "\tDomain: " << domain
            << "; initial-refines: " << initial_refines << std::endl;
  std::cout << std::endl;
  std::cout << adapt_opts << std::endl;

  auto T = InitialTriangulation(domain, initial_refines);
  auto B = Time::Bases();

  T.hierarch_basis_tree.UniformRefine(1);
  B.ortho_tree.UniformRefine(1);
  B.three_point_tree.UniformRefine(1);

  auto vec_Xd = std::make_shared<
      DoubleTreeVector<ThreePointWaveletFn, HierarchicalBasisFn>>(
      B.three_point_tree.meta_root(), T.hierarch_basis_tree.meta_root());
  vec_Xd->SparseRefine(1);

  std::pair<std::unique_ptr<LinearFormBase<Time::OrthonormalWaveletFn>>,
            std::unique_ptr<LinearFormBase<Time::ThreePointWaveletFn>>>
      problem_data;
  if (problem == "smooth")
    problem_data = SmoothProblem();
  else if (problem == "singular")
    problem_data = SingularProblem();
  else if (problem == "cylinder")
    problem_data = CylinderProblem();
  else if (problem == "moving-peak")
    problem_data = MovingPeakProblem(vec_Xd);
  else {
    std::cout << "problem not recognized :-(" << std::endl;
    return 1;
  }

  AdaptiveHeatEquation heat_eq(vec_Xd, std::move(problem_data.first),
                               std::move(problem_data.second), adapt_opts);

  size_t ndof_Xd = 0;
  Eigen::VectorXd x0 = Eigen::VectorXd::Zero(vec_Xd->container().size());
  double t_delta = heat_eq.Estimate(x0).second.second.error;
  std::cout << "t_init: " << t_delta << std::endl;
  size_t iter = 0;
  auto start_algorithm = std::chrono::steady_clock::now();
  while (ndof_Xd < max_dofs) {
    // Store a vector of all the nodes having maximum gradedness;
    std::vector<typename HeatEquation::TypeXVector::DNType*> max_gradedness;

    // A slight overestimate.
    ndof_Xd = vec_Xd->Bfs().size();
    size_t ndof_Xdd = heat_eq.vec_Xdd()->Bfs().size();
    size_t ndof_Ydd = heat_eq.vec_Ydd()->Bfs().size();
    std::cout << "iter: " << ++iter << "\n\tXDelta-size: " << ndof_Xd
              << "\n\tXDelta-Gradedness: "
              << vec_Xd->Gradedness(&max_gradedness)
              << "\n\tXDeltaDelta-size: " << ndof_Xdd
              << "\n\tYDeltaDelta-size: " << ndof_Ydd
              << "\n\ttotal-memory-kB: " << getmem() << std::flush;

    if (print_sampling) {
      auto sampling = PrintSampling(vec_Xd.get());
      std::cout << "\n\tsampling: ";
      for (auto [t, x, y, val] : sampling)
        std::cout << "" << t << "," << x << "," << y << "," << val << ";";
      std::cout << std::endl;
    }

    if (calculate_condition_numbers) {
      auto start = std::chrono::steady_clock::now();
      std::chrono::duration<double> duration_cond =
          std::chrono::steady_clock::now() - start;

      // Set the initial vector to something valid.
      heat_eq.vec_Ydd()->Reset();
      for (auto nv : heat_eq.vec_Ydd()->Bfs())
        if (!nv->node_1()->on_domain_boundary()) nv->set_random();
      auto lanczos_Y = tools::linalg::Lanczos(
          *heat_eq.heat_d_dd()->A(), *heat_eq.heat_d_dd()->P_Y(),
          heat_eq.vec_Ydd()->ToVectorContainer());

      // Set the initial vector to something valid.
      heat_eq.vec_Xd()->Reset();
      for (auto nv : heat_eq.vec_Xd()->Bfs())
        if (!nv->node_1()->on_domain_boundary()) nv->set_random();
      auto lanczos_X = tools::linalg::Lanczos(
          *heat_eq.heat_d_dd()->S(), *heat_eq.heat_d_dd()->P_X(),
          heat_eq.vec_Xd()->ToVectorContainer());
      std::cout << "\n\tlmin-PY-A: " << lanczos_Y.min()
                << "\n\tlmax-PY-A: " << lanczos_Y.max()
                << "\n\tlmin-PX-S: " << lanczos_X.min()
                << "\n\tlmax-PX-S: " << lanczos_X.max()
                << "\n\tcond-time: " << duration_cond.count() << std::flush;
    }

    Eigen::VectorXd solution = x0;
    double total_error;
    AdaptiveHeatEquation::TypeXVector* residual;
    int cycle = 1;

    auto start = std::chrono::steady_clock::now();
    auto rhs = heat_eq.RHS();
    std::chrono::duration<double> duration_rhs =
        std::chrono::steady_clock::now() - start;
    std::cout << "\n\trhs-time: " << duration_rhs.count();
    std::cout << "\n\trhs-g-linform-time: "
              << heat_eq.g_lin_form()->TimeLastApply();
    std::cout << "\n\trhs-u0-linform-time: "
              << heat_eq.u0_lin_form()->TimeLastApply();

    auto start_solve_estimate = std::chrono::steady_clock::now();
    do {
      t_delta /= adapt_opts.solve_factor;
      std::cout << "\n\tcycle: " << cycle << "\n\t\tt_delta: " << t_delta;
      // Solve.
      start = std::chrono::steady_clock::now();
      auto [cur_solution, pcg_data] = heat_eq.Solve(solution, rhs, t_delta);
      solution = cur_solution;
      t_delta = pcg_data.algebraic_error;
      std::chrono::duration<double> duration_solve =
          std::chrono::steady_clock::now() - start;
      std::cout << "\n\t\tsolve-PCG-steps: " << pcg_data.iterations
                << "\n\t\tsolve-PCG-initial-algebraic-error: "
                << pcg_data.initial_algebraic_error
                << "\n\t\tsolve-PCG-algebraic-error: "
                << pcg_data.algebraic_error
                << "\n\t\tsolve-time: " << duration_solve.count()
                << "\n\t\tsolve-memory: " << getmem() << std::flush;

      // Estimate.
      start = std::chrono::steady_clock::now();
      auto [residual, global_errors] = heat_eq.Estimate(solution);
      auto [residual_norm, global_error] = global_errors;
      total_error = global_error.error;
      std::chrono::duration<double> duration_estimate =
          std::chrono::steady_clock::now() - start;

      std::cout << "\n\t\tresidual-norm: " << residual_norm
                << "\n\t\testimate-time: " << duration_estimate.count()
                << "\n\t\testimate-memory: " << getmem() << std::flush;
      std::cout << "\n\t\tglobal-error: " << total_error
                << "\n\t\tYnorm-error: " << global_error.error_Yprime
                << "\n\t\tT0-error: " << global_error.error_t0 << std::flush;
      cycle++;
    } while (t_delta > adapt_opts.solve_xi * total_error);
    t_delta = total_error;

    std::chrono::duration<double> duration_solve_estimate =
        std::chrono::steady_clock::now() - start_solve_estimate;
    std::cout << "\n\tsolve-estimate-time: " << duration_solve_estimate.count();

    if (print_time_apply) {
      auto heat_d_dd = heat_eq.heat_d_dd();
      std::cout << "\n\tA-time-per-apply: " << heat_d_dd->A()->TimePerApply()
                << "\n\tB-time-per-apply: " << heat_d_dd->B()->TimePerApply()
                << "\n\tBT-time-per-apply: " << heat_d_dd->BT()->TimePerApply()
                << "\n\tG-time-per-apply: " << heat_d_dd->G()->TimePerApply()
                << "\n\tP_Y-time-per-apply: "
                << heat_d_dd->P_Y()->TimePerApply()
                << "\n\tP_X-time-per-apply: "
                << heat_d_dd->P_X()->TimePerApply()
                << "\n\tS-time-per-apply: " << heat_d_dd->S()->TimePerApply()
                << "\n\ttotal-time-apply: " << heat_d_dd->TotalTimeApply()
                << "\n\ttotal-time-construct: "
                << heat_d_dd->TotalTimeConstruct() << std::flush;
    }

    if (print_centers) {
      vec_Xd->FromVectorContainer(solution);
      auto print_dblnode = [](auto dblnode) {
        std::cout << "((" << dblnode->node_0()->level() << ","
                  << dblnode->node_0()->center() << "),"
                  << "(" << dblnode->node_1()->level() << ",("
                  << dblnode->node_1()->center().first << ","
                  << dblnode->node_1()->center().second
                  << ")) : " << dblnode->value() << ";";
      };

      std::cout << "\n\tcenters: ";
      for (auto dblnode : vec_Xd->Bfs()) print_dblnode(dblnode);

      std::cout << "\n\tcenters-max-gradedness: ";
      for (auto dblnode : max_gradedness) print_dblnode(dblnode);
    }

    if (print_time_slices.size()) {
      vec_Xd->FromVectorContainer(solution);
      for (double t : print_time_slices) {
        assert(t >= 0 && t <= 1);
        std::cerr << "time_slice " << t << " = ";
        PrintTimeSliceSS(t, vec_Xd.get());
        std::cerr << std::endl;
      }
      std::cerr << std::endl;
    }

#ifdef VERBOSE
    std::cerr << std::endl << "Adaptive::Trees" << std::endl;
    std::cerr << "  T.vertex:   #bfs =  " << T.vertex_tree.Bfs().size()
              << std::endl;
    std::cerr << "  T.element:  #bfs =  " << T.elem_tree.Bfs().size()
              << std::endl;
    std::cerr << "  T.hierarch: #bfs =  " << T.hierarch_basis_tree.Bfs().size()
              << std::endl;
    std::cerr << std::endl;
    std::cerr << "  B.elem:     #bfs =  " << B.elem_tree.Bfs().size()
              << std::endl;
    std::cerr << "  B.three_pt: #bfs =  " << B.three_point_tree.Bfs().size()
              << std::endl;
    std::cerr << "  B.ortho:    #bfs =  " << B.ortho_tree.Bfs().size()
              << std::endl;
#endif

    // Mark - Refine.
    auto marked_nodes = heat_eq.Mark(residual);

    start = std::chrono::steady_clock::now();
    vec_Xd->FromVectorContainer(solution);
    auto r_info = heat_eq.Refine(marked_nodes);
    x0 = vec_Xd->ToVectorContainer();
    std::chrono::duration<double> duration_refine =
        std::chrono::steady_clock::now() - start;

    std::cout << "\n\tnodes-marked: " << r_info.nodes_marked
              << "\n\tnodes-conforming: " << r_info.nodes_conforming
              << "\n\tresidual-norm-marked: " << r_info.res_norm_marked
              << "\n\tresidual-norm-conforming: " << r_info.res_norm_conforming
              << "\n\trefine-time: " << duration_refine.count()
              << "\n\ttotal-time-algorithm: "
              << std::chrono::duration<double>(
                     std::chrono::steady_clock::now() - start_algorithm)
                     .count()
              << std::endl;
  }

  return 0;
}


#include "drake/systems/primitives/linear_system.h"

#include <string>
#include <utility>

#include <Eigen/Dense>
#include <Eigen/LU>
#include <fmt/format.h>

#include "drake/common/autodiff.h"
#include "drake/common/default_scalars.h"
#include "drake/common/eigen_types.h"
#include "drake/common/symbolic.h"
#include "drake/common/symbolic_decompose.h"
#include "drake/math/autodiff.h"
#include "drake/math/autodiff_gradient.h"
#include "drake/systems/framework/event.h"
#include "drake/systems/framework/event_collection.h"

namespace drake {
namespace systems {

using std::make_unique;
using std::unique_ptr;

template <typename T>
LinearSystem<T>::LinearSystem(const Eigen::Ref<const Eigen::MatrixXd>& A,
                              const Eigen::Ref<const Eigen::MatrixXd>& B,
                              const Eigen::Ref<const Eigen::MatrixXd>& C,
                              const Eigen::Ref<const Eigen::MatrixXd>& D,
                              double time_period)
    : LinearSystem<T>(SystemTypeTag<LinearSystem>{}, A, B, C, D,
                      time_period) {}

template <typename T>
template <typename U>
LinearSystem<T>::LinearSystem(const LinearSystem<U>& other)
    : LinearSystem<T>(other.A(), other.B(), other.C(), other.D(),
                      other.time_period()) {}

template <typename T>
LinearSystem<T>::LinearSystem(SystemScalarConverter converter,
                              const Eigen::Ref<const Eigen::MatrixXd>& A,
                              const Eigen::Ref<const Eigen::MatrixXd>& B,
                              const Eigen::Ref<const Eigen::MatrixXd>& C,
                              const Eigen::Ref<const Eigen::MatrixXd>& D,
                              double time_period)
    : AffineSystem<T>(std::move(converter), A, B,
                      Eigen::VectorXd::Zero(A.rows()), C, D,
                      Eigen::VectorXd::Zero(C.rows()), time_period) {}

template <typename T>
unique_ptr<LinearSystem<T>> LinearSystem<T>::MakeLinearSystem(
    const Eigen::Ref<const VectorX<symbolic::Expression>>& dynamics,
    const Eigen::Ref<const VectorX<symbolic::Expression>>& output,
    const Eigen::Ref<const VectorX<symbolic::Variable>>& state_vars,
    const Eigen::Ref<const VectorX<symbolic::Variable>>& input_vars,
    const double time_period) {
  // Need to extract, A, B, C, D such that,
  //
  //     dynamics = Ax + Bu
  //     output   = Cx + Du
  //
  // where x = state_vars and u = input_vars.
  const int num_states = state_vars.size();
  DRAKE_DEMAND(num_states == dynamics.size());
  const int num_inputs = input_vars.size();
  const int num_outputs = output.size();

  Eigen::MatrixXd AB(num_states, num_states + num_inputs);
  VectorX<symbolic::Variable> vars(num_states + num_inputs);
  vars << state_vars, input_vars;
  DecomposeLinearExpressions(dynamics, vars, &AB);
  const auto A = AB.leftCols(num_states);
  const auto B = AB.rightCols(num_inputs);

  Eigen::MatrixXd CD(num_outputs, num_states + num_inputs);
  DecomposeLinearExpressions(output, vars, &CD);
  const auto C = CD.leftCols(num_states);
  const auto D = CD.rightCols(num_inputs);

  return make_unique<LinearSystem<T>>(A, B, C, D, time_period);
}

namespace {

// Helper function allows reuse for both FirstOrderTaylorApproximation and
// Linearize.
std::unique_ptr<AffineSystem<double>> DoFirstOrderTaylorApproximation(
    const System<double>& system, const Context<double>& context,
    std::variant<InputPortSelection, InputPortIndex> input_port_index,
    std::variant<OutputPortSelection, OutputPortIndex> output_port_index,
    std::optional<double> equilibrium_check_tolerance = std::nullopt) {
  system.ValidateContext(context);

  double time_period = 0.0;
  const bool is_discrete_system =
      system.IsDifferenceEquationSystem(&time_period);
  DRAKE_THROW_UNLESS(context.is_stateless() ||
                     context.has_only_continuous_state() || is_discrete_system);

  // Create an autodiff version of the system.
  std::unique_ptr<System<AutoDiffXd>> autodiff_system =
      drake::systems::System<double>::ToAutoDiffXd(system);

  // Initialize autodiff.
  std::unique_ptr<Context<AutoDiffXd>> autodiff_context =
      autodiff_system->CreateDefaultContext();
  autodiff_context->SetTimeStateAndParametersFrom(context);
  autodiff_system->FixInputPortsFrom(system, context, autodiff_context.get());

  const InputPort<AutoDiffXd>* input_port =
      autodiff_system->get_input_port_selection(input_port_index);
  const OutputPort<AutoDiffXd>* output_port =
      autodiff_system->get_output_port_selection(output_port_index);

  // Verify that the input port is not abstract valued.
  if (input_port &&
      input_port->get_data_type() == PortDataType::kAbstractValued) {
    throw std::logic_error(
        "Port requested for differentiation is abstract, and differentiation "
        "of abstract ports is not supported.");
  }

  const int num_inputs = input_port ? input_port->size() : 0;
  const int num_outputs = output_port ? output_port->size() : 0;

  const Eigen::VectorXd x0 =
      context.is_stateless()
          ? Eigen::VectorXd::Zero(0)
          : ((context.has_only_continuous_state())
                 ? context.get_continuous_state_vector().CopyToVector()
                 : context.get_discrete_state(0).get_value());
  const int num_states = x0.size();

  Eigen::VectorXd u0 = Eigen::VectorXd::Zero(num_inputs);
  if (input_port) {
    u0 = system.get_input_port(input_port->get_index()).Eval(context);
  }

  auto autodiff_args = math::initializeAutoDiffTuple(x0, u0);
  if (input_port) {
    VectorX<AutoDiffXd> input_vector = std::get<1>(autodiff_args);
    input_port->FixValue(autodiff_context.get(), input_vector);
  }

  Eigen::MatrixXd A(num_states, num_states), B(num_states, num_inputs);
  Eigen::VectorXd f0(num_states);
  if (num_states > 0) {
    if (autodiff_context->has_only_continuous_state()) {
      autodiff_context->get_mutable_continuous_state_vector().SetFromVector(
          std::get<0>(autodiff_args));
      std::unique_ptr<ContinuousState<AutoDiffXd>> autodiff_xdot =
          autodiff_system->AllocateTimeDerivatives();
      autodiff_system->CalcTimeDerivatives(*autodiff_context,
                                           autodiff_xdot.get());
      auto autodiff_xdot_vec = autodiff_xdot->CopyToVector();

      const Eigen::MatrixXd AB =
          math::autoDiffToGradientMatrix(autodiff_xdot_vec);
      A = AB.leftCols(num_states);
      B = AB.rightCols(num_inputs);

      const Eigen::VectorXd xdot0 =
          math::autoDiffToValueMatrix(autodiff_xdot_vec);

      if (equilibrium_check_tolerance &&
          !xdot0.isZero(*equilibrium_check_tolerance)) {
        throw std::runtime_error(
            "The nominal operating point (x0,u0) is not an equilibrium point "
            "of "
            "the system.  Without additional information, a time-invariant "
            "linearization of this system is not well defined.");
      }

      f0 = xdot0 - A * x0 - B * u0;
    } else {
      DRAKE_ASSERT(is_discrete_system);
      auto& autodiff_x0 =
          autodiff_context->get_mutable_discrete_state().get_mutable_vector();
      autodiff_x0.SetFromVector(std::get<0>(autodiff_args));
      std::unique_ptr<DiscreteValues<AutoDiffXd>> autodiff_x1 =
          autodiff_system->AllocateDiscreteVariables();
      autodiff_system->CalcDiscreteVariableUpdates(*autodiff_context,
                                                   autodiff_x1.get());
      auto autodiff_x1_vec = autodiff_x1->get_vector().CopyToVector();

      const Eigen::MatrixXd AB =
          math::autoDiffToGradientMatrix(autodiff_x1_vec);
      A = AB.leftCols(num_states);
      B = AB.rightCols(num_inputs);

      const Eigen::VectorXd x1 = math::autoDiffToValueMatrix(autodiff_x1_vec);

      if (equilibrium_check_tolerance &&
          !(x1 - x0).isZero(*equilibrium_check_tolerance)) {
        throw std::runtime_error(
            "The nominal operating point (x0,u0) is not an equilibrium point "
            "of the system.  Without additional information, a time-invariant "
            "linearization of this system is not well defined.");
      }

      f0 = x1 - A * x0 - B * u0;
    }
  } else {
    DRAKE_ASSERT(num_states == 0);
    A = Eigen::MatrixXd(0, 0);
    B = Eigen::MatrixXd(0, num_inputs);
    f0 = Eigen::VectorXd(0);
  }

  Eigen::MatrixXd C = Eigen::MatrixXd::Zero(num_outputs, num_states);
  Eigen::MatrixXd D = Eigen::MatrixXd::Zero(num_outputs, num_inputs);
  Eigen::VectorXd y0 = Eigen::VectorXd::Zero(num_outputs);

  if (output_port) {
    const auto& autodiff_y0 = output_port->Eval(*autodiff_context);
    const Eigen::MatrixXd CD = math::autoDiffToGradientMatrix(autodiff_y0);
    C = CD.leftCols(num_states);
    D = CD.rightCols(num_inputs);

    const Eigen::VectorXd y = math::autoDiffToValueMatrix(autodiff_y0);

    // Note: No tolerance check needed here.  We have defined that the output
    // for the system produced by Linearize is in the coordinates (y-y0).

    y0 = y - C * x0 - D * u0;
  }

  return std::make_unique<AffineSystem<double>>(A, B, f0, C, D, y0,
                                                time_period);
}

}  // namespace

std::unique_ptr<LinearSystem<double>> Linearize(
    const System<double>& system, const Context<double>& context,
    std::variant<InputPortSelection, InputPortIndex> input_port_index,
    std::variant<OutputPortSelection, OutputPortIndex> output_port_index,
    double equilibrium_check_tolerance) {
  std::unique_ptr<AffineSystem<double>> affine =
      DoFirstOrderTaylorApproximation(
          system, context, std::move(input_port_index),
          std::move(output_port_index), equilibrium_check_tolerance);

  return std::make_unique<LinearSystem<double>>(affine->A(), affine->B(),
                                                affine->C(), affine->D(),
                                                affine->time_period());
}

std::unique_ptr<AffineSystem<double>> FirstOrderTaylorApproximation(
    const System<double>& system, const Context<double>& context,
    std::variant<InputPortSelection, InputPortIndex> input_port_index,
    std::variant<OutputPortSelection, OutputPortIndex> output_port_index) {
  return DoFirstOrderTaylorApproximation(system, context,
                                         std::move(input_port_index),
                                         std::move(output_port_index));
}

/// Returns the controllability matrix:  R = [B, AB, ..., A^{n-1}B].
Eigen::MatrixXd ControllabilityMatrix(const LinearSystem<double>& sys) {
  DRAKE_DEMAND(sys.time_period() == 0.0);
  // TODO(russt): handle the discrete time case

  const int num_states = sys.B().rows(), num_inputs = sys.B().cols();
  Eigen::MatrixXd R(num_states, num_states * num_inputs);
  R.leftCols(num_inputs) = sys.B();
  for (int i = 1; i < num_states; i++) {
    R.middleCols(num_inputs * i, num_inputs) =
        sys.A() * R.middleCols(num_inputs * (i - 1), num_inputs);
  }
  return R;
}

/// Returns true iff the controllability matrix is full row rank.
bool IsControllable(const LinearSystem<double>& sys,
                    std::optional<double> threshold) {
  const auto R = ControllabilityMatrix(sys);
  Eigen::ColPivHouseholderQR<Eigen::MatrixXd> lu_decomp(R);
  if (threshold) {
    lu_decomp.setThreshold(threshold.value());
  }
  return lu_decomp.rank() == sys.A().rows();
}

/// Returns the observability matrix: O = [ C; CA; ...; CA^{n-1} ].
Eigen::MatrixXd ObservabilityMatrix(const LinearSystem<double>& sys) {
  DRAKE_DEMAND(sys.time_period() == 0.0);
  // TODO(russt): handle the discrete time case

  const int num_states = sys.C().cols(), num_outputs = sys.C().rows();
  Eigen::MatrixXd O(num_states * num_outputs, num_states);
  O.topRows(num_outputs) = sys.C();
  for (int i = 1; i < num_states; i++) {
    O.middleRows(num_outputs * i, num_outputs) =
        O.middleRows(num_outputs * (i - 1), num_outputs) * sys.A();
  }
  return O;
}

/// Returns true iff the observability matrix is full column rank.
bool IsObservable(const LinearSystem<double>& sys,
                  std::optional<double> threshold) {
  const auto O = ObservabilityMatrix(sys);
  Eigen::ColPivHouseholderQR<Eigen::MatrixXd> lu_decomp(O);
  if (threshold) {
    lu_decomp.setThreshold(threshold.value());
  }
  return lu_decomp.rank() == sys.A().rows();
}

}  // namespace systems
}  // namespace drake

DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
    class ::drake::systems::LinearSystem)

DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
    class ::drake::systems::TimeVaryingLinearSystem)


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

#include <boost/xpressive/xpressive_static.hpp>
#include <boost/tuple/tuple.hpp>

using namespace boost::xpressive;

typedef boost::tuple<std::string, std::string, std::string> Vertex;
typedef boost::tuple<int, int, int> Face;
typedef std::vector<Vertex> Vertices;
typedef std::vector<Face> Faces;
Vertices vertices;
Faces faces;

int usage(const char* argv0, bool failure = true)
{
  std::cerr << "Usage:\n"
            << "  " << argv0 << " INPUT.xyz OUTPUT.off\n";
  if(failure)
    return EXIT_FAILURE;
  else
    return EXIT_SUCCESS;
}

int incorrect_input(std::string error = "")
{
  std::cerr << "Incorrect input file.\n"
            << "  " << error << "\n";
  std::cerr << "So far: " << vertices.size() << " vertices, "
            << faces.size() << " faces\n";
  return EXIT_FAILURE;
}

int main(int argc, char** argv)
{
  if(argc != 3)
    return usage(argv[0]);
  mark_tag vertex_index(1), vertex_x(2), vertex_y(3), vertex_z(4);

  sregex tface_re = bos >> *space >> "TFACE" >> *space >> eos;

  sregex vertex_re = bos >> *space >> "VRTX"
                         >> +space >> (vertex_index=+_d)
                         >> +space >> (vertex_x=+(digit|'-'|'+'|'.'))
                         >> +space >> (vertex_y=+(digit|'-'|'+'|'.'))
                         >> +space >> (vertex_z=+(digit|'-'|'+'|'.'))
                         >> eos;

  sregex triangle_re = bos >> *space >> "TRGL"
                           >> +space >> (s1=+digit)
                           >> +space >> (s2=+digit)
                           >> +space >> (s3=+digit)
                           >> eos;
  sregex end_re = bos >> *space >> "END" >> *space >> eos;

  std::ifstream input(argv[1]);
  std::ofstream output(argv[2]);

  if(!input) {
    std::cerr << "Cannot read \"" << argv[1] << "\"!\n";
    return EXIT_FAILURE;
  }
  if(!output) {
    std::cerr << "Cannot write to \"" << argv[2] << "\"!\n";
    return EXIT_FAILURE;
  }

  std::string line;
  std::getline(input, line);
  smatch results;
  while(input && ! regex_match(line, tface_re)) // search line "TFACE"
  {
    std::getline(input, line);
  }
  std::getline(input, line);
  while(input && regex_match(line, results, vertex_re)) {
    vertices.push_back(boost::make_tuple(results[vertex_x],
                                         results[vertex_y],
                                         results[vertex_z]));
    std::getline(input, line);
  }
  while(input && regex_match(line, results, triangle_re)) {
    std::stringstream s;
    int i, j, k;
    s << results[1] << " " << results[2] << " " << results[3];
    s >> i >> j >> k;
    faces.push_back(boost::make_tuple(i, j, k));
    std::getline(input, line);
  }
  if(!input || !regex_match(line, end_re))
    return incorrect_input("premature end of file!");

  output << "OFF " << vertices.size() << " " << faces.size() << " " << "0\n";
  for(Vertices::const_iterator vit = vertices.begin(), vend = vertices.end();
      vit != vend; ++vit)
    output << boost::get<0>(*vit) << " "
           << boost::get<1>(*vit) << " "
           << boost::get<2>(*vit) << "\n";
  for(Faces::const_iterator fit = faces.begin(), fend = faces.end();
      fit != fend; ++fit)
    output << "3 " << boost::get<0>(*fit)
           << " " << boost::get<1>(*fit)
           << " " << boost::get<2>(*fit) << "\n";
  if(output)
    return EXIT_SUCCESS;
  else
    return EXIT_FAILURE;
};


/***
 *  $Id$
 **
 *  File: performance_indicators_diss.hpp
 *  Created: May 9, 2012
 *
 *  Author: Olga Wodo, Baskar Ganapathysubramanian
 *  Copyright (c) 2012 Olga Wodo, Baskar Ganapathysubramanian
 *  See accompanying LICENSE.
 *
 *  This file is part of GraSPI.
 */

#ifndef PERFORMANCE_INDICATORS_DISS_HPP
#define PERFORMANCE_INDICATORS_DISS_HPP

#include <climits>
#include <sstream>

#include "graspi_types.hpp"
#include "graph_dijkstra.hpp"
#include "graspi_predicates.hpp"
#include <boost/graph/filtered_graph.hpp>

namespace graspi {

  struct foo_w_diss{
      double A1,B1,C1;
      foo_w_diss(){ A1=6.265; B1=-23.0; C1=17.17; }
      double operator()(double d)const{
	  return A1*exp(-((d-B1)/C1)*((d-B1)/C1));
      }
  };

  inline int
  identify_n_vertices_within_distance( const std::vector<float>& d,
				       double Ld){
      int n_Ld = 0;
      for(unsigned int i = 0; i < d.size(); i++){
	  if( (d[i] < Ld) && (d[i] > 0) ) n_Ld++;
      }
      return n_Ld;
  }

  inline double
  identify_weighted_vertices_within_distance( const std::vector<float>& d,
					      double Ld){
      double wn_Ld = 0;
      foo_w_diss wfoo;

      for(unsigned int i = 0; i < d.size(); i++){
	  double d_i = d[i];
	  double wd_i = wfoo(d_i);
	  if( (d_i < Ld) && (d_i > 0) ) wn_Ld+= wd_i;
      }
      return wn_Ld;
  }

  inline std::pair<int,double>
  identify_n_weighted_vertices_within_distance( const std::vector<float>& d,
						double Ld){
      double wn_Ld = 0;
      int n_Ld = 0;
      foo_w_diss wfoo;

      for(unsigned int i = 0; i < d.size(); i++){
	  double d_i = d[i];
	  double wd_i = wfoo(d_i);
	  if( (d_i < Ld) && (d_i > 0) ){
	      wn_Ld+= wd_i;
	      n_Ld++;
	  }
      }
      return std::pair<int,double>(n_Ld,wn_Ld);
  }

  inline std::pair<double,double>
  wf_diss(
	  graph_t* G, const dim_g_t& d_g, const vertex_colors_t& C,
	  const edge_weights_t& W, const vertex_ccs_t& vCC,
	  const ccs_t& CC,
	  double Ld,
	  const std::string& filename_ColorToGreen,
	  const std::string& filename_WColorToGreen,
	  COLOR color = BLACK,
	  COLOR green = GREEN
	  ){
      int n_color = 0;
      int n_color_Ld = 0;
      double wn_color_Ld = 0;

      connect_color_green pred(*G,C,color,green);
      unsigned int n = boost::num_vertices(*G);
      vertex_t int_id = d_g.id(green);
      std::vector<float> d(n);

      determine_shortest_distances( G, W, int_id, pred, d);

      foo_w_diss wfoo;
      std::ostringstream oss_out_d;
      std::ostringstream oss_out_wd;
      for (unsigned int i = 0; i < d.size(); i++) {
	  unsigned int c = C[i];
	  if (c == color) n_color++;
	  if ( ( c == color )
	       && ( fabs(d[i]) < std::numeric_limits<float>::max() )
	       ) {
	      double d_i = d[i];
	      oss_out_d  << d_i       << std::endl;
	      oss_out_wd << d_i << " " << wfoo(d_i) << std::endl;
	  }
      }
      std::ofstream f_out(filename_ColorToGreen.c_str());
      std::string buffer = oss_out_d.str();
      int size = oss_out_d.str().size();
      f_out.write (buffer.c_str(),size);
      f_out.close();
      f_out.open(filename_WColorToGreen.c_str());
      buffer = oss_out_wd.str();
      size = oss_out_wd.str().size();
      f_out.write (buffer.c_str(),size);
      f_out.close();

      std::pair<int,double> pLd
	  = identify_n_weighted_vertices_within_distance(d,Ld);
      n_color_Ld = pLd.first;
      wn_color_Ld = pLd.second;

#ifdef DEBUG
      std::cout << "[DEBUG] Number of " << color << "vertices: "
		<< n_color << std::endl
		<< "[DEBUG] Number of " << color << "vertices in "
		<< Ld << " distance to green: "
		<< n_color_Ld << std::endl;
#endif

      return std::pair<double, double>(
				       (double)wn_color_Ld/n_color,
				       (double)n_color_Ld/n_color
				       );
  }

  inline std::pair<double,double>
  wf_diss(
	  graph_t* G, const dim_g_t& d_g, const vertex_colors_t& C,
	  const edge_weights_t& W, const vertex_ccs_t& vCC,
	  const ccs_t& CC,
	  double Ld,
	  COLOR color = BLACK,
	  COLOR green = GREEN
	  ){
      int n_color = 0;
      int n_color_Ld = 0;
      double wn_color_Ld = 0;

      connect_color_green pred(*G,C,color,green);
      unsigned int n = boost::num_vertices(*G);
      vertex_t int_id = d_g.id(green);
      std::vector<float> d(n);

      determine_shortest_distances( G, W, int_id, pred, d);

      foo_w_diss wfoo;
      for (unsigned int i = 0; i < d.size(); i++) {
	  unsigned int c = C[i];
	  if (c == color) n_color++;
      }

      std::pair<int,double> pLd
	  = identify_n_weighted_vertices_within_distance(d,Ld);
      n_color_Ld = pLd.first;
      wn_color_Ld = pLd.second;

      return std::pair<double, double>(
				       (double)wn_color_Ld/n_color,
				       (double)n_color_Ld/n_color
				       );
  }

}
#endif


#ifndef BOOST_SYSTEM_NO_DEPRECATED
#define BOOST_SYSTEM_NO_DEPRECATED 1
#endif

#include "Python.h"
#include <boost/intrusive/options.hpp>
#include <boost/python.hpp>
#include <boost/python/object.hpp>
#include <boost/python/object/function_object.hpp>
#include <boost/python/object/py_function.hpp>
#include <boost/python/call_method.hpp>
#include <boost/python/numeric.hpp>
#include <boost/python/extract.hpp>
#include <boost/python/register_ptr_to_python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <armadillo>
#include <utility>
#include "numpy/ndarrayobject.h"
#include "include/carmcmc.hpp"
#include "include/carpack.hpp"
#include "include/kfilter.hpp"

using namespace boost::python;

BOOST_PYTHON_FUNCTION_OVERLOADS(car1Overloads, RunCar1Sampler, 5, 7);
BOOST_PYTHON_FUNCTION_OVERLOADS(carmaOverloads, RunCarmaSampler, 8, 11);

BOOST_PYTHON_MODULE(_carmcmc){
    import_array();
    numeric::array::set_module_and_type("numpy", "ndarray");

    class_<std::vector<double> >("vecD")
        .def(vector_indexing_suite<std::vector<double> >());

    class_<std::vector<std::vector<double > > >("vecvecD")
        .def(vector_indexing_suite<std::vector<std::vector<double> > >());

    class_<std::vector<std::complex<double> > >("vecC")
        .def(vector_indexing_suite<std::vector<std::complex<double> > >());

    class_<std::pair<double, double> >("pairD")
        .def_readwrite("first", &std::pair<double, double>::first)
        .def_readwrite("second", &std::pair<double, double>::second);

    // carpack.hpp
    class_<CARMA_Base<double>, boost::noncopyable>("CARMA_Base_double", no_init);
    class_<CARMA_Base<arma::vec>, boost::noncopyable>("CARMA_Base_arma", no_init);

    class_<CAR1, bases<CARMA_Base<double> >, std::shared_ptr<CAR1> >("CAR1", no_init)
        .def(init<bool,std::string,std::vector<double>,std::vector<double>,std::vector<double>,optional<double> >())
        .def("getLogPrior", &CAR1::getLogPrior)
        .def("getLogDensity", &CAR1::getLogDensity)
        .def("getSamples", &CAR1::getSamples)
        .def("GetLogLikes", &CAR1::GetLogLikes)  // Base class parameters.hpp
    ;

    class_<CARp, bases<CARMA_Base<arma::vec> >, std::shared_ptr<CARp> >("CARp", no_init)
        .def(init<bool,std::string,std::vector<double>,std::vector<double>,std::vector<double>,int,optional<double> >())
        .def("getLogPrior", &CARp::getLogPrior)
        .def("getLogDensity", &CARp::getLogDensity)
        .def("getSamples", &CARp::getSamples)
        .def("GetLogLikes", &CARp::GetLogLikes)
        .def("SetMLE", &CARp::SetMLE)
    ;

    class_<CARMA, bases<CARp>, std::shared_ptr<CARMA> >("CARMA", no_init)
        .def(init<bool,std::string,std::vector<double>,std::vector<double>,std::vector<double>,int,int,optional<double> >())
        .def("getLogPrior", &CARMA::getLogPrior)
        .def("getLogDensity", &CARMA::getLogDensity)
        .def("getSamples", &CARMA::getSamples)
        .def("GetLogLikes", &CARMA::GetLogLikes)
        .def("SetMLE", &CARMA::SetMLE)
    ;

    // carmcmc.hpp
    def("run_mcmc_car1", RunCar1Sampler, car1Overloads());
    def("run_mcmc_carma", RunCarmaSampler, carmaOverloads());

    // kfilter.hpp
    class_<KalmanFilter<double>, boost::noncopyable>("KalmanFilter_double", no_init);
    class_<KalmanFilter<arma::cx_vec>, boost::noncopyable>("KalmanFilter_arma", no_init);

    class_<KalmanFilter1, bases<KalmanFilter<double> >, std::shared_ptr<KalmanFilter1> >("KalmanFilter1", no_init)
        .def(init<std::vector<double>,std::vector<double>,std::vector<double> >())
        .def(init<std::vector<double>,std::vector<double>,std::vector<double>,double,double>())
        .def("Simulate", &KalmanFilter1::Simulate)
        .def("Filter", &KalmanFilter1::Filter)
        .def("Predict", &KalmanFilter1::Predict)
        .def("GetMean", &KalmanFilter1::GetMeanSvec)
        .def("GetVar", &KalmanFilter1::GetVarSvec)
    ;
    class_<KalmanFilterp, bases<KalmanFilter<arma::cx_vec> >, std::shared_ptr<KalmanFilterp> >("KalmanFilterp", no_init)
        .def(init<std::vector<double>,std::vector<double>,std::vector<double> >())
        .def(init<std::vector<double>,std::vector<double>,std::vector<double>,double,
             std::vector<std::complex<double> >,std::vector<double> >())
        .def("Simulate", &KalmanFilterp::Simulate)
        .def("Filter", &KalmanFilterp::Filter)
        .def("Predict", &KalmanFilterp::Predict)
        .def("GetMean", &KalmanFilterp::GetMeanSvec)
        .def("GetVar", &KalmanFilterp::GetVarSvec)
    ;
};


// Copyright 2015 Alexandre Vaillancourt
//
// 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 <unordered_map>
#include <vector>
#include <iostream>
#include <ctime>

#include <boost/random.hpp>

typedef std::unordered_map<unsigned int, unsigned int> Echelles;
typedef std::unordered_map<unsigned int, unsigned int> Serpents;

typedef boost::random::uniform_int_distribution<> RndGen;

boost::random::mt19937 rng;

const Echelles
getEchelles()
{
  Echelles echelles;
  echelles[2]  = 18;
  echelles[8]  = 50;
  echelles[10] = 30;
  echelles[26] = 74;
  echelles[40] = 60;
  echelles[44] = 64;
  echelles[52] = 60;
  echelles[68] = 94;
  echelles[78] = 100;
  echelles[84] = 96;

  return echelles;
}

const Serpents
getSerpents()
{
  Serpents serpents;
  serpents[16] = 6;
  serpents[22] = 20;
  serpents[32] = 7;
  serpents[48] = 28;
  serpents[58] = 24;
  serpents[76] = 56;
  serpents[80] = 42;
  serpents[86] = 36;
  serpents[92] = 72;
  serpents[98] = 62;

  return serpents;
}

static const unsigned int DICE_SIZE = 10u;
static const unsigned int DICE_STREAK_MAX = 10u;
static const unsigned int END_MISS_STREAK_MAX = 20u;
static const unsigned int RUNS = 1000000u;
#define VERBOSE 0
#define VERBOSE_STATS 1

RndGen
getAGen()
{
  return RndGen(1,DICE_SIZE);
}

int main(void)
{

  RndGen gen = getAGen();
  boost::random::mt19937 engine(time(0));
  const Echelles echelles = getEchelles();
  const Serpents serpents = getSerpents();

  std::vector<unsigned int> laddersHits(100u, 0u);
  std::vector<unsigned int> snakesHits(100u, 0u);
  std::vector<unsigned int> diceHits( DICE_SIZE + DICE_STREAK_MAX, 0u );
  std::vector<unsigned int> endMissStreaks( END_MISS_STREAK_MAX, 0u );
  unsigned int diceRollCount = 0u;
  unsigned int turnCount = 0u;

  for ( unsigned int runIdx = 0u; runIdx < RUNS; ++runIdx )
  {

    unsigned int endMissStreak = 0u;

    unsigned int currentPos = 0;
    while(currentPos != 100u)
    {
#if VERBOSE
      std::cout << "starting turn at " << currentPos << std::endl;
#endif

      const unsigned int initialPosition = currentPos;
      int diceRoll = gen(engine);
      ++diceRollCount;
#if VERBOSE
      std::cout << "rolled " << diceRoll << std::endl;
#endif
      unsigned int maxValueStreak = 0u;
      while( DICE_SIZE == diceRoll )
      {
        ++maxValueStreak;
        currentPos += diceRoll;
        diceRoll = gen(engine);
        ++diceRollCount;
#if VERBOSE
      std::cout << DICE_SIZE << ", rerolled " << diceRoll << std::endl;
#endif
      }

      currentPos += diceRoll;
      diceHits[diceRoll - 1u] += 1;
      if ( 0u != maxValueStreak )
      {
        maxValueStreak = std::min( maxValueStreak, DICE_STREAK_MAX );
        diceHits[DICE_SIZE - 2 + maxValueStreak] += 1; // Store the values of the streak; max dice value stores the single occurence, max dice + 1 stores the 2 occurences in a row, etc..
      }
#if VERBOSE
      std::cout << "currently at " << currentPos << std::endl;
#endif

      const auto echelle = echelles.find(currentPos);
      const auto serpent = serpents.find(currentPos);

      if(echelle != echelles.end())
      {
        currentPos = echelle->second;
        laddersHits[echelle->first] += 1u;
#if VERBOSE
      std::cout << "ladder(" << echelle->first << "), going up at " << currentPos << std::endl;
#endif
      }
      else if (serpent != serpents.end())
      {
        currentPos = serpent->second;
        snakesHits[serpent->first] += 1u;
#if VERBOSE
      std::cout << "snake(" << serpent->first << "), going down at " << currentPos << std::endl;
#endif
      }

      if ( currentPos > 100u )
      {
        currentPos = initialPosition;
        ++endMissStreak;
#if VERBOSE
      std::cout << "went over 100, going back down to " << currentPos << std::endl;
#endif
      }
      else
      {
        if ( endMissStreak != 0u )
        {
          endMissStreak = std::min( endMissStreak, END_MISS_STREAK_MAX );
          endMissStreaks[endMissStreak - 1u] += 1u;
          endMissStreak = 0u;
        }
      }
      ++turnCount;
    }

    if ( runIdx % 1000u == 0u)
      std::cout << runIdx << std::endl;
  }
#if VERBOSE
  std::cout << "game end; turns (" << turnCount << ") diceRolls (" << diceRollCount << ")" << std::endl;
#endif

#if VERBOSE_STATS
  std::ostringstream ossLadders;
  std::ostringstream ossSnakes;
  std::ostringstream ossDiceHits;
  std::ostringstream ossEndMissStreaks;

  const double dRUNS = static_cast<double>( RUNS );

  for ( unsigned int i = 0u; i < 100u; ++i )
  {
    const auto echelle = echelles.find(i);
    const auto serpent = serpents.find(i);
    if(echelle != echelles.end())
    {
      ossLadders << "lad" << (i + 1u) << " " << laddersHits[i] << " " << static_cast<double>(laddersHits[i]) / dRUNS << std::endl;
    }
    else if ( serpent != serpents.end() )
    {
      ossSnakes << "sna" << (i + 1u) << " " << snakesHits[i] << " " << static_cast<double>(snakesHits[i]) / dRUNS << std::endl;
    }
  }
  for ( unsigned int i = 0u; i < (DICE_SIZE + DICE_STREAK_MAX - 1); ++i )
  {
    if ( i < (DICE_SIZE - 1u) )
    {
      ossDiceHits << "d" << DICE_SIZE << "|" << (i + 1u) << " " << diceHits[i] << " " << static_cast<double>(diceHits[i]) / dRUNS << std::endl;
    }
    else
    {
      ossDiceHits << "d" << DICE_SIZE << "|" << DICE_SIZE << "x" << (i - DICE_SIZE + 2u) << " " << diceHits[i] << " " << static_cast<double>(diceHits[i]) / dRUNS << std::endl;
    }
  }

  for ( unsigned int i = 0u; i < END_MISS_STREAK_MAX; ++i )
  {
    ossEndMissStreaks << "endMissStreak" << ( i + 1u ) << " " << endMissStreaks[i] << " " << static_cast<double>(endMissStreaks[i]) / dRUNS << std::endl;
  }

  std::cout <<
    "runs " << RUNS << std::endl <<
    ossLadders.str() <<
    ossSnakes.str() <<
    ossDiceHits.str() <<
    ossEndMissStreaks.str() <<
    "turns " << turnCount << " " << static_cast<double>(turnCount) / dRUNS << std::endl <<
    "diceRolls " << diceRollCount << " " << static_cast<double>(diceRollCount) / dRUNS <<
    std::endl;
#endif

    return 0;
}


#include <SimilarityGraphImpl.h>

#include <ImageUtil.h>
#include <GraphUtils.h>
#include <SimilarityGraphVisualizationStrategy.h>

#include <boost/graph/graph_utility.hpp>

#include <numeric>
#include <tuple>

namespace
{
/*
    Composes the given functions into a single
    function that returns the sum of their return values

    @param funcA A function that returns a valid operand of operator +
    @param funcA A function that returns a valid operand of operator +
    @returns A new function that sums the given functions and returns
             a valid operand of operator +
*/
template<typename FuncA, typename FuncB>
auto recursiveSum(FuncA funcA, FuncB funcB)
{
    return [=](const auto& edge)
    {
        return funcA(edge) + funcB(edge);
    };
}
}

namespace dpa::graph::internal
{
bool SimilarityGraphImpl::build(const di::Image<di::RGB, stbi_uc>& image)
{
    // Graph may be built from a loaded or programatically created image
    if (!image.getHeight() || !image.getWidth())
        return false;

    if (auto convertedImage = di::utility::RGB_To_YCbCr(image); convertedImage)
    {
        // Create the adjacency matrix with the correct number of vertices
        m_graph = Graph(convertedImage.value().getWidth() * convertedImage.value().getHeight());

        // Set the pixel colors on each node
        if (!setNodeProperties(convertedImage.value()))
            return false;
    }

    const di::internal::Point2D imageDims = { image.getWidth(), image.getHeight() };

    // Connect each of the nodes to make an 8-connected lattice graph
    connectHorizontally(imageDims);
    connectVertically(imageDims);
    connectForwardDiagonally(imageDims);
    connectBackwardDiagonally(imageDims);

    // Set the image dimensions so we can visualize the graph properly
    m_imageDims = { image.getWidth(), image.getHeight() };

    return true;
}

void SimilarityGraphImpl::printGraph(std::ostream& stream) const noexcept
{
    boost::print_graph(m_graph, boost::get(boost::vertex_index, m_graph), stream);
}

void SimilarityGraphImpl::connectHorizontally(const utility::Point2D<Vertex>& dims)
{
    const auto [imageWidth, imageHeight] = dims;
    for (auto h = 0; h < imageHeight; ++h)
    {
        for (auto w = 1; w < imageWidth; ++w)
        {
            auto curr = utility::FlattenPoint<Vertex>({ w, h }, imageWidth);
            auto prev = utility::FlattenPoint<Vertex>({ w - 1, h }, imageWidth);

            boost::add_edge(prev, curr, m_graph);
        }
    }
}

void SimilarityGraphImpl::connectVertically(const utility::Point2D<Vertex>& dims)
{
    const auto [imageWidth, imageHeight] = dims;
    for (auto w = 0; w < imageWidth; ++w)
    {
        for (auto h = 1; h < imageHeight; ++h)
        {
            auto curr = utility::FlattenPoint<Vertex>({ w, h }, imageWidth);
            auto prev = utility::FlattenPoint<Vertex>({ w, h - 1 }, imageWidth);

            boost::add_edge(prev, curr, m_graph);
        }
    }
}

void SimilarityGraphImpl::connectBackwardDiagonally(const utility::Point2D<Vertex>& dims)
{
    const auto [imageWidth, imageHeight] = dims;
    for (auto h = 1; h < imageHeight; ++h)
    {
        for (auto w = 1; w < imageWidth; ++w)
        {
            auto curr = utility::FlattenPoint<Vertex>({ w, h }, imageWidth);
            auto prev = utility::FlattenPoint<Vertex>({ w - 1, h - 1 }, imageWidth);

            boost::add_edge(prev, curr, m_graph);
        }
    }
}

void SimilarityGraphImpl::connectForwardDiagonally(const utility::Point2D<Vertex>& dims)
{
    const auto [imageWidth, imageHeight] = dims;
    for (auto h = 1; h < imageHeight; ++h)
    {
        for (auto w = imageWidth - 1; w > 0; --w)
        {
            auto curr = utility::FlattenPoint<Vertex>({ w - 1, h }, imageWidth);
            auto prev = utility::FlattenPoint<Vertex>({ w, h - 1 }, imageWidth);

            boost::add_edge(curr, prev, m_graph);
        }
    }
}

bool SimilarityGraphImpl::setNodeProperties(const di::Image<di::YCbCr, stbi_uc>& image)
{
    // The graph needs to be initialized with vertices first
    if (!boost::num_vertices(m_graph))
        return false;

    return di::utility::foreach_pixel(image,
        [&](const di::internal::Point2D& pos, di::YCbCr<stbi_uc> pixel)
        {
            const auto idx = utility::FlattenPoint<Vertex>(pos, image.getWidth());
            std::tie(m_graph[idx].Y, m_graph[idx].Cb, m_graph[idx].Cr) = pixel;

            return true;
        });
}

SimilarityGraphImpl::OptionalEdgeFilter SimilarityGraphImpl::CreateEdgeWeightFilter(
    heuristics::FilteredEdges filteredEdges, EdgeFilter dissimilar) const noexcept
{
    return [=](const Edge& edge)
        -> std::optional<bool>
        {
            const auto source = boost::source(edge, m_graph);
            const auto target = boost::target(edge, m_graph);

            const auto [imageWidth, imageHeight] = m_imageDims;

            // Bail if we're working with a vertical or horizontal edge
            if (utility::GetSlopeClassification(source, target, imageWidth) !=
                utility::SlopeClassification::eInvalid)
            {
                // Ensure we're only evaluating crossing edges
                const auto [xSource, xTarget] = utility::GetCrossingEdge(source, target, imageWidth);
                if (auto [crossingEdge, found] = boost::edge(xSource, xTarget, m_graph); found)
                {
                    // We need to only evaluate this against the crossing edge only
                    // if the crossing edge is connected
                    if (!dissimilar(crossingEdge))
                    {
                        auto crossingEdgeComparator = CreateCrossingEdgeFilter(filteredEdges);
                        return crossingEdgeComparator(edge, crossingEdge);
                    }
                }
            }

            return std::nullopt;
        };
}

SimilarityGraphImpl::EdgeWeightFunc SimilarityGraphImpl::CreateEdgeWeightAccumulator(
    heuristics::FilteredEdges filteredEdges) const noexcept
{
    EdgeWeightFunc curvesWeight = [&](const auto& edge) { return m_graph[edge].curvesWeight; };
    EdgeWeightFunc islandsWeight = [&](const auto& edge) { return m_graph[edge].islandsWeight; };
    EdgeWeightFunc sparsePixelsWeight = [&](const auto& edge) { return m_graph[edge].sparsePixelsWeight; };

    // default to a zero ret function as to not affect the total weight
    EdgeWeightFunc edgeWeightSum = [&](const auto&) { return 0; };

    // Recursively compose weight functions based on the filter
    heuristics::VisitEdgeFilters([&](heuristics::FilteredEdges edgeFilter)
        {
            if ((edgeFilter & filteredEdges) == heuristics::FilteredEdges::eCurves)
                edgeWeightSum = recursiveSum(edgeWeightSum, curvesWeight);

            if ((edgeFilter & filteredEdges) == heuristics::FilteredEdges::eIslands)
                edgeWeightSum = recursiveSum(edgeWeightSum, islandsWeight);

            if ((edgeFilter & filteredEdges) == heuristics::FilteredEdges::eSparsePixels)
                edgeWeightSum = recursiveSum(edgeWeightSum, sparsePixelsWeight);
        });

    return edgeWeightSum;
}

SimilarityGraphImpl::OptionalCrossingEdgeFilter SimilarityGraphImpl::CreateCrossingEdgeFilter(
    heuristics::FilteredEdges filteredEdges) const noexcept
{
    return [=](const Edge& edge, const Edge& crossing)
        -> std::optional<bool>
    {
        auto edgeWeightAccumulator = CreateEdgeWeightAccumulator(filteredEdges);

        auto edgeWeight = edgeWeightAccumulator(edge);
        auto crossingWeight = edgeWeightAccumulator(crossing);

        if (edgeWeight == crossingWeight)
            return std::make_optional(false);
        else if (edgeWeight || crossingWeight)
            return std::make_optional(edgeWeight > crossingWeight);

        return std::nullopt;
    };
}

SimilarityGraphImpl::EdgeFilter SimilarityGraphImpl::CreateEdgeFilter(heuristics::FilteredEdges filteredEdges) const noexcept
{
    // if no filters are given, then we want to show every edge
    EdgeFilter edgeFilter = [](const auto&) { return true; };

    // If we have some flag packed in, then build the filter function
    if (filteredEdges != heuristics::FilteredEdges::eNone)
    {
        edgeFilter = [=](const auto& edge)
        {
            auto s = boost::source(edge, m_graph); s;
            auto t = boost::target(edge, m_graph); t;

            // The paradigm here is that we want to return true to keep the edge,
            // and false to remove it
            EdgeFilter dissimilar = [&](const auto& edge) { return m_graph[edge].dissimilar; };
            if (hasFilter(filteredEdges, heuristics::FilteredEdges::eDissimilar))
            {
                // If the edge is dissimilar, then we don't want to keep it. If it is similar,
                // then we want to try and evaluate it with crossing edge filters
                if (dissimilar(edge))
                    return false;
            }

            if (hasFilter(filteredEdges, heuristics::FilteredEdges::eCurves) ||
                hasFilter(filteredEdges, heuristics::FilteredEdges::eIslands) ||
                hasFilter(filteredEdges, heuristics::FilteredEdges::eSparsePixels))
            {
                // Only evaluate crossing edges that are connected
                auto edgeWeightFilter = CreateEdgeWeightFilter(filteredEdges, dissimilar);

                if (auto crossingResult = edgeWeightFilter(edge); crossingResult.has_value())
                    return crossingResult.value();
            }

            return true;
        };
    }

    return edgeFilter;
}

bool SimilarityGraphImpl::writeTex(std::ostream& output, heuristics::FilteredEdges filteredEdges)
{
    auto filter = CreateEdgeFilter(filteredEdges);
    auto filteredGraph = boost::filtered_graph(m_graph, filter);

    auto strategy = SimilarityGraphVisualizationStrategy<decltype(filteredGraph)>{};
    auto visualizer = LaTeXGraphVisualizer<decltype(filteredGraph)>{ strategy };

    return visualizer.writeTex(filteredGraph, m_imageDims, output);
}

std::set<std::tuple<std::size_t, std::size_t>> SimilarityGraphImpl::getEdges(heuristics::FilteredEdges filteredEdges) noexcept
{
    std::set<std::tuple<std::size_t, std::size_t>> edges;

    auto filter = CreateEdgeFilter(filteredEdges);
    auto filteredGraph = boost::filtered_graph(m_graph, filter);

    for (const auto& edge : boost::make_iterator_range(boost::edges(filteredGraph)))
        edges.insert({boost::source(edge, filteredGraph), boost::target(edge, filteredGraph)});

    return edges;
}
}


// Copyright 2012 The Trustees of Indiana University.

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

//  Authors: Nick Edmonds
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_GRAPH500_GENERATOR_HPP
#define BOOST_GRAPH_GRAPH500_GENERATOR_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph500/graph_generator.hpp> // Graph500 generator
#include <boost/format.hpp>
#include <iostream>

// For debugging; remove later
#include <mpi.h>

namespace boost {

  template<typename Graph, typename WeightIterator=void, typename EdgeWeightType=void>
  class graph500_iterator
    : public iterator_facade<
    graph500_iterator<Graph, WeightIterator, EdgeWeightType>,
    std::pair< typename graph_traits<Graph>::vertices_size_type,
	       std::pair<typename graph_traits<Graph>::vertices_size_type,
                         EdgeWeightType> >,
               std::input_iterator_tag,
               const
    std::pair< typename graph_traits<Graph>::vertices_size_type,
	       std::pair<typename graph_traits<Graph>::vertices_size_type,
			 EdgeWeightType> >&>
  {
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    graph500_iterator() : scale(0), current(0) {}

    // rmat parameters encoded in preprocessor macros
    graph500_iterator(vertices_size_type scale, edges_size_type start_edge,
		      uint64_t seed1, uint64_t seed2, WeightIterator w_iter)
      : scale(scale), current(start_edge), packed_edges(new packed_edge[1 << 16]), weight_iterator(w_iter)
    {
      // make_mrg_seed() from Graph500
      seed[0] = (seed1 & 0x3FFFFFFF) + 1;
      seed[1] = ((seed1 >> 30) & 0x3FFFFFFF) + 1;
      seed[2] = (seed2 & 0x3FFFFFFF) + 1;
      seed[3] = ((seed2 >> 30) & 0x3FFFFFFF) + 1;
      seed[4] = ((seed2 >> 60) << 4) + (seed1 >> 60) + 1;

      generate_kronecker_range(seed, scale,
			       start_edge / (1 << 16) * (1 << 16), start_edge / (1 << 16) * (1 << 16) + (1 << 16),
			       packed_edges.get());

      update_cache(false);
    }

    // rmat values parameterised
    graph500_iterator(vertices_size_type scale, edges_size_type start_edge,
		      uint64_t seed1, uint64_t seed2, WeightIterator w_iter,
		      int32_t rmata,
		      int32_t rmatbc)
      : scale(scale), current(start_edge), packed_edges(new packed_edge[1 << 16]), weight_iterator(w_iter)
    {
      // make_mrg_seed() from Graph500
      seed[0] = (seed1 & 0x3FFFFFFF) + 1;
      seed[1] = ((seed1 >> 30) & 0x3FFFFFFF) + 1;
      seed[2] = (seed2 & 0x3FFFFFFF) + 1;
      seed[3] = ((seed2 >> 30) & 0x3FFFFFFF) + 1;
      seed[4] = ((seed2 >> 60) << 4) + (seed1 >> 60) + 1;

      generate_kronecker_range(seed, scale,
			       start_edge / (1 << 16) * (1 << 16), start_edge / (1 << 16) * (1 << 16) + (1 << 16),
			       packed_edges.get(),
			       rmata,
			       rmatbc);

      update_cache(false);
    }

    void update_cache(bool run_from_increment) {
      if (run_from_increment && current % (1 << 16) == 0) {
        generate_kronecker_range(seed, scale, current, current + (1 << 16), packed_edges.get());
      }
      current_edge = std::make_pair(get_v0_from_edge(&packed_edges[current % (1 << 16)]),
				    std::make_pair(get_v1_from_edge(&packed_edges[current % (1 << 16)]), (*weight_iterator)));
    }

    const std::pair<vertices_size_type, std::pair<vertices_size_type, EdgeWeightType> >&
    dereference() const { return current_edge; }

    void increment() {
      ++current;
      ++weight_iterator;
      update_cache(true);
    }

    bool equal(const graph500_iterator& other) const
    { return current == other.current; }

  private:

    vertices_size_type scale;
    edges_size_type current;
    std::pair<vertices_size_type, std::pair<vertices_size_type, EdgeWeightType >
	      > current_edge; // Now current edge contains the weight also

    uint_fast32_t seed[5];
    shared_array<packed_edge> packed_edges;
    WeightIterator weight_iterator;

  };

  // Graph generator without weight : Is there a better way to do this
  // without duplicating lot of code ?
  template<typename Graph>
  class graph500_iterator<Graph, void, void>
    : public iterator_facade<
    graph500_iterator<Graph>,
    std::pair< typename graph_traits<Graph>::vertices_size_type,
	       typename graph_traits<Graph>::vertices_size_type>,
    std::input_iterator_tag,
    const
    std::pair< typename graph_traits<Graph>::vertices_size_type,
	       typename graph_traits<Graph>::vertices_size_type>&>
  {
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

  public:
    graph500_iterator() : scale(0), current(0) {}

    // rmat parameters encoded in preprocessor macros
    graph500_iterator(vertices_size_type scale, edges_size_type start_edge,
		      uint64_t seed1, uint64_t seed2)
      : scale(scale), current(start_edge), packed_edges(new packed_edge[1 << 16])
    {
      // make_mrg_seed() from Graph500
      seed[0] = (seed1 & 0x3FFFFFFF) + 1;
      seed[1] = ((seed1 >> 30) & 0x3FFFFFFF) + 1;
      seed[2] = (seed2 & 0x3FFFFFFF) + 1;
      seed[3] = ((seed2 >> 30) & 0x3FFFFFFF) + 1;
      seed[4] = ((seed2 >> 60) << 4) + (seed1 >> 60) + 1;

      generate_kronecker_range(seed, scale,
			       start_edge / (1 << 16) * (1 << 16), start_edge / (1 << 16) * (1 << 16) + (1 << 16),
			       packed_edges.get());

      update_cache(false);
    }

    // rmat values parameterised
    graph500_iterator(vertices_size_type scale, edges_size_type start_edge,
		      uint64_t seed1, uint64_t seed2,
		      int32_t rmata,
		      int32_t rmatbc)
      : scale(scale), current(start_edge), packed_edges(new packed_edge[1 << 16])
    {
      // make_mrg_seed() from Graph500
      seed[0] = (seed1 & 0x3FFFFFFF) + 1;
      seed[1] = ((seed1 >> 30) & 0x3FFFFFFF) + 1;
      seed[2] = (seed2 & 0x3FFFFFFF) + 1;
      seed[3] = ((seed2 >> 30) & 0x3FFFFFFF) + 1;
      seed[4] = ((seed2 >> 60) << 4) + (seed1 >> 60) + 1;

      generate_kronecker_range(seed, scale,
			       start_edge / (1 << 16) * (1 << 16), start_edge / (1 << 16) * (1 << 16) + (1 << 16),
			       packed_edges.get(),
			       rmata,
			       rmatbc);

      update_cache(false);
    }

    void update_cache(bool run_from_increment) {
      if (run_from_increment && current % (1 << 16) == 0) {
        generate_kronecker_range(seed, scale, current, current + (1 << 16), packed_edges.get());
      }
      current_edge = std::make_pair(get_v0_from_edge(&packed_edges[current % (1 << 16)]),
				    get_v1_from_edge(&packed_edges[current % (1 << 16)]));
    }

    const std::pair<vertices_size_type, vertices_size_type> &
    dereference() const { return current_edge; }

    void increment() {
      ++current;
      update_cache(true);
    }

    bool equal(const graph500_iterator& other) const
    { return current == other.current; }

  private:

    vertices_size_type scale;
    edges_size_type current;
    std::pair<vertices_size_type, vertices_size_type> current_edge; // Current edge
    uint_fast32_t seed[5];
    shared_array<packed_edge> packed_edges;

  };

} // end namespace boost

#endif // BOOST_GRAPH_GRAPH500_GENERATOR_HPP