// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Framework/TestingFramework.hpp"

#include <array>
#include <boost/functional/hash.hpp>
#include <functional>
#include <limits>
#include <string>
#include <type_traits>
#include <unordered_map>
#include <utility>

#include "DataStructures/DataBox/Tag.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/Tags.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/LogicalCoordinates.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/DirectionMap.hpp"
#include "Domain/Structure/Element.hpp"  // IWYU pragma: keep
#include "Domain/Structure/ElementId.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/MinmodHelpers.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/MinmodTci.hpp"
#include "Helpers/Evolution/DiscontinuousGalerkin/Limiters/TestHelpers.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "NumericalAlgorithms/Spectral/Spectral.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/MakeArray.hpp"
#include "Utilities/TaggedTuple.hpp"

namespace {

auto make_two_neighbors(const double left, const double right) noexcept {
  DirectionMap<1, double> result;
  result[Direction<1>::lower_xi()] = left;
  result[Direction<1>::upper_xi()] = right;
  return result;
}

auto make_four_neighbors(const std::array<double, 4>& values) noexcept {
  DirectionMap<2, double> result;
  result[Direction<2>::lower_xi()] = values[0];
  result[Direction<2>::upper_xi()] = values[1];
  result[Direction<2>::lower_eta()] = values[2];
  result[Direction<2>::upper_eta()] = values[3];
  return result;
}

auto make_six_neighbors(const std::array<double, 6>& values) noexcept {
  DirectionMap<3, double> result;
  result[Direction<3>::lower_xi()] = values[0];
  result[Direction<3>::upper_xi()] = values[1];
  result[Direction<3>::lower_eta()] = values[2];
  result[Direction<3>::upper_eta()] = values[3];
  result[Direction<3>::lower_zeta()] = values[4];
  result[Direction<3>::upper_zeta()] = values[5];
  return result;
}

// Test that TCI detects a troubled cell when expected
template <size_t VolumeDim>
void test_tci_detection(
    const bool expected_detection, const double tvb_constant,
    const DataVector& input, const Mesh<VolumeDim>& mesh,
    const Element<VolumeDim>& element,
    const std::array<double, VolumeDim>& element_size,
    const DirectionMap<VolumeDim, double>& effective_neighbor_means,
    const DirectionMap<VolumeDim, double>& effective_neighbor_sizes) noexcept {
  Limiters::Minmod_detail::BufferWrapper<VolumeDim> buffer(mesh);
  const bool detection = Limiters::Tci::tvb_minmod_indicator(
      make_not_null(&buffer), tvb_constant, input, mesh, element, element_size,
      effective_neighbor_means, effective_neighbor_sizes);
  CHECK(detection == expected_detection);
}

void test_tci_on_linear_function(
    const size_t number_of_grid_points,
    const Spectral::Quadrature quadrature) noexcept {
  INFO("Testing TCI on linear function");
  CAPTURE(number_of_grid_points);
  CAPTURE(quadrature);
  const auto element = TestHelpers::Limiters::make_element<1>();
  const Mesh<1> mesh(number_of_grid_points, Spectral::Basis::Legendre,
                     quadrature);

  // Lambda takes tvb_scale = tvb_constant * h^2, to facilitate specifying
  // critical threshold values for testing
  const auto test_tci = [&mesh, &element](
                            const bool expected, const DataVector& input,
                            const double left_mean, const double right_mean,
                            const double tvb_scale) noexcept {
    const double h = 1.2;
    const double tvb_constant = tvb_scale / square(h);
    const auto element_size = make_array<1>(h);
    test_tci_detection(expected, tvb_constant, input, mesh, element,
                       element_size, make_two_neighbors(left_mean, right_mean),
                       make_two_neighbors(h, h));
  };

  // mean = 1.6
  // delta_left = delta_right = 0.2
  const DataVector input = [&mesh]() noexcept {
    const auto x = get<0>(logical_coordinates(mesh));
    return DataVector{1.6 + 0.2 * x};
  }();

  // Test trigger due to left, right neighbors
  test_tci(false, input, 1.35, 1.85, 0.0);
  test_tci(true, input, 1.45, 1.85, 0.0);
  test_tci(true, input, 1.35, 1.75, 0.0);
  test_tci(true, input, 1.45, 1.75, 0.0);

  // Test trigger due to slope changing sign
  test_tci(true, input, 1.7, 1.85, 0.0);
  test_tci(true, input, 1.45, 1.5, 0.0);

  // Test TVB can avoid the triggers
  test_tci(true, input, 1.45, 1.75, 0.19);
  test_tci(true, input, 1.7, 1.85, 0.19);
  test_tci(true, input, 1.45, 1.5, 0.19);
  test_tci(false, input, 1.45, 1.75, 0.21);
  test_tci(false, input, 1.7, 1.85, 0.21);
  test_tci(false, input, 1.45, 1.5, 0.21);
}

void test_tci_on_quadratic_function(
    const size_t number_of_grid_points,
    const Spectral::Quadrature quadrature) noexcept {
  INFO("Testing TCI on quadratic function");
  CAPTURE(number_of_grid_points);
  CAPTURE(quadrature);
  const auto element = TestHelpers::Limiters::make_element<1>();
  const Mesh<1> mesh(number_of_grid_points, Spectral::Basis::Legendre,
                     quadrature);

  // Lambda takes tvb_scale = tvb_constant * h^2, to facilitate specifying
  // critical threshold values for testing
  const auto test_tci = [&mesh, &element](
                            const bool expected, const DataVector& input,
                            const double left_mean, const double right_mean,
                            const double tvb_scale) noexcept {
    const double h = 1.2;
    const double tvb_constant = tvb_scale / square(h);
    const auto element_size = make_array<1>(h);
    test_tci_detection(expected, tvb_constant, input, mesh, element,
                       element_size, make_two_neighbors(left_mean, right_mean),
                       make_two_neighbors(h, h));
  };

  // mean = 1.5
  // delta_left = 0.1
  // delta_right = 0.3
  const DataVector input = [&mesh]() noexcept {
    const auto x = get<0>(logical_coordinates(mesh));
    return DataVector{1.45 + 0.2 * x + 0.15 * square(x)};
  }();

  // Test trigger due to left, right neighbors
  test_tci(false, input, 1.15, 1.85, 0.0);
  test_tci(true, input, 1.25, 1.85, 0.0);
  test_tci(true, input, 1.15, 1.75, 0.0);
  test_tci(true, input, 1.25, 1.75, 0.0);

  // Test TVB can avoid the trigger
  test_tci(true, input, 1.25, 1.85, 0.11);
  test_tci(true, input, 1.25, 1.85, 0.29);
  test_tci(false, input, 1.25, 1.85, 0.31);

  // mean = 1.5
  // delta_left = -0.1
  // delta_right = 0.3
  const DataVector input2 = [&mesh]() noexcept {
    const auto x = get<0>(logical_coordinates(mesh));
    return DataVector{1.4 + 0.1 * x + 0.3 * square(x)};
  }();

  // Because left-to-mean and mean-to-right slopes have different signs,
  // any TCI call with TVB=0 should trigger
  test_tci(true, input2, 1.7, 1.9, 0.0);
  test_tci(true, input2, 1.7, 1.3, 0.0);
  test_tci(true, input2, 1.3, 1.7, 0.0);
  test_tci(true, input2, 1.1, 1.9, 0.0);

  // Conversely, because left-to-mean and mean-to-right slopes have different
  // signs, calls with TVB>0 give results that depend on neighbor means
  test_tci(true, input2, 1.7, 1.3, 0.11);
  test_tci(true, input2, 1.7, 1.3, 0.29);
  test_tci(false, input2, 1.7, 1.3, 0.31);

  test_tci(true, input2, 1.3, 1.7, 0.11);
  test_tci(true, input2, 1.3, 1.7, 0.29);
  test_tci(false, input2, 1.3, 1.7, 0.31);

  test_tci(false, input2, 1.1, 1.9, 0.11);
  test_tci(false, input2, 1.1, 1.9, 0.29);
  test_tci(false, input2, 1.1, 1.9, 0.31);
}

void test_tci_at_boundary(const size_t number_of_grid_points,
                          const Spectral::Quadrature quadrature) noexcept {
  INFO("Testing TCI at boundary");
  CAPTURE(number_of_grid_points);
  CAPTURE(quadrature);
  const double tvb_constant = 0.0;
  const Mesh<1> mesh(number_of_grid_points, Spectral::Basis::Legendre,
                     quadrature);
  const auto element_size = make_array<1>(2.0);

  const auto input = [&mesh]() noexcept {
    const auto coords = logical_coordinates(mesh);
    return DataVector{1.2 * get<0>(coords)};
  }();

  // Test with element that has external lower-xi boundary
  const auto element_at_lower_xi_boundary =
      TestHelpers::Limiters::make_element<1>({{Direction<1>::lower_xi()}});
  for (const double neighbor_mean : {-1.21, -1.19, 0.0, 1.19, 1.21}) {
    const bool expected_tci = neighbor_mean < 1.2;
    test_tci_detection(
        expected_tci, tvb_constant, input, mesh, element_at_lower_xi_boundary,
        element_size,
        {{std::make_pair(Direction<1>::upper_xi(), neighbor_mean)}},
        {{std::make_pair(Direction<1>::upper_xi(), element_size[0])}});
  }

  // Test with element that has external upper-xi boundary
  const auto element_at_upper_xi_boundary =
      TestHelpers::Limiters::make_element<1>({{Direction<1>::upper_xi()}});
  for (const double neighbor_mean : {-1.21, -1.19, 0.0, 1.19, 1.21}) {
    const bool expected_tci = neighbor_mean > -1.2;
    test_tci_detection(
        expected_tci, tvb_constant, input, mesh, element_at_upper_xi_boundary,
        element_size,
        {{std::make_pair(Direction<1>::lower_xi(), neighbor_mean)}},
        {{std::make_pair(Direction<1>::lower_xi(), element_size[0])}});
  }

  const auto element_with_no_neighbors = TestHelpers::Limiters::make_element<1>(
      {{Direction<1>::lower_xi(), Direction<1>::upper_xi()}});
  test_tci_detection(false, tvb_constant, input, mesh,
                     element_with_no_neighbors, element_size, {{}}, {{}});
}

void test_tci_with_different_size_neighbor(
    const size_t number_of_grid_points,
    const Spectral::Quadrature quadrature) noexcept {
  INFO("Testing TCI with neighboring elements of different size");
  CAPTURE(number_of_grid_points);
  CAPTURE(quadrature);
  const double tvb_constant = 0.0;
  const auto element = TestHelpers::Limiters::make_element<1>();
  const Mesh<1> mesh(number_of_grid_points, Spectral::Basis::Legendre,
                     quadrature);
  const double dx = 1.0;
  const auto element_size = make_array<1>(dx);

  const auto test_tci = [&tvb_constant, &element, &mesh, &element_size](
                            const bool expected_detection,
                            const DataVector& local_input, const double left,
                            const double right, const double left_size,
                            const double right_size) noexcept {
    test_tci_detection(expected_detection, tvb_constant, local_input, mesh,
                       element, element_size, make_two_neighbors(left, right),
                       make_two_neighbors(left_size, right_size));
  };

  const double eps = 100.0 * std::numeric_limits<double>::epsilon();

  const auto input = [&mesh]() noexcept {
    const auto coords = logical_coordinates(mesh);
    return DataVector{2.0 + 1.2 * get<0>(coords)};
  }();

  // Establish baseline using evenly-sized elements
  test_tci(false, input, 0.8 - eps, 3.2 + eps, dx, dx);

  const double larger = 2.0 * dx;
  const double smaller = 0.5 * dx;

  // Larger neighbor with same mean => true reduction in slope => trigger
  test_tci(true, input, 0.8 - eps, 3.2, dx, larger);
  // Larger neighbor with larger mean => same slope => no trigger
  test_tci(false, input, 0.8 - eps, 3.8 + eps, dx, larger);

  // Smaller neighbor with same mean => increased slope => no trigger
  test_tci(false, input, 0.8 - eps, 3.2 + eps, dx, smaller);
  // Smaller neighbor with lower mean => same slope => no trigger
  test_tci(false, input, 0.8 - eps, 2.9 + eps, dx, smaller);

  test_tci(true, input, 0.8, 3.2 + eps, larger, dx);
  test_tci(false, input, 0.2 - eps, 3.2 + eps, larger, dx);

  test_tci(false, input, 0.8 - eps, 3.2 + eps, smaller, dx);
  test_tci(false, input, 1.1 - eps, 3.2 + eps, smaller, dx);
}

// In 1D, test combinations of TVB constant, polynomial order, etc.
// Check that each combination has the expected TCI behavior.
void test_tvb_minmod_tci_1d() noexcept {
  INFO("Testing MinmodTci in 1D");
  for (const auto quadrature :
       {Spectral::Quadrature::GaussLobatto, Spectral::Quadrature::Gauss}) {
    for (const auto num_grid_points : std::array<size_t, 2>{{2, 4}}) {
      test_tci_on_linear_function(num_grid_points, quadrature);
      test_tci_at_boundary(num_grid_points, quadrature);
      test_tci_with_different_size_neighbor(num_grid_points, quadrature);
    }
    // This test only makes sense with more than 2 grid points
    test_tci_on_quadratic_function(3, quadrature);
    test_tci_on_quadratic_function(4, quadrature);
  }
}

void test_tvb_minmod_tci_2d_impl(
    const Spectral::Quadrature quadrature) noexcept {
  CAPTURE(quadrature);
  const double tvb_constant = 0.0;
  const auto element = TestHelpers::Limiters::make_element<2>();
  const Mesh<2> mesh(3, Spectral::Basis::Legendre, quadrature);
  const auto element_size = make_array<2>(2.0);

  const auto test_tci =
      [&tvb_constant, &element, &mesh, &element_size](
          const bool expected_detection, const DataVector& local_input,
          const std::array<double, 4>& neighbor_means) noexcept {
        test_tci_detection(expected_detection, tvb_constant, local_input, mesh,
                           element, element_size,
                           make_four_neighbors(neighbor_means),
                           make_four_neighbors(make_array<4>(2.0)));
      };

  const auto input = [&mesh]() noexcept {
    const auto coords = logical_coordinates(mesh);
    const auto& x = get<0>(coords);
    const auto& y = get<1>(coords);
    return DataVector{3.0 + x + 2.0 * y + 0.1 * x * y};
  }();

  // Case with no activation
  test_tci(false, input, {{1.9, 4.2, -0.5, 5.6}});

  // Limit because of xi-direction neighbors
  test_tci(true, input, {{2.2, 4.2, -0.5, 5.6}});
  test_tci(true, input, {{1.9, 3.2, -0.5, 5.6}});

  // Limit because of eta-direction neighbors
  test_tci(true, input, {{1.9, 4.2, 1.5, 5.6}});
  test_tci(true, input, {{1.9, 4.2, -0.5, 2.9}});

  // Limit for xi and eta directions
  test_tci(true, input, {{2.2, 4.2, 1.5, 5.6}});
  test_tci(true, input, {{3.9, 4.2, -0.5, 2.9}});
}

// In 2D, test that the dimension-by-dimension application of the TCI works as
// expected.
void test_tvb_minmod_tci_2d() noexcept {
  INFO("Testing MinmodTci in 2D");
  test_tvb_minmod_tci_2d_impl(Spectral::Quadrature::GaussLobatto);
  test_tvb_minmod_tci_2d_impl(Spectral::Quadrature::Gauss);
}

void test_tvb_minmod_tci_3d_impl(
    const Spectral::Quadrature quadrature) noexcept {
  CAPTURE(quadrature);
  const double tvb_constant = 0.0;
  const auto element = TestHelpers::Limiters::make_element<3>();
  const Mesh<3> mesh(3, Spectral::Basis::Legendre, quadrature);
  const auto element_size = make_array<3>(2.0);

  const auto test_tci =
      [&tvb_constant, &element, &mesh, &element_size](
          const bool expected_detection, const DataVector& local_input,
          const std::array<double, 6>& neighbor_means) noexcept {
        test_tci_detection(expected_detection, tvb_constant, local_input, mesh,
                           element, element_size,
                           make_six_neighbors(neighbor_means),
                           make_six_neighbors(make_array<6>(2.0)));
      };

  const auto input = [&mesh]() noexcept {
    const auto coords = logical_coordinates(mesh);
    const auto& x = get<0>(coords);
    const auto& y = get<1>(coords);
    const auto& z = get<2>(coords);
    return DataVector{2.0 - 1.6 * x + 0.4 * y + 0.4 * z + 0.1 * x * y -
                      0.1 * x * z - 0.2 * y * z};
  }();

  // Case with no activation
  test_tci(false, input, {{3.8, -0.1, 1.5, 2.7, 1.2, 2.5}});

  // Limit because of xi-direction neighbors
  test_tci(true, input, {{3.4, -0.1, 1.5, 2.7, 1.2, 2.5}});
  test_tci(true, input, {{3.8, 2.1, 1.5, 2.7, 1.2, 2.5}});

  // Limit because of eta-direction neighbors
  test_tci(true, input, {{3.8, -0.1, 1.9, 2.7, 1.2, 2.5}});
  test_tci(true, input, {{3.8, -0.1, 1.5, 2.3, 1.2, 2.5}});

  // Limit because of zeta-direction neighbors
  test_tci(true, input, {{3.8, -0.1, 1.5, 2.7, 2.2, 2.5}});
  test_tci(true, input, {{3.8, -0.1, 1.5, 2.7, 1.2, 2.1}});

  // Limit for xi, eta, and zeta directions
  test_tci(true, input, {{3.4, -0.1, 1.5, 2.3, 1.2, 2.1}});
  test_tci(true, input, {{3.8, 2.1, 2.1, 2.7, 2.2, 2.5}});
}

// In 3D, test that the dimension-by-dimension application of the TCI works as
// expected.
void test_tvb_minmod_tci_3d() noexcept {
  INFO("Testing MinmodTci in 3D");
  test_tvb_minmod_tci_3d_impl(Spectral::Quadrature::GaussLobatto);
  test_tvb_minmod_tci_3d_impl(Spectral::Quadrature::Gauss);
}

struct ScalarTag : db::SimpleTag {
  using type = Scalar<DataVector>;
  static std::string name() noexcept { return "Scalar"; }
};

template <size_t VolumeDim>
struct VectorTag : db::SimpleTag {
  using type = tnsr::I<DataVector, VolumeDim>;
  static std::string name() noexcept { return "Vector"; }
};

void test_tvb_minmod_tci_several_tensors() noexcept {
  INFO("Testing MinmodTci action on several tensors");
  // Test that TCI returns true if just one component needs limiting, which
  // we do by limiting a scalar and vector in 3D
  const double tvb_constant = 0.0;
  const auto element = TestHelpers::Limiters::make_element<3>();
  const size_t number_of_grid_points = 2;
  const Mesh<3> mesh(number_of_grid_points, Spectral::Basis::Legendre,
                     Spectral::Quadrature::GaussLobatto);
  const auto element_size = make_array<3>(1.0);

  const auto linear_data = [&mesh](const double mean, const double slope_x,
                                   const double slope_y,
                                   const double slope_z) noexcept {
    const auto coords = logical_coordinates(mesh);
    const auto& x = get<0>(coords);
    const auto& y = get<1>(coords);
    const auto& z = get<2>(coords);
    return DataVector{mean + slope_x * x + slope_y * y + slope_z * z};
  };

  Scalar<DataVector> local_scalar{linear_data(1.8, 1.4, 0.1, -0.2)};
  tnsr::I<DataVector, 3> local_vector{
      {{linear_data(-0.4, -0.3, 0.3, 1.0), linear_data(0.02, 0.01, 0.01, 0.2),
        linear_data(2.3, 0.5, -0.3, -0.2)}}};

  struct TestPackagedData {
    tuples::TaggedTuple<::Tags::Mean<ScalarTag>, ::Tags::Mean<VectorTag<3>>>
        means;
    std::array<double, 3> element_size =
        make_array<3>(std::numeric_limits<double>::signaling_NaN());
  };
  std::unordered_map<std::pair<Direction<3>, ElementId<3>>, TestPackagedData,
                     boost::hash<std::pair<Direction<3>, ElementId<3>>>>
      neighbor_data{};
  TestPackagedData& lower_xi_neighbor =
      neighbor_data[std::make_pair(Direction<3>::lower_xi(), ElementId<3>(1))];
  TestPackagedData& upper_xi_neighbor =
      neighbor_data[std::make_pair(Direction<3>::upper_xi(), ElementId<3>(2))];
  TestPackagedData& lower_eta_neighbor =
      neighbor_data[std::make_pair(Direction<3>::lower_eta(), ElementId<3>(3))];
  TestPackagedData& upper_eta_neighbor =
      neighbor_data[std::make_pair(Direction<3>::upper_eta(), ElementId<3>(4))];
  TestPackagedData& lower_zeta_neighbor = neighbor_data[std::make_pair(
      Direction<3>::lower_zeta(), ElementId<3>(5))];
  TestPackagedData& upper_zeta_neighbor = neighbor_data[std::make_pair(
      Direction<3>::upper_zeta(), ElementId<3>(6))];
  lower_xi_neighbor.element_size = element_size;
  upper_xi_neighbor.element_size = element_size;
  lower_eta_neighbor.element_size = element_size;
  upper_eta_neighbor.element_size = element_size;
  lower_zeta_neighbor.element_size = element_size;
  upper_zeta_neighbor.element_size = element_size;

  // Case where neither tensor triggers limiting
  get(get<::Tags::Mean<ScalarTag>>(lower_xi_neighbor.means)) = 0.3;
  get(get<::Tags::Mean<ScalarTag>>(upper_xi_neighbor.means)) = 3.3;
  get(get<::Tags::Mean<ScalarTag>>(lower_eta_neighbor.means)) = 1.6;
  get(get<::Tags::Mean<ScalarTag>>(upper_eta_neighbor.means)) = 2.2;
  get(get<::Tags::Mean<ScalarTag>>(lower_zeta_neighbor.means)) = 2.1;
  get(get<::Tags::Mean<ScalarTag>>(upper_zeta_neighbor.means)) = 1.4;
  get<0>(get<::Tags::Mean<VectorTag<3>>>(lower_xi_neighbor.means)) = 0.1;
  get<0>(get<::Tags::Mean<VectorTag<3>>>(upper_xi_neighbor.means)) = -0.9;
  get<0>(get<::Tags::Mean<VectorTag<3>>>(lower_eta_neighbor.means)) = -1.1;
  get<0>(get<::Tags::Mean<VectorTag<3>>>(upper_eta_neighbor.means)) = 0.2;
  get<0>(get<::Tags::Mean<VectorTag<3>>>(lower_zeta_neighbor.means)) = -1.8;
  get<0>(get<::Tags::Mean<VectorTag<3>>>(upper_zeta_neighbor.means)) = 0.7;
  get<1>(get<::Tags::Mean<VectorTag<3>>>(lower_xi_neighbor.means)) = 0.0;
  get<1>(get<::Tags::Mean<VectorTag<3>>>(upper_xi_neighbor.means)) = 0.05;
  get<1>(get<::Tags::Mean<VectorTag<3>>>(lower_eta_neighbor.means)) = -0.1;
  get<1>(get<::Tags::Mean<VectorTag<3>>>(upper_eta_neighbor.means)) = 0.1;
  get<1>(get<::Tags::Mean<VectorTag<3>>>(lower_zeta_neighbor.means)) = -0.7;
  get<1>(get<::Tags::Mean<VectorTag<3>>>(upper_zeta_neighbor.means)) = 0.3;
  get<2>(get<::Tags::Mean<VectorTag<3>>>(lower_xi_neighbor.means)) = 1.4;
  get<2>(get<::Tags::Mean<VectorTag<3>>>(upper_xi_neighbor.means)) = 2.9;
  get<2>(get<::Tags::Mean<VectorTag<3>>>(lower_eta_neighbor.means)) = 2.7;
  get<2>(get<::Tags::Mean<VectorTag<3>>>(upper_eta_neighbor.means)) = 1.8;
  get<2>(get<::Tags::Mean<VectorTag<3>>>(lower_zeta_neighbor.means)) = 3.1;
  get<2>(get<::Tags::Mean<VectorTag<3>>>(upper_zeta_neighbor.means)) = 0.5;
  const bool trigger_base_case =
      Limiters::Tci::tvb_minmod_indicator<3, TestPackagedData, ScalarTag,
                                          VectorTag<3>>(
          tvb_constant, local_scalar, local_vector, mesh, element, element_size,
          neighbor_data);
  CHECK_FALSE(trigger_base_case);

  // Case where the scalar triggers limiting
  get(get<::Tags::Mean<ScalarTag>>(upper_xi_neighbor.means)) = 2.0;
  const bool trigger_scalar =
      Limiters::Tci::tvb_minmod_indicator<3, TestPackagedData, ScalarTag,
                                          VectorTag<3>>(
          tvb_constant, local_scalar, local_vector, mesh, element, element_size,
          neighbor_data);
  CHECK(trigger_scalar);

  // Case where the vector x-component triggers limiting
  get(get<::Tags::Mean<ScalarTag>>(upper_xi_neighbor.means)) = 3.3;
  get<0>(get<::Tags::Mean<VectorTag<3>>>(lower_zeta_neighbor.means)) = -0.1;
  const bool trigger_vector_x =
      Limiters::Tci::tvb_minmod_indicator<3, TestPackagedData, ScalarTag,
                                          VectorTag<3>>(
          tvb_constant, local_scalar, local_vector, mesh, element, element_size,
          neighbor_data);
  CHECK(trigger_vector_x);

  // Case where the vector y-component triggers limiting
  get<0>(get<::Tags::Mean<VectorTag<3>>>(lower_zeta_neighbor.means)) = -1.8;
  get<1>(get<::Tags::Mean<VectorTag<3>>>(upper_eta_neighbor.means)) = -0.2;
  const bool trigger_vector_y =
      Limiters::Tci::tvb_minmod_indicator<3, TestPackagedData, ScalarTag,
                                          VectorTag<3>>(
          tvb_constant, local_scalar, local_vector, mesh, element, element_size,
          neighbor_data);
  CHECK(trigger_vector_y);

  // Case where the vector z-component triggers limiting
  get<1>(get<::Tags::Mean<VectorTag<3>>>(upper_eta_neighbor.means)) = 0.1;
  get<2>(get<::Tags::Mean<VectorTag<3>>>(lower_xi_neighbor.means)) = 1.9;
  const bool trigger_vector_z =
      Limiters::Tci::tvb_minmod_indicator<3, TestPackagedData, ScalarTag,
                                          VectorTag<3>>(
          tvb_constant, local_scalar, local_vector, mesh, element, element_size,
          neighbor_data);
  CHECK(trigger_vector_z);
}

}  // namespace

SPECTRE_TEST_CASE("Unit.Evolution.DG.Limiters.MinmodTci", "[Limiters][Unit]") {
  test_tvb_minmod_tci_1d();
  test_tvb_minmod_tci_2d();
  test_tvb_minmod_tci_3d();

  test_tvb_minmod_tci_several_tensors();
}


//  Copyright (c) 2007 John Maddock
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// This header just defines the function entry points, and adds dispatch
// to the right implementation method.  Most of the implementation details
// are in separate headers and copyright Xiaogang Zhang.
//
#ifndef BOOST_MATH_BESSEL_HPP
#define BOOST_MATH_BESSEL_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/detail/bessel_jy.hpp>
#include <boost/math/special_functions/detail/bessel_jn.hpp>
#include <boost/math/special_functions/detail/bessel_yn.hpp>
#include <boost/math/special_functions/detail/bessel_ik.hpp>
#include <boost/math/special_functions/detail/bessel_i0.hpp>
#include <boost/math/special_functions/detail/bessel_i1.hpp>
#include <boost/math/special_functions/detail/bessel_kn.hpp>
#include <boost/math/special_functions/sin_pi.hpp>
#include <boost/math/special_functions/cos_pi.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/math/special_functions/trunc.hpp>
#include <boost/math/special_functions/round.hpp>
#include <boost/math/tools/rational.hpp>
#include <boost/math/tools/promotion.hpp>

namespace boost{ namespace math{

namespace detail{

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

   bessel_j_small_z_series_term(T v_, T x)
      : N(0), v(v_)
   {
      BOOST_MATH_STD_USING
      mult = x / 2;
      term = pow(mult, v) / boost::math::tgamma(v+1, Policy());
      mult *= -mult;
   }
   T operator()()
   {
      T r = term;
      ++N;
      term *= mult / (N * (N + v));
      return r;
   }
private:
   unsigned N;
   T v;
   T mult;
   T term;
};

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

   sph_bessel_j_small_z_series_term(unsigned v_, T x)
      : N(0), v(v_)
   {
      BOOST_MATH_STD_USING
      mult = x / 2;
      term = pow(mult, T(v)) / boost::math::tgamma(v+1+T(0.5f), Policy());
      mult *= -mult;
   }
   T operator()()
   {
      T r = term;
      ++N;
      term *= mult / (N * T(N + v + 0.5f));
      return r;
   }
private:
   unsigned N;
   unsigned v;
   T mult;
   T term;
};

template <class T, class Policy>
inline T bessel_j_small_z_series(T v, T x, const Policy& pol)
{
   bessel_j_small_z_series_term<T, Policy> s(v, x);
   boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
   T zero = 0;
   T result = boost::math::tools::sum_series(s, boost::math::policies::digits<T, Policy>(), max_iter, zero);
#else
   T result = boost::math::tools::sum_series(s, boost::math::policies::digits<T, Policy>(), max_iter);
#endif
   policies::check_series_iterations("boost::math::bessel_j_small_z_series<%1%>(%1%,%1%)", max_iter, pol);
   return result;
}

template <class T, class Policy>
inline T sph_bessel_j_small_z_series(unsigned v, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names
   sph_bessel_j_small_z_series_term<T, Policy> s(v, x);
   boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
   T zero = 0;
   T result = boost::math::tools::sum_series(s, boost::math::policies::digits<T, Policy>(), max_iter, zero);
#else
   T result = boost::math::tools::sum_series(s, boost::math::policies::digits<T, Policy>(), max_iter);
#endif
   policies::check_series_iterations("boost::math::sph_bessel_j_small_z_series<%1%>(%1%,%1%)", max_iter, pol);
   return result * sqrt(constants::pi<T>() / 4);
}

template <class T, class Policy>
T cyl_bessel_j_imp(T v, T x, const bessel_no_int_tag& t, const Policy& pol)
{
   BOOST_MATH_STD_USING
   static const char* function = "boost::math::bessel_j<%1%>(%1%,%1%)";
   if(x < 0)
   {
      // better have integer v:
      if(floor(v) == v)
      {
         T r = cyl_bessel_j_imp(v, -x, t, pol);
         if(iround(v, pol) & 1)
            r = -r;
         return r;
      }
      else
         return policies::raise_domain_error<T>(
            function,
            "Got x = %1%, but we need x >= 0", x, pol);
   }
   if(x == 0)
      return (v == 0) ? 1 : (v > 0) ? 0 :
         policies::raise_domain_error<T>(
            function,
            "Got v = %1%, but require v >= 0 or a negative integer: the result would be complex.", v, pol);

   if((v >= 0) && ((x < 1) || (v > x * x / 4)))
   {
      return bessel_j_small_z_series(v, x, pol);
   }

   T j, y;
   bessel_jy(v, x, &j, &y, need_j, pol);
   return j;
}

template <class T, class Policy>
inline T cyl_bessel_j_imp(T v, T x, const bessel_maybe_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING  // ADL of std names.
   typedef typename bessel_asymptotic_tag<T, Policy>::type tag_type;
   if((fabs(v) < 200) && (floor(v) == v))
   {
      if(fabs(x) > asymptotic_bessel_j_limit<T>(v, tag_type()))
         return asymptotic_bessel_j_large_x_2(v, x);
      else
         return bessel_jn(iround(v, pol), x, pol);
   }
   return cyl_bessel_j_imp(v, x, bessel_no_int_tag(), pol);
}

template <class T, class Policy>
inline T cyl_bessel_j_imp(int v, T x, const bessel_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING
   typedef typename bessel_asymptotic_tag<T, Policy>::type tag_type;
   if(fabs(x) > asymptotic_bessel_j_limit<T>(abs(v), tag_type()))
   {
      T r = asymptotic_bessel_j_large_x_2(static_cast<T>(abs(v)), x);
      if((v < 0) && (v & 1))
         r = -r;
      return r;
   }
   else
      return bessel_jn(v, x, pol);
}

template <class T, class Policy>
inline T sph_bessel_j_imp(unsigned n, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names
   if(x < 0)
      return policies::raise_domain_error<T>(
         "boost::math::sph_bessel_j<%1%>(%1%,%1%)",
         "Got x = %1%, but function requires x > 0.", x, pol);
   //
   // Special case, n == 0 resolves down to the sinus cardinal of x:
   //
   if(n == 0)
      return boost::math::sinc_pi(x, pol);
   //
   // When x is small we may end up with 0/0, use series evaluation
   // instead, especially as it converges rapidly:
   //
   if(x < 1)
      return sph_bessel_j_small_z_series(n, x, pol);
   //
   // Default case is just a naive evaluation of the definition:
   //
   return sqrt(constants::pi<T>() / (2 * x))
      * cyl_bessel_j_imp(T(n)+T(0.5f), x, bessel_no_int_tag(), pol);
}

template <class T, class Policy>
T cyl_bessel_i_imp(T v, T x, const Policy& pol)
{
   //
   // This handles all the bessel I functions, note that we don't optimise
   // for integer v, other than the v = 0 or 1 special cases, as Millers
   // algorithm is at least as inefficient as the general case (the general
   // case has better error handling too).
   //
   BOOST_MATH_STD_USING
   if(x < 0)
   {
      // better have integer v:
      if(floor(v) == v)
      {
         T r = cyl_bessel_i_imp(v, -x, pol);
         if(iround(v, pol) & 1)
            r = -r;
         return r;
      }
      else
         return policies::raise_domain_error<T>(
         "boost::math::cyl_bessel_i<%1%>(%1%,%1%)",
            "Got x = %1%, but we need x >= 0", x, pol);
   }
   if(x == 0)
   {
      return (v == 0) ? 1 : 0;
   }
   if(v == 0.5f)
   {
      // common special case, note try and avoid overflow in exp(x):
      T e = exp(x / 2);
      return e * (e / sqrt(2 * x * constants::pi<T>()));
   }
   if(policies::digits<T, Policy>() <= 64)
   {
      if(v == 0)
      {
         return bessel_i0(x);
      }
      if(v == 1)
      {
         return bessel_i1(x);
      }
   }
   T I, K;
   bessel_ik(v, x, &I, &K, need_i, pol);
   return I;
}

template <class T, class Policy>
inline T cyl_bessel_k_imp(T v, T x, const bessel_no_int_tag& /* t */, const Policy& pol)
{
   static const char* function = "boost::math::cyl_bessel_k<%1%>(%1%,%1%)";
   BOOST_MATH_STD_USING
   if(x < 0)
   {
      return policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but we need x > 0", x, pol);
   }
   if(x == 0)
   {
      return (v == 0) ? policies::raise_overflow_error<T>(function, 0, pol)
         : policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but we need x > 0", x, pol);
   }
   T I, K;
   bessel_ik(v, x, &I, &K, need_k, pol);
   return K;
}

template <class T, class Policy>
inline T cyl_bessel_k_imp(T v, T x, const bessel_maybe_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING
   if((floor(v) == v))
   {
      return bessel_kn(itrunc(v), x, pol);
   }
   return cyl_bessel_k_imp(v, x, bessel_no_int_tag(), pol);
}

template <class T, class Policy>
inline T cyl_bessel_k_imp(int v, T x, const bessel_int_tag&, const Policy& pol)
{
   return bessel_kn(v, x, pol);
}

template <class T, class Policy>
inline T cyl_neumann_imp(T v, T x, const bessel_no_int_tag&, const Policy& pol)
{
   static const char* function = "boost::math::cyl_neumann<%1%>(%1%,%1%)";
   if(x <= 0)
   {
      return (v == 0) && (x == 0) ?
         policies::raise_overflow_error<T>(function, 0, pol)
         : policies::raise_domain_error<T>(
               function,
               "Got x = %1%, but result is complex for x <= 0", x, pol);
   }
   T j, y;
   bessel_jy(v, x, &j, &y, need_y, pol);
   //
   // Post evaluation check for internal overflow during evaluation,
   // can occur when x is small and v is large, in which case the result
   // is -INF:
   //
   if(!(boost::math::isfinite)(y))
      return -policies::raise_overflow_error<T>(function, 0, pol);
   return y;
}

template <class T, class Policy>
inline T cyl_neumann_imp(T v, T x, const bessel_maybe_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING
   typedef typename bessel_asymptotic_tag<T, Policy>::type tag_type;
   if(floor(v) == v)
   {
      if((fabs(x) > asymptotic_bessel_y_limit<T>(tag_type())) && (fabs(x) > 5 * abs(v)))
      {
         T r = asymptotic_bessel_y_large_x_2(static_cast<T>(abs(v)), x);
         if((v < 0) && (itrunc(v, pol) & 1))
            r = -r;
         return r;
      }
      else
         return bessel_yn(itrunc(v, pol), x, pol);
   }
   return cyl_neumann_imp<T>(v, x, bessel_no_int_tag(), pol);
}

template <class T, class Policy>
inline T cyl_neumann_imp(int v, T x, const bessel_int_tag&, const Policy& pol)
{
   BOOST_MATH_STD_USING
   typedef typename bessel_asymptotic_tag<T, Policy>::type tag_type;
   if((fabs(x) > asymptotic_bessel_y_limit<T>(tag_type())) && (fabs(x) > 5 * abs(v)))
   {
      T r = asymptotic_bessel_y_large_x_2(static_cast<T>(abs(v)), x);
      if((v < 0) && (v & 1))
         r = -r;
      return r;
   }
   else
      return bessel_yn(v, x, pol);
}

template <class T, class Policy>
inline T sph_neumann_imp(unsigned v, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING // ADL of std names
   static const char* function = "boost::math::sph_neumann<%1%>(%1%,%1%)";
   //
   // Nothing much to do here but check for errors, and
   // evaluate the function's definition directly:
   //
   if(x < 0)
      return policies::raise_domain_error<T>(
         function,
         "Got x = %1%, but function requires x > 0.", x, pol);

   if(x < 2 * tools::min_value<T>())
      return -policies::raise_overflow_error<T>(function, 0, pol);

   T result = cyl_neumann_imp(T(v)+0.5f, x, bessel_no_int_tag(), pol);
   T tx = sqrt(constants::pi<T>() / (2 * x));

   if((tx > 1) && (tools::max_value<T>() / tx < result))
      return -policies::raise_overflow_error<T>(function, 0, pol);

   return result * tx;
}

} // namespace detail

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_bessel_j(T1 v, T2 x, const Policy& pol)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_j_imp<value_type>(v, static_cast<value_type>(x), tag_type(), pol), "boost::math::cyl_bessel_j<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_bessel_j(T1 v, T2 x)
{
   return cyl_bessel_j(v, x, policies::policy<>());
}

template <class T, class Policy>
inline typename detail::bessel_traits<T, T, Policy>::result_type sph_bessel(unsigned v, T x, const Policy& pol)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T, T, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::sph_bessel_j_imp<value_type>(v, static_cast<value_type>(x), pol), "boost::math::sph_bessel<%1%>(%1%,%1%)");
}

template <class T>
inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type sph_bessel(unsigned v, T x)
{
   return sph_bessel(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_bessel_i(T1 v, T2 x, const Policy& pol)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_i_imp<value_type>(v, static_cast<value_type>(x), pol), "boost::math::cyl_bessel_i<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_bessel_i(T1 v, T2 x)
{
   return cyl_bessel_i(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_bessel_k(T1 v, T2 x, const Policy& pol)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_bessel_k_imp<value_type>(v, static_cast<value_type>(x), tag_type(), pol), "boost::math::cyl_bessel_k<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_bessel_k(T1 v, T2 x)
{
   return cyl_bessel_k(v, x, policies::policy<>());
}

template <class T1, class T2, class Policy>
inline typename detail::bessel_traits<T1, T2, Policy>::result_type cyl_neumann(T1 v, T2 x, const Policy& pol)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T1, T2, Policy>::result_type result_type;
   typedef typename detail::bessel_traits<T1, T2, Policy>::optimisation_tag tag_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::cyl_neumann_imp<value_type>(v, static_cast<value_type>(x), tag_type(), pol), "boost::math::cyl_neumann<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename detail::bessel_traits<T1, T2, policies::policy<> >::result_type cyl_neumann(T1 v, T2 x)
{
   return cyl_neumann(v, x, policies::policy<>());
}

template <class T, class Policy>
inline typename detail::bessel_traits<T, T, Policy>::result_type sph_neumann(unsigned v, T x, const Policy& pol)
{
   BOOST_FPU_EXCEPTION_GUARD
   typedef typename detail::bessel_traits<T, T, Policy>::result_type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::sph_neumann_imp<value_type>(v, static_cast<value_type>(x), pol), "boost::math::sph_neumann<%1%>(%1%,%1%)");
}

template <class T>
inline typename detail::bessel_traits<T, T, policies::policy<> >::result_type sph_neumann(unsigned v, T x)
{
   return sph_neumann(v, x, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_BESSEL_HPP


#ifndef STAN_MATH_PRIM_SCAL_FUN_INC_BETA_HPP
#define STAN_MATH_PRIM_SCAL_FUN_INC_BETA_HPP

#include <boost/math/special_functions/beta.hpp>

namespace stan {
  namespace math {

    inline double inc_beta(double a,
                           double b,
                           double x) {
      using boost::math::ibeta;
      return ibeta(a, b, x);
    }

  }
}
#endif


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_SWAR_FUNCTION_SCALAR_COMPARATOR_HPP_INCLUDED
#define NT2_TOOLBOX_SWAR_FUNCTION_SCALAR_COMPARATOR_HPP_INCLUDED
#include <boost/fusion/tuple.hpp>

#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/any.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::comparator_, tag::cpu_,
                            (A0)(A1)(A2),
                            (arithmetic_<A0>)(arithmetic_<A1>)(arithmetic_<A2>)
                           )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::comparator_(tag::arithmetic_,tag::arithmetic_,tag::arithmetic_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A2>
    struct result<This(A0, A0, A2)>
    {
      typedef typename meta::strip<A0>::type           stA0;
      typedef boost::fusion::tuple<stA0,stA0,bool>     type;
    };

    NT2_FUNCTOR_CALL(3)
    {
      typename NT2_RETURN_TYPE(3)::type res;
      eval( a0, a1, a2
           , boost::fusion::at_c<0>(res)
           , boost::fusion::at_c<1>(res)
           , boost::fusion::at_c<2>(res)
           );
      return res;
    }
  private:
    template<class A0,class A2,class R0,class R1> inline void
    eval(A0 const& a0, A0 const& a1, A2 const& a2, R0& r0, R1& r1, bool& modified)const
    {
      r0 = a0;
      r1 = a1;
      if (a2)
      {
        if ( (modified = (a0 > a1)) ) std::swap(r0, r1);
      }
      else
      {
        if ( (modified = (a1 > a0)) ) std::swap(r0, r1);
      }
//       r0 =  nt2::min(a0, a1);
//       r1 =  nt2::max(a0, a1);
//       if (a2) {
//    A0 t =  r1;
//    r1 = r0;
//    r0 = t;
//       }
//       modified = any(a0-r0);
    }

  };
} }

#endif
// modified by jt the 26/12/2010


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"
#include "utilities.hpp"
#include <boost/test/unit_test.hpp>

#include <qle/termstructures/staticallycorrectedyieldtermstructure.hpp>

#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/nullcalendar.hpp>

#include <boost/make_shared.hpp>

using namespace QuantExt;
using namespace QuantLib;
using namespace boost::unit_test_framework;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(StaticallyCorrectedYieVldTermStructureTest)

BOOST_AUTO_TEST_CASE(testCorrectedYts) {

    SavedSettings backup;

    Date refDate(18, Feb, 2016);
    Settings::instance().evaluationDate() = refDate;

    boost::shared_ptr<SimpleQuote> q = boost::make_shared<SimpleQuote>(0.02);
    Handle<Quote> hq(q);
    Handle<YieldTermStructure> floating(boost::make_shared<FlatForward>(0, NullCalendar(), hq, Actual365Fixed()));

    Handle<YieldTermStructure> source(boost::make_shared<FlatForward>(refDate, 0.02, Actual365Fixed()));
    Handle<YieldTermStructure> target(boost::make_shared<FlatForward>(refDate, 0.03, Actual365Fixed()));

    // we can not test the flavours of roll down with flat
    // termstructures, todo add tests with non-flat ts for this

    Handle<YieldTermStructure> corrected1(
        boost::make_shared<StaticallyCorrectedYieldTermStructure>(floating, source, target, ConstantDiscounts));
    Handle<YieldTermStructure> corrected2(
        boost::make_shared<StaticallyCorrectedYieldTermStructure>(floating, source, target, ForwardForward));

    Real tol = 1.0E-12;

    BOOST_CHECK_MESSAGE(std::fabs(target->discount(1.0) - corrected1->discount(1.0)) < tol,
                        "can not verify corrected1 df ("
                            << corrected1->discount(1.0) << ") against target df (" << target->discount(1.0)
                            << ") on eval date " << Settings::instance().evaluationDate() << ", difference is "
                            << (corrected1->discount(1.0) - target->discount(1.0)) << ", tolerance is " << tol);
    BOOST_CHECK_MESSAGE(std::fabs(target->discount(1.0) - corrected2->discount(1.0)) < tol,
                        "can not verify corrected2 df ("
                            << corrected2->discount(1.0) << ") against target df (" << target->discount(1.0)
                            << ") on eval date " << Settings::instance().evaluationDate() << ", difference is "
                            << (corrected2->discount(1.0) - target->discount(1.0)) << ", tolerance is " << tol);

    // move market data
    q->setValue(0.03);

    BOOST_CHECK_MESSAGE(std::fabs(floating->discount(1.0) * target->discount(1.0) / source->discount(1.0) -
                                  corrected1->discount(1.0)) < tol,
                        "can not verify corrected1 df ("
                            << corrected1->discount(1.0) << ") against expected df ("
                            << floating->discount(1.0) * target->discount(1.0) / source->discount(1.0)
                            << ") on eval date " << Settings::instance().evaluationDate() << ", difference is "
                            << (corrected1->discount(1.0) -
                                floating->discount(1.0) * target->discount(1.0) / source->discount(1.0))
                            << ", tolerance is " << tol);
    BOOST_CHECK_MESSAGE(std::fabs(floating->discount(1.0) * target->discount(1.0) / source->discount(1.0) -
                                  corrected2->discount(1.0)) < tol,
                        "can not verify corrected2 df ("
                            << corrected2->discount(1.0) << ") against expected df ("
                            << floating->discount(1.0) * target->discount(1.0) / source->discount(1.0)
                            << ") on eval date " << Settings::instance().evaluationDate() << ", difference is "
                            << (corrected2->discount(1.0) -
                                floating->discount(1.0) * target->discount(1.0) / source->discount(1.0))
                            << ", tolerance is " << tol);

    // move forward in time
    Settings::instance().evaluationDate() = Date(18, Feb, 2022);
    Real t = Actual365Fixed().yearFraction(refDate, Settings::instance().evaluationDate());

    BOOST_CHECK_MESSAGE(std::fabs(floating->discount(1.0) * target->discount(1.0) / source->discount(1.0) -
                                  corrected1->discount(1.0)) < tol,
                        "can not verify corrected1 df ("
                            << corrected1->discount(1.0) << ") against expected df ("
                            << floating->discount(1.0) * target->discount(1.0) / source->discount(1.0)
                            << ") on eval date " << Settings::instance().evaluationDate() << ", difference is "
                            << (corrected1->discount(1.0) -
                                floating->discount(1.0) * target->discount(1.0) / source->discount(1.0))
                            << ", tolerance is " << tol);
    BOOST_CHECK_MESSAGE(std::fabs(floating->discount(1.0) * target->discount(t + 1.0) * source->discount(t) /
                                      (target->discount(t) * source->discount(t + 1.0)) -
                                  corrected2->discount(1.0)) < tol,
                        "can not verify corrected2 df ("
                            << corrected2->discount(1.0) << ") against expected df ("
                            << floating->discount(1.0) * target->discount(t + 1.0) * source->discount(t) /
                                   (target->discount(t) * source->discount(t + 1.0))
                            << ") on eval date " << Settings::instance().evaluationDate() << ", difference is "
                            << floating->discount(1.0) * target->discount(t + 1.0) * source->discount(t) /
                                       (target->discount(t) * source->discount(t + 1.0)) -
                                   corrected2->discount(1.0)
                            << ", tolerance is " << tol);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


// Copyright (C) 2018 Thejaka Amila Kanewala

// Boost Software License - Version 1.0 - August 17th, 2003

// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:

// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.

// 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, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

//  Authors: Thejaka Kanewala
/**************************************************************************
/* This source file implements the permutation algorithm described P. Sanders [1]
   in a distributed setting. This is a direct implementation is based on the
   algorithm described in [2].

[1] Sanders, Peter. "Random permutations on distributed, \
external and hierarchical memory." Information Processing Letters 67.6 (1998): 305-309.
[2] Langr, Daniel, et al. "Algorithm 947: Paraperm---Parallel Generation of
Random Permutations with MPI." ACM Transactions on Mathematical Software (TOMS) 41.1 (2014): 5.
*/

#include <mpi.h>
#include <cmath>
#include <vector>
#include <algorithm>

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#define SP_DATA_TYPE MPI_UNSIGNED_LONG

template<typename perm_t>
class sanders_permutation {

  typedef std::vector<perm_t> permute_vector_t;

public:
  // n - number to permute
  sanders_permutation(perm_t& pn):n(pn) {}

  //  N - total number of processors
  void permute(int N, permute_vector_t& p_out);

  void verify(int N, permute_vector_t& p_out);

private:
  perm_t& n;
  int rank;

#ifdef PRINT_DEBUG
  int debug_rank;
#endif

  void error(std::string mpifn, std::string desc) {
    std::cout << "[ERROR] Permuting numbers -- MPI function : "
	      << mpifn << ", description : " << desc
	      << std::endl;
  }

  void run_phase1(unsigned int count, perm_t pos, unsigned int N,
		  perm_t** temp, unsigned int& total);
  void run_phase2(perm_t** temp, unsigned int total);
  void run_phase3(perm_t* temp, unsigned int size,
		  unsigned int m,
		  perm_t pos,
		  unsigned int count,
		  permute_vector_t& perm);

};

#define SANDERS_PERM_PARAMS \
  typename perm_t

#define SANDERS_PERM_TYPE \
  sanders_permutation<perm_t>

template<SANDERS_PERM_PARAMS>
void
SANDERS_PERM_TYPE::verify(int N, permute_vector_t& p_out) {
  std::sort(p_out.begin(), p_out.end());
  typename permute_vector_t::iterator ite = p_out.begin();
  for (; ite != p_out.end(); ++ite) {
    //TODO
  }
}

template<SANDERS_PERM_PARAMS>
void
SANDERS_PERM_TYPE::permute(int N, permute_vector_t& p_out) {

  if (MPI_Comm_rank(MPI_COMM_WORLD, &rank) != 0)
    error("MPI_Comm_rank", "Error getting the rank");

#ifdef PRINT_DEBUG
  std::cout << "Current process rank : " << rank << std::endl;
#endif

  double j = (double)n/(double)N;

  unsigned int m = std::ceil(j);
  perm_t pos = rank * (perm_t)m;
  unsigned int count = m;

  //if (r + 1)m > n then count ← n − pos
  if (((rank+1)*m) > n)
    count = (n-pos);

  //if pos ≥ n then count ← 0
  if (pos >= n)
    count = 0;

  perm_t* temp;
  unsigned int sz = 0;

#ifdef PRINT_DEBUG
  std::cout << "r:" << rank << "m:" << m
	    << ", pos:" << pos << ", count:" << count
	    << std::endl;
#endif
  //  int* perm; // final output

  run_phase1(count, pos, N, &temp, sz);
  run_phase2(&temp, sz);

  unsigned int allocsz = m;
  if ((rank == (N-1)) &&
      count > 0)
    allocsz = count;

  p_out.resize(allocsz);

  run_phase3(temp, sz, m, pos, count, p_out);

  delete[] temp;

#ifdef PRINT_DEBUG
  if (rank == debug_rank) {
    std::cout << "@Rank " << rank << std::endl;
    for(unsigned int q=0; q < allocsz; ++q) {
      std::cout << p_out[q] << ",";
    }

    std::cout << std::endl;
  }
#endif

}

struct sort_indices
{
private:
  unsigned int* parr;
public:
  sort_indices(unsigned int* p_parr) : parr(p_parr) {}
  bool operator()(int i, int j) const { return parr[i]<parr[j]; }
};

template<SANDERS_PERM_PARAMS>
void
SANDERS_PERM_TYPE::run_phase1(unsigned int count, perm_t pos, unsigned int N,
			      perm_t** temp, unsigned int& total) {

  perm_t* sendbuf = new perm_t[count+1];
  perm_t* sortedsendbuf = new perm_t[count+1];
  unsigned int* destprocs = new unsigned int[count+1];
  unsigned int* indices = new unsigned int[count+1];

  // for random number generation
  boost::random::mt19937 gen;
  boost::random::uniform_int_distribution<> dist(0, (N-1));

  for (unsigned int k=0; k <= (count-1); ++k) {
    sendbuf[k] = pos+(perm_t)k;
    destprocs[k] = dist(gen);
    indices[k] = k;
  }

  sendbuf[count] = 0;
  destprocs[count] = N;
  indices[count] = count;

#ifdef PRINT_DEBUG
  std::cout << "printing unsorted ..." << std::endl;
  for (unsigned int i=0; i < (count+1); ++i) {
    std::cout << "(" << destprocs[i] << ","
	      << sendbuf[i] << "), ";
  }

  std::cout << std::endl;
#endif

  // sort arrays by rank
  std::sort(indices, (indices+count+1), sort_indices(destprocs));

  // copy sendbuf to sortedsendbuf
  for(unsigned int i=0; i < (count+1); ++i) {
    sortedsendbuf[i] = sendbuf[indices[i]];
  }

#ifdef PRINT_DEBUG
  if (rank == debug_rank) {
    std::cout << "printing sorted ..." << std::endl;
    for (unsigned int i=0; i < (count+1); ++i) {
      std::cout << "(" << destprocs[indices[i]] << ","
		<< sortedsendbuf[i] << "), ";
    }

    std::cout << std::endl;
  }
#endif

  // we do not need sendbuf now
  delete[] sendbuf;

  std::vector<int> sendcnts;
  sendcnts.resize(N, 0);

  int k = 0;
  for (unsigned int rp=0; rp <= (N-1); ++rp) {
    while(rp == destprocs[indices[k]]) {
      ++sendcnts[rp];
      ++k;
    }
  }

#ifdef PRINT_DEBUG
  if (rank == debug_rank) {
    std::cout << "printing sendcnts..." << std::endl;
    for (unsigned int i=0; i < N; ++i) {
      std::cout << sendcnts[i] << ", ";
    }

    std::cout << std::endl;
  }
#endif

  delete[] destprocs;
  delete[] indices;

  //calculate displacements
  std::vector<int> sdispls;
  sdispls.resize(N, 0);
  for (unsigned int hp = 1; hp <= (N-1); ++hp) {
    sdispls[hp] = sdispls[hp-1]+sendcnts[hp-1];
  }

#ifdef PRINT_DEBUG
  if (rank == debug_rank) {
    std::cout << "printing displacements..." << std::endl;
    for (unsigned int i=0; i < N; ++i) {
      std::cout << sdispls[i] << ", ";
    }

    std::cout << std::endl;
  }
#endif

  std::vector<int> recvcnts;
  recvcnts.resize(N,0);
  if (MPI_Alltoall(&sendcnts[0], 1, MPI_UNSIGNED,
		   &recvcnts[0], 1, MPI_UNSIGNED, MPI_COMM_WORLD) != 0) {
    error("MPI_Alltoall", "Error exchanging send counts and receive counts in phase 1");
  }

#ifdef PRINT_DEBUG
  if (rank == debug_rank) {
    std::cout << "printing recev..." << std::endl;
    for (unsigned int i=0; i < N; ++i) {
      std::cout << recvcnts[i] << ", ";
    }

    std::cout << std::endl;
  }
#endif

  std::vector<int> rdispls;
  rdispls.resize(N, 0);
  for(unsigned int rp=1; rp <= (N-1); ++rp) {
    rdispls[rp] = rdispls[rp-1]+recvcnts[rp-1];
  }

#ifdef PRINT_DEBUG
  if (rank == debug_rank) {
    std::cout << "printing rdispls..." << std::endl;
    for (unsigned int i=0; i < N; ++i) {
      std::cout << rdispls[i] << ", ";
    }

    std::cout << std::endl;
  }
#endif

  for (unsigned int rp=0; rp <= (N-1); ++rp) {
    total += recvcnts[rp];
  }

#ifdef PRINT_DEBUG
  std::cout << "r:" << rank << "total : " << total << std::endl;
#endif

  (*temp) = new perm_t[total];
  //std::vector<perm_t> temp;
  //temp.resize(total);

  if (MPI_Alltoallv(&sortedsendbuf[0], &sendcnts[0], &sdispls[0], SP_DATA_TYPE,
			     (*temp), &recvcnts[0], &rdispls[0], SP_DATA_TYPE,
		    MPI_COMM_WORLD) != 0)
    error("MPI_Alltoallv", "Error exchanging permuted values in phase 1");

#ifdef PRINT_DEBUG
  if (rank == debug_rank) {
    for(unsigned int q=0; q < total; ++q) {
      std::cout << (*temp)[q] << ",";
    }
  }
  std::cout << std::endl;
#endif

  delete[] sortedsendbuf;

  if (MPI_Barrier(MPI_COMM_WORLD) != 0)
    error("MPI_Barrier", "Error synchronizing processes in phase 1");
}

template<SANDERS_PERM_PARAMS>
void
SANDERS_PERM_TYPE::run_phase2(perm_t** temp, unsigned int total) {
  boost::random::mt19937 gen;

  if (total > 1) {
    for (int k=(total-1); k >=1; --k) {
      boost::random::uniform_int_distribution<> dist(0, k);
      int l = dist(gen);
      int t = (*temp)[k];
      (*temp)[k] = (*temp)[l];
      (*temp)[l] = t;
    }
  }

  if (MPI_Barrier(MPI_COMM_WORLD) != 0)
    error("MPI_Barrier", "Error synchronizing processes in phase 2");

#ifdef PRINT_DEBUG
  std::cout << "printing after local permuation " << std::endl;
  if (rank == 1) {
    for(unsigned int q=0; q < total; ++q) {
      std::cout << (*temp)[q] << ",";
    }
  }
  std::cout << std::endl;
#endif

}

template<SANDERS_PERM_PARAMS>
void
SANDERS_PERM_TYPE::run_phase3(perm_t* temp, unsigned int sz,
			      unsigned int m,
			      perm_t pos,
			      unsigned int count,
			      permute_vector_t& perm) {

  perm_t size = (perm_t)sz;
  perm_t first;

  if (MPI_Scan(&size, &first, 1,
	       SP_DATA_TYPE, MPI_SUM, MPI_COMM_WORLD) != 0)
    error("MPI_Scan", "Error getting prefix sums in phase 3");

#ifdef PRINT_DEBUG
  std::cout << "rank : " << rank << " first : " << first << std::endl;
#endif

  first = first - size;
  perm_t last = first + size - 1;
  double fmdiv = (double)first / (double)m;
  unsigned int rp = std::floor(fmdiv);
  perm_t firstp = first;
  unsigned int remains = count;

  std::vector<MPI_Request> requests;
  MPI_Request request;
  perm_t* buf = new perm_t[2];

#ifdef PRINT_DEBUG
  if (rank == debug_rank) {
    std::cout << "firstp:" << firstp << ", last:" << last
	      << ", remains:" << remains << ", rp:" << rp
	      << ", pos:" << pos
	      << ", first:" << first
	      << std::endl;
  }
#endif

  do {
    perm_t lastp = (rp+1)*m - 1;
    if (lastp > last)
      lastp = last;

    unsigned int countp = lastp-firstp+1;
    if (rank == (int)rp) {
      for (unsigned int k=firstp; k<=lastp; ++k) {
	perm[k-pos]=temp[k-first];
      }

      remains -= countp;
    } else {
      buf[0] = firstp;
      buf[1] = (perm_t)countp;

      if (MPI_Isend(&buf[0], 2, SP_DATA_TYPE, rp, 1,
		    MPI_COMM_WORLD, &request) != 0)
	error("MPI_Isend", "Error exchanging first and last values in phase 3");

      requests.push_back(request);

      if (MPI_Isend(&temp[firstp-first], countp, SP_DATA_TYPE, rp, 2,
		    MPI_COMM_WORLD, &request) != 0)
	error("MPI_Isend", "Error sending surplus elements to others in phas 3");

      requests.push_back(request);
    }

    rp += 1;
    firstp += countp;
#ifdef PRINT_DEBUG
    if (rank == debug_rank) {
      std::cout << "rp: " << rp << "firstp:" << firstp << std::endl;
    }
#endif
  }while(firstp < last);

#ifdef PRINT_DEBUG
  if (rank == debug_rank)
    std::cout << "remains 1: " << remains << std::endl;
#endif

  while(remains > 0) {
    MPI_Status status;
    if (MPI_Recv(&buf[0], 2, SP_DATA_TYPE, MPI_ANY_SOURCE, 1,
		 MPI_COMM_WORLD, &status)!=0)
      error("MPI_Recv", "Error while receiving first and last values in phase 3");

    firstp = buf[0];
    unsigned int countp = buf[1];

    if (MPI_Recv(&perm[firstp-pos], countp, SP_DATA_TYPE,
		 status.MPI_SOURCE, 2, MPI_COMM_WORLD, &status) != 0)
            error("MPI_Recv", "Error while receiving additional values in phase 3");

    remains -= countp;
#ifdef PRINT_DEBUG
    if (rank == debug_rank)
      std::cout << "remains : " << remains << std::endl;
#endif
  }

  MPI_Status status;
  for (unsigned int k=0; k < requests.size(); ++k) {
    if (MPI_Wait(&requests[k], &status) != 0)
      error("MPI_Wait", "Error waiting for requests in phase 3");
  }

  requests.clear();
  delete[] buf;

  if (MPI_Barrier(MPI_COMM_WORLD) !=0)
    error("MPI_Barrier", "Error invoking barrier in phase 3");

}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/significants.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>

STF_CASE_TPL (" significants",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::significants;
  using iT =  bd::as_integer_t<T>;
  using r_t = decltype(significants(T(), iT()));

  // return type conformity test
  STF_TYPE_IS( r_t, T );

#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(significants(bs::Inf<T>(), 1), bs::Inf<r_t>(), 0.5);
  STF_ULP_EQUAL(significants(bs::Minf<T>(), 1), bs::Minf<r_t>(), 0.5);
  STF_ULP_EQUAL(significants(bs::Nan<T>(), 1), bs::Nan<r_t>(), 0.5);
#endif
  STF_ULP_EQUAL(significants(T(0), 1), T(0), 0.5);
  STF_ULP_EQUAL(significants(T(25.34), 1), T(30), 0.5);
  STF_ULP_EQUAL(significants(T(25.34), 2), T(25), 0.5);
  STF_ULP_EQUAL(significants(T(25.34), 3), T(25.3), 0.5);
  STF_ULP_EQUAL(significants(T(25.34), 4), T(25.34), 0.5);
  STF_ULP_EQUAL(significants(T(-25.34), 1), T(-30), 0.5);
  STF_ULP_EQUAL(significants(T(-25.34), 2), T(-25), 0.5);
  STF_ULP_EQUAL(significants(T(-25.34), 3), T(-25.3), 0.5);
  STF_ULP_EQUAL(significants(T(-25.34), 4), T(-25.34), 0.5);
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/preprocessor/list/for_each.hpp>
#include <boost/preprocessor/tuple/to_list.hpp>
#include <limits>

#include "DataStructures/Tensor/TypeAliases.hpp"
#include "Options/Options.hpp"
#include "PointwiseFunctions/AnalyticSolutions/AnalyticSolution.hpp"
#include "PointwiseFunctions/GeneralRelativity/Tags.hpp"  // IWYU pragma: keep
#include "PointwiseFunctions/Hydro/EquationsOfState/PolytropicFluid.hpp"  // IWYU pragma: keep
#include "PointwiseFunctions/Hydro/TagsDeclarations.hpp"  // IWYU pragma: keep
#include "Utilities/MakeWithValue.hpp"
#include "Utilities/TMPL.hpp"
#include "Utilities/TaggedTuple.hpp"

/// \cond
namespace PUP {
class er;  // IWYU pragma: keep
}  // namespace PUP
/// \endcond

// IWYU pragma: no_include <pup.h>

// IWYU pragma: no_include "Utilities/GenerateInstantiations.hpp"

namespace RelativisticEuler {
namespace Solutions {

/*!
 * \brief A static spherically symmetric star
 *
 * An analytic solution for a static, spherically-symmetric star found by
 * solving the Tolman-Oppenheimer-Volkoff (TOV) equations.  The equation of
 * state is assumed to be that of a polytropic fluid.
 *
 * \tparam RadialSolution selects not only how the TOV equations are solved, but
 * also the radial coordinate that the solution is given in (e.g. areal or
 * isotropic).  See the documentation of the specific `RadialSolution`s for
 * more details.
 */
template <typename RadialSolution>
class TovStar : public MarkAsAnalyticSolution {
 public:
  /*!
   * \brief The radial variables needed to compute the full `TOVStar` solution
   *
   * RadialSolution must provide a method that fills the radial variables
   * given the equation of state and the Cartesian coordinates \f$x^i\f$
   *
   * If the spherically symmetric metric is written as
   *
   * \f[
   * ds^2 = - e^{2 \Phi_t} dt^2 + e^{2 \Phi_r} dr^2 + e^{2 \Phi_\Omega} r^2
   * d\Omega^2
   * \f]
   *
   * where \f$r = \delta_{mn} x^m x^n\f$ is the `radial_coordinate` and
   * \f$\Phi_t\f$, \f$\Phi_r\f$, and \f$\Phi_\Omega\f$ are respectvely the
   * `metric_time_potential`, `metric_radial_potential`, and
   * `metric_angular_potential`, then the lapse, shift, and spatial metric in
   * the Cartesian coordinates are
   *
   * \f{align*}
   * \alpha &= e^{\Phi_t} \\
   * \beta^i &= 0 \\
   * \gamma_{ij} &= \delta_{ij} e^{2 \Phi_r} + \delta_{im} \delta_{jn}
   * \frac{x^m x^n}{r^2} \left( e^{2 \Phi_r} - e^{2 \Phi_\Omega} \right)
   * \f}
   *
   */
  template <typename DataType>
  struct RadialVariables {
    explicit RadialVariables(DataType radial_coordinate_in)
        : radial_coordinate(std::move(radial_coordinate_in)),
          rest_mass_density(
              make_with_value<Scalar<DataType>>(radial_coordinate, 0.0)),
          pressure(make_with_value<Scalar<DataType>>(radial_coordinate, 0.0)),
          specific_internal_energy(
              make_with_value<Scalar<DataType>>(radial_coordinate, 0.0)),
          specific_enthalpy(
              make_with_value<Scalar<DataType>>(radial_coordinate, 0.0)),
          metric_time_potential(
              make_with_value<DataType>(radial_coordinate, 0.0)),
          dr_metric_time_potential(
              make_with_value<DataType>(radial_coordinate, 0.0)),
          metric_radial_potential(
              make_with_value<DataType>(radial_coordinate, 0.0)),
          dr_metric_radial_potential(
              make_with_value<DataType>(radial_coordinate, 0.0)),
          metric_angular_potential(
              make_with_value<DataType>(radial_coordinate, 0.0)),
          dr_metric_angular_potential(
              make_with_value<DataType>(radial_coordinate, 0.0)) {}
    DataType radial_coordinate{};
    Scalar<DataType> rest_mass_density{};
    Scalar<DataType> pressure{};
    Scalar<DataType> specific_internal_energy{};
    Scalar<DataType> specific_enthalpy{};
    DataType metric_time_potential{};
    DataType dr_metric_time_potential{};
    DataType metric_radial_potential{};
    DataType dr_metric_radial_potential{};
    DataType metric_angular_potential{};
    DataType dr_metric_angular_potential{};
  };

  using equation_of_state_type = EquationsOfState::PolytropicFluid<true>;

  /// The central density of the star.
  struct CentralDensity {
    using type = double;
    static constexpr Options::String help = {
        "The central density of the star."};
    static type lower_bound() noexcept { return 0.; }
  };

  /// The polytropic constant of the polytropic fluid.
  struct PolytropicConstant {
    using type = double;
    static constexpr Options::String help = {
        "The polytropic constant of the fluid."};
    static type lower_bound() noexcept { return 0.; }
  };

  /// The polytropic exponent of the polytropic fluid.
  struct PolytropicExponent {
    using type = double;
    static constexpr Options::String help = {
        "The polytropic exponent of the fluid."};
    static type lower_bound() noexcept { return 1.; }
  };

  using options =
      tmpl::list<CentralDensity, PolytropicConstant, PolytropicExponent>;

  static constexpr Options::String help = {
      "A static, spherically-symmetric star found by solving the \n"
      "Tolman-Oppenheimer-Volkoff (TOV) equations, with a given central \n"
      "density and polytropic fluid."};

  TovStar() = default;
  TovStar(const TovStar& /*rhs*/) = delete;
  TovStar& operator=(const TovStar& /*rhs*/) = delete;
  TovStar(TovStar&& /*rhs*/) noexcept = default;
  TovStar& operator=(TovStar&& /*rhs*/) noexcept = default;
  ~TovStar() = default;

  TovStar(double central_rest_mass_density, double polytropic_constant,
          double polytropic_exponent) noexcept;

  /// Retrieve a collection of variables at `(x, t)`
  template <typename DataType, typename... Tags>
  tuples::TaggedTuple<Tags...> variables(const tnsr::I<DataType, 3>& x,
                                         const double /*t*/,
                                         tmpl::list<Tags...> /*meta*/) const
      noexcept {
    auto radial_vars =
        radial_tov_solution().radial_variables(equation_of_state_, x);
    return {get<Tags>(variables(x, tmpl::list<Tags>{}, radial_vars))...};
  }

  // clang-tidy: no runtime references
  void pup(PUP::er& /*p*/) noexcept;  //  NOLINT

  const EquationsOfState::PolytropicFluid<true>& equation_of_state() const
      noexcept {
    return equation_of_state_;
  }

 private:
  template <typename LocalRadialSolution>
  friend bool operator==(const TovStar<LocalRadialSolution>& lhs,
                         const TovStar<LocalRadialSolution>& rhs) noexcept;

  const RadialSolution& radial_tov_solution() const noexcept;

  template <typename DataType>
  using SpatialVelocity = hydro::Tags::SpatialVelocity<DataType, 3>;

  template <typename DataType>
  using MagneticField = hydro::Tags::MagneticField<DataType, 3>;

  template <typename DataType>
  using DerivLapse = ::Tags::deriv<gr::Tags::Lapse<DataType>, tmpl::size_t<3>,
                                   Frame::Inertial>;

  template <typename DataType>
  using Shift = gr::Tags::Shift<3, Frame::Inertial, DataType>;

  template <typename DataType>
  using DerivShift =
      ::Tags::deriv<gr::Tags::Shift<3, Frame::Inertial, DataType>,
                    tmpl::size_t<3>, Frame::Inertial>;

  template <typename DataType>
  using SpatialMetric = gr::Tags::SpatialMetric<3, Frame::Inertial, DataType>;

  template <typename DataType>
  using DerivSpatialMetric =
      ::Tags::deriv<gr::Tags::SpatialMetric<3, Frame::Inertial, DataType>,
                    tmpl::size_t<3>, Frame::Inertial>;

  template <typename DataType>
  using InverseSpatialMetric =
      gr::Tags::InverseSpatialMetric<3, Frame::Inertial, DataType>;

  template <typename DataType>
  using ExtrinsicCurvature =
      gr::Tags::ExtrinsicCurvature<3, Frame::Inertial, DataType>;

#define FUNC_DECL(r, data, elem)                                \
  template <typename DataType>                                  \
  tuples::TaggedTuple<elem> variables(                          \
      const tnsr::I<DataType, 3>& x, tmpl::list<elem> /*meta*/, \
      const RadialVariables<DataType>& radial_vars) const noexcept;

#define MY_LIST                                                                \
  BOOST_PP_TUPLE_TO_LIST(                                                      \
      17, (hydro::Tags::RestMassDensity<DataType>,                             \
           hydro::Tags::SpecificInternalEnergy<DataType>,                      \
           hydro::Tags::Pressure<DataType>, SpatialVelocity<DataType>,         \
           MagneticField<DataType>,                                            \
           hydro::Tags::DivergenceCleaningField<DataType>,                     \
           hydro::Tags::LorentzFactor<DataType>,                               \
           hydro::Tags::SpecificEnthalpy<DataType>, gr::Tags::Lapse<DataType>, \
           DerivLapse<DataType>, Shift<DataType>, DerivShift<DataType>,        \
           SpatialMetric<DataType>, DerivSpatialMetric<DataType>,              \
           gr::Tags::SqrtDetSpatialMetric<DataType>,                           \
           InverseSpatialMetric<DataType>, ExtrinsicCurvature<DataType>))

  BOOST_PP_LIST_FOR_EACH(FUNC_DECL, _, MY_LIST)
#undef MY_LIST
#undef FUNC_DECL

  double central_rest_mass_density_ =
      std::numeric_limits<double>::signaling_NaN();
  double polytropic_constant_ = std::numeric_limits<double>::signaling_NaN();
  double polytropic_exponent_ = std::numeric_limits<double>::signaling_NaN();
  EquationsOfState::PolytropicFluid<true> equation_of_state_{};
};

template <typename RadialSolution>
bool operator!=(const TovStar<RadialSolution>& lhs,
                const TovStar<RadialSolution>& rhs) noexcept;

}  // namespace Solutions
}  // namespace RelativisticEuler


/***********************************************************
//              copyright Qingfeng Xia, 2020
// 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 <iostream>
#include "demo.h"

enum E {E_0=0, E_9bit = 0x100, E_17bit = 0x10000, E_all_one=0xffffffff};
enum class TE{TE_0 =0, TE_9bit = 0x100, TE_17bit = 0x10000};
// TE_all_one=0xffffffff for strong typed enum

template<typename SourceType>
void test_enum(std::string fname, SourceType enum_value)
{
    std::cout << "=========" << "enum" << "==============\n";
    try
    {
        unsigned char uc = std::to_unsigned<unsigned char>(enum_value);
    }
    catch(std::overflow_error e)
    {
        std::cout << fname << "(" << type_name<SourceType>() << ") has error : " << e.what() << '\n';
    }
}

void test_byte()
{
#if __cplusplus >= 201703L
    std::cout << "=========" << "std::byte" << "==============\n";
    // implicitly conversion by assignment is not allowed from int to std::byte
    std::byte bb = static_cast<std::byte>(1);
    std::byte b{42};
    std::cout << std::to_integer<int>(b) << "\n";
    //std::cout << std::to_integer<int, std::byte>(std::byte{40}) << "\n";
    //std::numeric_limits<T> does not support `std::byte`
    //std::byte bc = std::to_unsigned<std::byte>(1);
#endif
}

/// static_cast without messsage as the second parameter is only for C++17
void test_type_traits()
{
#if ! defined(_MSC_VER) || _MSC_VER >= 1900
    static_assert(std::is_same<int, int32_t>::value, "");

    // bool to unsigned or signal int are safe, is that must be
    static_assert(static_cast<int>(true) == 1, "");
    // int(true) == 1 failed on g++ 5

    //static_assert(std::is_same<long, long long>::value); // failed!
    // static_assert(std::is_same<int64_t, long>::value);
    // failed on macos 'std::is_same<long long, long>::value'

    //static_assert(std::is_same<int64_t, long>::value); // failed on macos
    static_assert(! std::numeric_limits<int>::has_infinity, "");

    static_assert(std::is_signed<double>::value, ""); // true for floating point
    //static_assert(std::is_signed<E>::value, ""); // failed for enum
    //static_assert(std::is_signed<bool>::value, ""); // failed for bool
    static_assert(std::is_unsigned<bool>::value, ""); // true for bool
    static_assert(std::is_integral<bool>::value, ""); // true for bool

    //static_assert(std::is_same<int*, void*>::value, "");

    static_assert(! std::is_arithmetic<int&>::value, "");
    static_assert(std::is_integral<char16_t>::value, ""); // true
    static_assert(std::is_unsigned<char16_t>::value, ""); // true
    //static_assert(std::is_same<char16_t, uint16_t>::value, "");
    //static_assert(std::is_same<char16_t, int16_t>::value, "");
#endif
}

#include "third-party/half.hpp"

void test_half()
{
    std::cout << "=========" << "half float" << "==============\n";
    using namespace half_float;
    half h1 = std::numeric_cast<half>(100);
    static_assert(std::is_numeric<half>::value, "");

    int8_t v1 = std::to_integer<int8_t>(half{1});
    try{
        int8_t v1 = std::to_integer<int8_t>(half{1000});
    }
    catch(std::runtime_error e)
    {
        std::cout << "`std::to_integer<int8_t>(half{1000})` error : " << e.what() << '\n';
    }
}

#if USE_BOOST_MULTIPRECISION
#include <boost/multiprecision/cpp_int.hpp>

// this  may shared the general test_conversion()
void test_boost_multiprecision()
{
   using namespace boost::multiprecision;

    std::cout << "=========" << "boost::multiprecision" << "==============\n";
   static_assert(std::detail::supports_arithmetic_operations<int128_t>::value, "");
   int128_t v = 1;
   //static_assert(std::is_arithmetic<int128_t>::value, "");

    int128_t i = std::numeric_cast<int128_t>(1);
    //int128_t i = std::numeric_cast<int128_t>(1.23);  // compiling error
    try{
        unsigned char uc = std::to_unsigned<unsigned char>(int128_t{1000});
        std::cout << "boost::multiprecision it128_t converted to ubyte " << uc << "\n";
    }
    catch(std::overflow_error e)
    {
        std::cout << " to_unsigned(boost_mp) error : " << e.what() << '\n';
    }

}
#endif

void test_implicit_conversion()
{
    const unsigned char uc = 1000.0;  // narrowing
    if(uc == UCHAR_MAX)
    {
        std::cout << "big floating point value truncated to UCHAR_MAX = "
        << UCHAR_MAX << std::endl;
    }

    const unsigned char uc1 = 500;  // narrowing from int to uint8_t
    std::cout << "const unsigned char uc1 = 500 gives uc1 = " << int(uc1) << std::endl;
    if(uc1 == UCHAR_MAX)
    {
        std::cout << "big integer value truncated to UCHAR_MAX = "
        << UCHAR_MAX << std::endl;
    }
}

void test_qualifier()
{
    /// volatile (deprecated) and reference
    volatile int i_value = 1;
    unsigned char uc_v = std::to_integer<unsigned char>(i_value);
    volatile int& i_ref = i_value;
    unsigned char uc_vref = std::to_integer<unsigned char>(i_ref);

    /// const reference
    const int c_value = 1;
    unsigned char uc_c = std::to_integer<unsigned char>(c_value);
    const int& c_ref = c_value;
    unsigned char uc_cref = std::to_integer<unsigned char>(c_ref);
}

int main()
{
    std::cout << std::boolalpha;

    test_conversion<double, unsigned int>("to_unsigned");
    test_conversion<double, uint64_t>("to_integer");
    test_conversion<double, int64_t>("numeric_cast");
    test_conversion<int64_t, bool>("to_integer");
    //test_conversion<int64_t, std::byte>("to_unsigned");
    test_conversion<int32_t, char16_t>("to_integer");

    float f = std::numeric_cast<float>(2);  // OK
    test_conversion<int, float>("numeric_cast");  // error, called `to_integer`
    test_conversion<uint32_t, int16_t>("to_integer");

    // todo: make test_enum a template function
    //test_enum("to_integer", E_9bit);
    test_enum("to_integer", TE::TE_9bit);
    //E e_cast = std::to_unsigned<E>(TE::TE_9bit);  // runtime_error error, why?

    test_byte();
    test_half();
    test_conversion<half_float::half, char16_t>("to_unsigned");
    test_conversion<char32_t, half_float::half>("numeric_cast");

#if USE_BOOST_MULTIPRECISION
    test_boost_multiprecision();  // not working
    //test_conversion<boost::multiprecision::int128_t, char16_t>("to_integer");
#endif
    test_implicit_conversion();

    static_assert(std::detail::_is_numeric<int>::value, "");
    test_qualifier();

    std::cout << "============= demo completed ============\n";
    return 0;

}


#include "sensors/sensor-extrinsics.h"

#include <random>

#include <Eigen/Core>
#include <aslam/common/memory.h>
#include <aslam/common/pose-types.h>
#include <aslam/common/yaml-serialization.h>
#include <glog/logging.h>
#include <maplab-common/macros.h>
#include <maplab-common/yaml-file-serializable.h>

namespace vi_map {

Extrinsics::Extrinsics() : extrinsics_type_(ExtrinsicsType::kInvalid) {}

Extrinsics::UniquePtr Extrinsics::createFromYaml(const YAML::Node& yaml_node) {
  Extrinsics::UniquePtr extrinsics(new Extrinsics());
  if (!extrinsics->deserialize(yaml_node)) {
    LOG(ERROR) << "YAML deserialization failed.";
    extrinsics.release();
  }
  return extrinsics;
}

bool Extrinsics::deserialize(const YAML::Node& yaml_node) {
  std::string type_as_string;
  if (YAML::safeGet(
          yaml_node, static_cast<std::string>(kYamlFieldNameExtrinsicsType),
          &type_as_string)) {
    CHECK(!type_as_string.empty());
    extrinsics_type_ = stringToExtrinsicsType(type_as_string);
  } else {
    LOG(ERROR) << "Unable to find the extrinsics type.";
    return false;
  }
  CHECK(extrinsics_type_ != ExtrinsicsType::kInvalid);

  Eigen::Matrix4d T_R_S_matrix;
  if (!YAML::safeGet(
          yaml_node, static_cast<std::string>(kYamlFieldName_T_R_S),
          &T_R_S_matrix)) {
    LOG(ERROR) << "Unable to find the T_R_S node.";
    return false;
  }
  T_R_S_ = aslam::Transformation(T_R_S_matrix);
  return true;
}

Extrinsics::UniquePtr Extrinsics::createRandomExtrinsics() {
  constexpr int kNumExtrinsicsTypes =
      static_cast<int>(ExtrinsicsType::kInvalid);
  CHECK_GT(kNumExtrinsicsTypes, 0);

  std::default_random_engine random_engine;
  std::uniform_int_distribution<int> extrinsics_type_distribution(
      0, kNumExtrinsicsTypes);
  const int extrinsics_type_int = extrinsics_type_distribution(random_engine);
  CHECK_GE(extrinsics_type_int, 0);
  CHECK_LT(extrinsics_type_int, kNumExtrinsicsTypes);

  const ExtrinsicsType extrinsics_type =
      static_cast<ExtrinsicsType>(extrinsics_type_int);

  aslam::Transformation T_R_S;
  T_R_S.setRandom();
  if (extrinsics_type == ExtrinsicsType::kPositionOnly) {
    T_R_S.getRotation().setIdentity();
  }
  Extrinsics::UniquePtr extrinsics =
      aligned_unique<Extrinsics>(extrinsics_type, T_R_S);
  return extrinsics;
}

}  // namespace vi_map


#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <vector>
#include <fstream>
#include <string>
#include <time.h>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>

using namespace std;
using namespace gtsam;
using namespace boost::algorithm;

using symbol_shorthand::X;

// Testing params
const size_t max_loop_count = 8000; //200 //2000 //8000

const bool active_all_loop_detach = false;
//const bool active_all_loop_detach = true;

noiseModel::Diagonal::shared_ptr prior_noise_model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.0001, 0.0001, 0.0001).finished());

noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0/50.0, 1.0/50.0, 1.0/100.0).finished());

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

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

  //ifstream in("../data/mh_T1_city10000_04.txt"); //Type #1 only
  //ifstream in("../data/mh_T3b_city10000_10.txt"); //Type #3 only
  ifstream in("../data/mh_T1_T3_city10000_04.txt"); //Type #1 + Type #3

  size_t pose_count = 0;
  size_t loop_count = 0;

  std::list<double> time_list;

  ISAM2Params parameters;
  MHParams mh_parameters(10, 30); //T1: (10, 30), T3: (10, 10)

  mh_parameters.isPrintPruningDetails = true;

  //parameters.optimizationParams = gtsam::ISAM2DoglegParams(0.1);  //_initialDelta = 1.0 //Dogleg NOT implemented yet!!!!
  parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0); //_wildfireThreshold = 0.001

  parameters.relinearizeThreshold = 0.01;

  parameters.relinearizeSkip = 1;
  MHISAM2* mh_isam2 = new MHISAM2(parameters, mh_parameters);

  NonlinearFactorGraph* mh_graph = new NonlinearFactorGraph();

  Values mh_init_values;
  Values mh_results;

  double x = 0.0;
  double y = 0.0;
  double rad = 0.0;

  std::vector<Pose2> prior_arr;
  prior_arr.push_back(Pose2(x, y, rad));

  MHGenericValue<Pose2> prior_value(prior_arr[0], mh_isam2->getFirstHypoLayer());

  mh_init_values.mhInsert(X(0), prior_value);
  pose_count++;

  mh_graph->add(MHPriorFactor<Pose2>(X(0), prior_arr, prior_noise_model));

  mh_isam2->update(*mh_graph, mh_init_values);
  mh_graph->resize(0);
  mh_init_values.clear();
  mh_results = mh_isam2->mhCalculateBestEstimate();

  //*
  size_t key_s = 0;
  size_t key_t = 0;

  clock_t start_time = clock();

  string str;
  while (getline(in, str) && loop_count < max_loop_count) {

    //cout << str << endl;
    vector<string> parts;
    split(parts, str, is_any_of(" "));

    key_s = stoi(parts[1]);
    key_t = stoi(parts[3]);
    int empty = stoi(parts[4]); // 0 or 1
    bool allow_empty = (empty == 0) ? false : true; // 0 or 1
    int m_num = stoi(parts[5]);
    vector<double> x_arr(m_num);
    vector<double> y_arr(m_num);
    vector<double> rad_arr(m_num);
    for (int i = 0; i < m_num; ++i) {
      x_arr[i] = stod(parts[6 + 3*i]);
      y_arr[i] = stod(parts[7 + 3*i]);
      rad_arr[i] = stod(parts[8 + 3*i]);
    }

    std::vector<Pose2> odom_arr;
    for (int i = 0; i < m_num; ++i) {
      odom_arr.push_back(Pose2(x_arr[i], y_arr[i], rad_arr[i]));
    }

    //flags: m_num, is_loop, is_detach (for loop only)
    const bool is_loop_detach = (active_all_loop_detach || allow_empty) && (key_s != key_t - 1); //active && loop

    if (key_s == key_t - 1) { //odom
      if (m_num > 1) { //MH
        mh_isam2->createNextHypoLayer(odom_arr.size(), 3); //mhsiao: should get dim from factor type
      }
    } else { //loop
      if (is_loop_detach) { //SG/MH w/ detach = MH
        mh_isam2->createNextHypoLayer(odom_arr.size(), 3, true, true); //mhsiao: should get dim from factor type
      } else if (m_num > 1) { //MH w/o detach
        mh_isam2->createNextHypoLayer(odom_arr.size(), 3, true, false); //mhsiao: should get dim from factor type
      } else { //SG
        mh_isam2->accumulateCommonDim(3);
      }
    }

    if (key_s == key_t - 1) { //new X(key)
      MHGenericValue<Pose2> pose_value(mh_results.mhAt<Pose2>(X(key_s)), odom_arr, mh_isam2->getLastHypoLayer() );
      mh_init_values.mhInsert(X(key_t), pose_value);
      pose_count++;
    } else { //loop
      if (m_num > 1 || is_loop_detach) {
        mh_isam2->setVariableToExpand(X(key_t), mh_isam2->getLastHypoLayer());
      }
      loop_count++;
    }

    if (is_loop_detach) { //loop w/ detach
      mh_graph->add(MHBetweenFactor<Pose2>(X(key_s), X(key_t), odom_arr, pose_noise_model, true));
    } else { //loop w/o detach || NOT loop
      mh_graph->add(MHBetweenFactor<Pose2>(X(key_s), X(key_t), odom_arr, pose_noise_model));
    }

    if (m_num > 1 || is_loop_detach) {
      mh_isam2->assocLatestHypoLayerWith(mh_graph->back());
    }

    mh_isam2->update(*mh_graph, mh_init_values);
    mh_graph->resize(0);
    mh_init_values.clear();
    mh_results = mh_isam2->mhCalculateBestEstimate();

    //*
    if (loop_count%50 == 0 && key_s != key_t - 1) {
       std::cout << "loop_count: " << loop_count << std::endl;

       //MHISAM2::HypoList& curr_hypo_list = mh_isam2->getLastHypoLayer()->getNodeList();
       //std::cout << "Last layer hypos:  " << curr_hypo_list.size() << std::endl;

       std::cout << "acc_time:  " << time_list.back() << std::endl;
    }
    // */

    if (key_s == key_t - 1) {
      clock_t cur_time = clock();
      time_list.push_back(cur_time - start_time);
    }

    if (time_list.size()%100 == 0 && (key_s == key_t - 1)) {
      string step_file_idx = std::to_string(100000 + time_list.size());

      ofstream step_outfile;
      string step_file_name = "mh_step_files/MH_ISAM2_TEST_city10000_S" + step_file_idx;
      step_outfile.open(step_file_name + ".txt");

      MHISAM2::HypoListIter it = mh_isam2->getLastHypoLayer()->getNodeList().begin();
      for (size_t i = 0; i < (pose_count); ++i) {
        Pose2 out_pose = mh_results.mhAtHypo<Pose2>(X(i), (*it));

        Vector tt = out_pose.translation().vector();
        step_outfile << tt(0) << " " << tt(1) << " "; // [x1 y1] [x2 y2] ...
      }
      step_outfile.close();
    }

  } // END while loop

  clock_t end_time = clock();
  clock_t total_time = end_time - start_time;
  cout << "total_time: " << total_time << endl;
  cout << "# layers: " << mh_isam2->getLastHypoLayer()->getLayerIdx() << endl;

  mh_isam2->printAllFinalHypo();
  //*

  ofstream outfile;
  string file_name = "MH_ISAM2_TEST_city10000";
  outfile.open(file_name + ".txt");

  for (size_t i = 0; i < (key_t + 1); ++i) {
    vector<Pose2> out_arr = mh_results.mhAt<Pose2>(X(i));

    for (size_t h = 0; h < out_arr.size(); ++h) {

      outfile << out_arr[h].x() << " " << out_arr[h].y() << " " << out_arr[h].theta() << endl; //WRONG FORMAT... JUST FOR SIMPLE SHOW
    }
  }
  outfile.close();
  cout << "output " << file_name << ".txt file." << endl;
  // */

  //*
  // Output each hypo
  ofstream outfile_hypo;
  string hypo_file_name = "MH_ISAM2_TEST_city10000_hypos";
  outfile_hypo.open(hypo_file_name + ".txt");

  int hypo_count = 0;
  MHISAM2::HypoList& max_hypo_list = mh_isam2->getLastHypoLayer()->getNodeList();
  for (MHISAM2::HypoListIter it = max_hypo_list.begin(); it != max_hypo_list.end(); ++it) {
    for (size_t i = 0; i < (pose_count); ++i) {
      Pose2 out_pose = mh_results.mhAtHypo<Pose2>(X(i), (*it));

      Vector tt = out_pose.translation().vector();
      outfile_hypo << tt(0) << " " << tt(1) << " "; // [x1 y1] [x2 y2] ...
    }
    outfile_hypo << endl;
    hypo_count++;
  }
  outfile_hypo.close();
  cout << "output " << hypo_file_name << ".txt file." << endl;
  // */

  //*
  // Output each mode
  ofstream outfile_mode;
  string mode_file_name = "MH_ISAM2_TEST_city10000_modes";
  outfile_mode.open(mode_file_name + ".txt");
  outfile_mode << "total_time:  " << total_time << endl;
  outfile_mode << "# layers: " << mh_isam2->getLastHypoLayer()->getLayerIdx() << endl;
  outfile_mode << "overall original hypos:  " << mh_isam2->calculateExpGrowHypoNum() << endl;
  outfile_mode << "overall final hypos:  " << max_hypo_list.size() << endl;
  outfile_mode << "loop_count:  " << loop_count << endl;

  hypo_count = 0;
  for (MHISAM2::HypoListIter it = max_hypo_list.begin(); it != max_hypo_list.end(); ++it) {
    outfile_mode << "==== hypo: " << hypo_count << " ====" << endl;
    for (size_t a = 0; a < ((*it)->ancestor_arr_.size() + 1); ++a) {
      outfile_mode << (*it)->findAncestor(a)->mode_id_ << endl;
    }
    hypo_count++;
  }
  outfile_mode.close();
  cout << "output " << mode_file_name << ".txt file." << endl;

  //*
  ofstream outfile_time;
  string time_file_name = "MH_ISAM2_TEST_city10000_time";
  outfile_time.open(time_file_name + ".txt");
  for (auto acc_time : time_list) {
    outfile_time << acc_time << endl;
  }
  outfile_time.close();
  cout << "output " << time_file_name << ".txt file." << endl;
  // */

  return 0;
}


#include <cpr/cpr.h>
#include <cxxopts.hpp>
#include <iostream>
#include <nlohmann/json.hpp>
#include <Eigen/Dense>
#include <string>

// for convenience
using json = nlohmann::json;
using Eigen::MatrixXi;

enum TILE {SPACE=0, WALL = -1, BEGIN=-2, END=-3};

std::pair<bool, std::vector<std::pair<int, int>>> r_pathfinder(MatrixXi &maze, int x, int y, int prev) {
  std::vector<std::pair<int, int>> direction = {{0, 1}, {0, -1}, {1, 0}, {-1, 0}};

  // check maze boundaries
  if (x < 0 || x >= maze.rows() || y < 0 || y >= maze.cols() ) {
    return {false, {}};
  }

  int value = maze(x, y);

  if (value > prev+1 || value == TILE::SPACE || value == TILE::BEGIN) {
    maze(x, y) = prev+1;

    for (auto&& dir : direction) {
      auto tmp = r_pathfinder(maze, x+dir.first, y+dir.second, prev+1);
      if (tmp.first == true) {
        tmp.second.push_back({x, y});
        return {true, tmp.second};
      }
    }

  } else if (value == TILE::END) {
    return {true, {{x, y}}};
  }
  return {false, {}};
}

int main(int argc, char *argv[]) {
  try {
    std::cout << "Noops Challenge 2019 - Mazebot" << std::endl;

    // =================================================================================================
    // CLI
    cxxopts::Options options(argv[0], "My entry for NOOPS Challenge - Mazebot");
    options.positional_help("[optional args]").show_positional_help();

    // clang-format off
    options.add_options()("help", "Print help")
      ("url", "The Noops challenge API url", cxxopts::value<std::string>()->default_value("https://api.noopschallenge.com"), "URL")
      ;
    // clang-format on
    options.parse_positional({"url"});
    auto result = options.parse(argc, argv);

    if (result.count("help")) {
      std::cout << options.help() << std::endl;
      return 0;
    }

    // =================================================================================================
    // Get
    std::string noops_url = result["url"].as<std::string>();
    std::cout << "noops url : " << noops_url << std::endl;

    auto r = cpr::Get(cpr::Url{noops_url + "/mazebot/random"});

    // =================================================================================================
    // Parse
    auto response = json::parse(r.text);
    // std::cout << response.dump(2) << std::endl;

    // =================================================================================================
    // Code

    int ncol = response["map"].size();
    int nrow = response["map"][0].size();
    std::cout << "Maze size: col: " << ncol << " row: " << nrow << std::endl;

    MatrixXi maze_map(ncol, nrow);
    std::pair<int, int> start(0, 0);

    // dump map in json to map in matrix
    for (int i = 0; i < ncol; ++i) {
      for (int j = 0; j < nrow; ++j) {
        switch (int(response["map"][i][j].get<std::string>()[0])) {
          case 88:  // X = 88
            maze_map(i, j) = TILE::WALL;
            break;
          case 32:  // space = 32
            maze_map(i, j) = TILE::SPACE;
            break;
          case 65:  // A = 65
            maze_map(i, j) = TILE::BEGIN;
            start = {i, j};
            break;
          case 66:  // B = 66
            maze_map(i, j) = TILE::END;
            break;
          default:
            throw std::runtime_error("Unknow character");
        }
      }
    }

    // apply the recursive path finder function
    auto tmp = r_pathfinder(maze_map, start.first, start.second, 0);

    // draw found path in maze with dot
    for (std::size_t i = 1; i < tmp.second.size() - 1; ++i) {
      response["map"][tmp.second[i].first][tmp.second[i].second] = ".";
    }

    std::cout << std::endl;
    for (auto i : response["map"]) {
      for (auto j : i.get<std::vector<std::string>>()) {
        std::cout << j;
      }
      std::cout << std::endl;
    }

  } catch (const cxxopts::OptionException &e) {
    std::cout << "error parsing options: " << e.what() << std::endl;
    return 1;
  }
  return 0;
}