#include <boost/random/inversive_congruential.hpp>


#include <iostream>
#include <limits>
#include <boost/math/constants/constants.hpp>

int main ()
{
    double pi = boost::math::constants::pi<double>();

    std::cout.precision(std::numeric_limits<double>::max_digits10);
    std::cout << pi << std::endl;
}


#include <iostream>
#include <vector>

#include <Eigen/Core>
#include <stan/math.hpp>

#include "celerite/solver/cholesky.h"

#define DO_TEST(NAME, VAR1, VAR2)                            \
{                                                            \
  double base, comp, delta;                                  \
  base = VAR1;                                               \
  comp = VAR2;                                               \
  delta = std::abs(base - comp);                             \
  if (delta > 1e-10) {                                       \
    std::cerr << "Test failed: '" << #NAME << "' - error: " << delta << " " << base << " " << comp << std::endl; \
    return 1;                                                \
  } else                                                     \
    std::cerr << "Test passed: '" << #NAME << "' - error: " << delta << std::endl; \
}

int main (int argc, char* argv[])
{
  typedef stan::math::var g_t;
  typedef Eigen::Matrix<g_t, Eigen::Dynamic, 1> v_t;
  typedef Eigen::Matrix<g_t, Eigen::Dynamic, Eigen::Dynamic> m_t;

  srand(42);

  size_t N = 1024;
  if (argc >= 2) N = atoi(argv[1]);
  size_t niter = 10;
  if (argc >= 3) niter = atoi(argv[2]);

  // Set up the coefficients.
  size_t p_real = 2, p_complex = 1;
  g_t jitter = g_t(0.01);
  v_t a_real(p_real), c_real(p_real),
      a_comp(p_complex), b_comp(p_complex), c_comp(p_complex), d_comp(p_complex);

  a_real << 1.3, 1.5;
  c_real << 0.5, 0.2;
  a_comp << 1.0;
  b_comp << 0.1;
  c_comp << 1.0;
  d_comp << 1.0;

  // Generate some fake data.
  Eigen::VectorXd x = Eigen::VectorXd::Random(N),
                  yerr2 = Eigen::VectorXd::Random(N),
                  y;

  // Set the scale of the uncertainties.
  yerr2.array() *= 0.1;
  yerr2.array() += 0.3;

  // The times need to be sorted.
  std::sort(x.data(), x.data() + x.size());

  // Compute the y values.
  y = sin(x.array());

  celerite::solver::CholeskySolver<g_t> cholesky;
  cholesky.compute(jitter, a_real, c_real, a_comp, b_comp, c_comp, d_comp, x, yerr2);

  std::vector<g_t> params;
  for (int i = 0; i < p_real; ++i) {
    params.push_back(a_real(i));
    params.push_back(c_real(i));
  }
  for (int i = 0; i < p_complex; ++i) {
    params.push_back(a_comp(i));
    params.push_back(b_comp(i));
    params.push_back(c_comp(i));
    params.push_back(d_comp(i));
  }
  std::vector<double> g;

  g_t ll = -0.5 * (cholesky.dot_solve(y) + cholesky.log_determinant());
  ll.grad(params, g);
  for (int i = 0; i < g.size(); ++i) std::cout << g[i] << std::endl;

  return 0;
}


// Copyright (c) 2020 GeometryFactory Sarl (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// $URL$
// $Id$
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
// Author(s): Ahmed Essam <theartful.ae@gmail.com>

#include <CGAL/ipower.h>
#include <QGraphicsView>

#include "ArrangementTypes.h"
#include "ArrangementTypesUtils.h"
#include "PointSnapper.h"

#include <boost/optional.hpp>

template <typename Arr_>
class PointSnapper : public PointSnapperBase
{
  using Arrangement = Arr_;
  using Compute_squared_distance_2 = Rat_kernel::Compute_squared_distance_2;

public:
  PointSnapper(QGraphicsScene*, GridGraphicsItem*, Arrangement*);
  boost::optional<Point_2> snapToArrangement(const QPointF& qpt) override;

private:
  Arrangement* arr;
};

PointSnapperBase::PointSnapperBase(
  QGraphicsScene* scene, GridGraphicsItem* grid) :
    GraphicsSceneMixin(scene),
    gridGraphicsItem{grid}, snapToGridEnabled{false},
    snapToArrangementEnabled{false}
{
}

namespace
{
struct ExplicitLambda
{
  template <typename Arrangement>
  void operator()(demo_types::TypeHolder<Arrangement>)
  {
    Arrangement* arr = nullptr;
    CGAL::assign(arr, arr_obj);
    res = new PointSnapper<Arrangement>(scene, grid, arr);
  }

  PointSnapperBase*& res;
  QGraphicsScene* scene;
  GridGraphicsItem* grid;
  CGAL::Object& arr_obj;
};
} // anonymous namespace

PointSnapperBase* PointSnapperBase::create(
  demo_types::TraitsType tt, QGraphicsScene* scene, GridGraphicsItem* grid,
  CGAL::Object arr_obj)
{
  PointSnapperBase* res;
  ExplicitLambda explicit_lambda{res, scene, grid, arr_obj};
  demo_types::visitArrangementType(tt, explicit_lambda);
  return res;
}

auto PointSnapperBase::snapPoint(const QPointF& qpt) -> Point_2
{
  if (this->snapToGridEnabled && this->snapToArrangementEnabled)
  {
    Point_2 pt = {qpt.x(), qpt.y()};
    auto gridPt = snapToGrid(qpt);
    auto arrPt = snapToArrangement(qpt);
    if (!arrPt)
      return gridPt;
    else if (
      compute_squared_distance_2(pt, gridPt) <
      compute_squared_distance_2(pt, *arrPt))
      return gridPt;
    else
      return *arrPt;
  }
  else if (this->snapToGridEnabled)
  {
    return snapToGrid(qpt);
  }
  else if (this->snapToArrangementEnabled)
  {
    auto arrPt = snapToArrangement(qpt);
    if (arrPt)
      return *snapToArrangement(qpt);
    else
      return {qpt.x(), qpt.y()};
  }
  else
    return Point_2{qpt.x(), qpt.y()};
}

// snap to grid without loss of precision
auto PointSnapperBase::snapToGrid(const QPointF& qpt) -> Point_2
{
  Rational two{2};
  Rational five{5};
  Rational half{0.5};
  // can't use 0.2 since it's not perfectly representable as a float/double
  Rational fifth{Rational{1} / five};

  Rational x;
  {
    int a = gridGraphicsItem->getXPower2();
    int b = gridGraphicsItem->getXPower5();
    // we have to calculate l in BigRat to be exact
    Rational lx;
    if (a < 0)
      lx = CGAL::ipower(half, -a);
    else
      lx = CGAL::ipower(two, a);
    if (b < 0)
      lx *= CGAL::ipower(fifth, -b);
    else
      lx *= CGAL::ipower(five, b);
    x = lx * std::lround(CGAL::to_double(Rational(qpt.x()) / lx));
  }

  Rational y;
  {
    int a = gridGraphicsItem->getYPower2();
    int b = gridGraphicsItem->getYPower5();
    // we have to calculate l in BigRat to be exact
    Rational ly;
    if (a < 0)
      ly = CGAL::ipower(half, -a);
    else
      ly = CGAL::ipower(two, a);
    if (b < 0)
      ly *= CGAL::ipower(fifth, -b);
    else
      ly *= CGAL::ipower(five, b);
    y = ly * std::lround(CGAL::to_double(Rational(qpt.y()) / ly));
  }

  return Point_2{x, y};
}

void PointSnapperBase::setSnapToGrid(bool val)
{
  this->snapToGridEnabled = val;
}
void PointSnapperBase::setSnapToArrangement(bool val)
{
  this->snapToArrangementEnabled = val;
}
bool PointSnapperBase::isSnapToGridEnabled() { return this->snapToGridEnabled; }
bool PointSnapperBase::isSnapToArrangementEnabled()
{
  return this->snapToArrangementEnabled;
}

template <typename Arr_>
PointSnapper<Arr_>::PointSnapper(
  QGraphicsScene* scene, GridGraphicsItem* grid, Arrangement* arr_) :
    PointSnapperBase(scene, grid),
    arr{arr_}
{
}

template <typename Arrangement>
inline boost::optional<PointSnapperBase::Point_2> snapToArrangement(
  const QPointF& qpt, const QTransform& worldTransform, Arrangement* arr)
{
  using Point_2 = PointSnapperBase::Point_2;
  using Rational = PointSnapperBase::Rational;
  using Compute_squared_distance_2 =
    PointSnapperBase::Compute_squared_distance_2;

  Compute_squared_distance_2 compute_squared_distance_2;

  Point_2 initialPoint{qpt.x(), qpt.y()};
  Point_2 closestPoint = initialPoint;

  bool first = true;
  Rational minDist(0);

  static constexpr int PIXEL_RADIUS = 15;
  Rational maxDist =
    PIXEL_RADIUS * PIXEL_RADIUS *
    std::abs(1 / (worldTransform.m11() * worldTransform.m22()));

  for (auto vit = arr->vertices_begin(); vit != arr->vertices_end(); ++vit)
  {
    auto arr_point = vit->point();
    Point_2 point{arr_point.x(), arr_point.y()};
    auto dist = compute_squared_distance_2(initialPoint, point);
    if (first || (dist < minDist))
    {
      first = false;
      minDist = dist;
      closestPoint = point;
    }
  }

  if (!first && minDist < maxDist)
    return closestPoint;
  else
    return {};
}

template <typename Traits>
struct SnapToArrangement
{
  using Point_2 = PointSnapperBase::Point_2;
  template <typename Arrangement>
  boost::optional<Point_2>
  operator()(const QPointF& qpt, const QTransform&, Arrangement*)
  {
    return Point_2{qpt.x(), qpt.y()};
  }
};
template <typename Kernel>
struct SnapToArrangement<CGAL::Arr_linear_traits_2<Kernel>>
{
  using Point_2 = PointSnapperBase::Point_2;
  template <typename Arrangement>
  boost::optional<Point_2> operator()(
    const QPointF& qpt, const QTransform& worldTransform, Arrangement* arr)
  {
    return snapToArrangement(qpt, worldTransform, arr);
  }
};
template <typename Kernel>
struct SnapToArrangement<CGAL::Arr_segment_traits_2<Kernel>>
{
  using Point_2 = PointSnapperBase::Point_2;
  template <typename Arrangement>
  boost::optional<Point_2> operator()(
    const QPointF& qpt, const QTransform& worldTransform, Arrangement* arr)
  {
    return snapToArrangement(qpt, worldTransform, arr);
  }
};
template <typename Kernel>
struct SnapToArrangement<CGAL::Arr_polyline_traits_2<Kernel>>
{
  using Point_2 = PointSnapperBase::Point_2;
  template <typename Arrangement>
  boost::optional<Point_2> operator()(
    const QPointF& qpt, const QTransform& worldTransform, Arrangement* arr)
  {
    return snapToArrangement(qpt, worldTransform, arr);
  }
};

template <typename Arr_>
auto PointSnapper<Arr_>::snapToArrangement(const QPointF& qpt)
  -> boost::optional<Point_2>
{
  using Traits = typename Arrangement::Geometry_traits_2;
  auto view = getView();
  if (view)
    return SnapToArrangement<Traits>{}(qpt, view->transform(), arr);
  else
    return {};
}


#ifndef PYTHONIC_INCLUDE_NUMPY_ARCCOS_HPP
#define PYTHONIC_INCLUDE_NUMPY_ARCCOS_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/acos.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{

  namespace wrapper
  {
    double arccos(long const &v);
    template <class T>
    auto arccos(T const &v) -> decltype(boost::simd::acos(v));
  }

#define NUMPY_NARY_FUNC_NAME arccos
#define NUMPY_NARY_FUNC_SYM wrapper::arccos
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


// Copyright 2018 The Simons Foundation, Inc. - All
// Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NETKET_FFNN_HPP
#define NETKET_FFNN_HPP

#include <iostream>
#include <memory>
#include <sstream>
#include <vector>

#include <Eigen/Dense>

#include "Layers/layer.hpp"
#include "Machine/abstract_machine.hpp"
#include "Utils/all_utils.hpp"

namespace netket {

class FFNN : public AbstractMachine {
  std::vector<AbstractLayer *> layers_;  // Pointers to hidden layers

  std::vector<int> layersizes_;
  int depth_;
  int nlayer_;
  int npar_;
  int nv_;
  std::vector<VectorType> din_;

  std::vector<std::vector<int>> changed_nodes_;
  std::vector<VectorType> new_output_;

  std::unique_ptr<SumOutput> sum_output_layer_;

  std::vector<VectorType> Vforward_;

 public:
  explicit FFNN(std::shared_ptr<const AbstractHilbert> hilbert,
                std::vector<AbstractLayer *> layers)
      : AbstractMachine(hilbert),
        layers_(std::move(layers)),
        nv_(hilbert->Size()) {
    Init();
  }

  void Init() {
    nlayer_ = layers_.size();

    std::string buffer = "";
    // Check that layer sizes are consistent
    layersizes_.push_back(nv_);
    for (int i = 0; i < nlayer_; ++i) {
      layersizes_.push_back(layers_[i]->Noutput());

      if (layersizes_[i] != layers_[i]->Ninput()) {
        throw InvalidInputError("input/output layer sizes do not match");
      }
    }

    // Check that final layer has only 1 unit otherwise add pooling layer
    if (layersizes_.back() != 1) {
      nlayer_ += 1;

      sum_output_layer_ = netket::make_unique<SumOutput>(layersizes_.back());
      layers_.push_back(sum_output_layer_.get());
      layersizes_.push_back(1);
    }
    depth_ = layersizes_.size();

    din_.resize(depth_);
    din_.back().resize(1);
    din_.back()(0) = 1.0;

    npar_ = 0;
    for (int i = 0; i < nlayer_; ++i) {
      npar_ += layers_[i]->Npar();
    }

    for (int i = 0; i < nlayer_; ++i) {
      Vforward_.push_back(VectorXcd(layersizes_[i + 1]));
    }

    changed_nodes_.resize(nlayer_);
    new_output_.resize(nlayer_);

    InfoMessage(buffer) << "# FFNN Initizialized with " << nlayer_
                        << " Layers: ";
    for (int i = 0; i < depth_ - 1; ++i) {
      InfoMessage(buffer) << layersizes_[i] << " -> ";
    }
    InfoMessage(buffer) << layersizes_[depth_ - 1];
    InfoMessage(buffer) << std::endl;
    for (int i = 0; i < nlayer_; ++i) {
      InfoMessage(buffer) << "# Layer " << i + 1 << " : " << layers_[i]->Name()
                          << std::endl;
    }
    InfoMessage(buffer) << "# Total Number of Parameters = " << npar_
                        << std::endl;
  }

  int Nvisible() const override { return layersizes_[0]; }

  int Npar() const override { return npar_; }

  VectorType GetParameters() override {
    VectorType pars(npar_);
    int start_idx = 0;
    for (auto const layer : layers_) {
      int num_of_pars = layer->Npar();
      layer->GetParameters(pars.segment(start_idx, num_of_pars));
      start_idx += num_of_pars;
    }
    return pars;
  }

  void SetParameters(VectorConstRefType pars) override {
    int start_idx = 0;
    for (auto const layer : layers_) {
      int num_of_pars = layer->Npar();
      layer->SetParameters(pars.segment(start_idx, num_of_pars));
      start_idx += num_of_pars;
    }
  }

  void Forward(VisibleConstType v) {
    layers_[0]->Forward(v, Vforward_[0]);
    for (int i = 1; i < nlayer_; ++i) {
      layers_[i]->Forward(Vforward_[i - 1], Vforward_[i]);
    }
  }

  Complex LogValSingle(VisibleConstType v, const any & /*lookup*/) override {
    Forward(v);
    return Vforward_[nlayer_ - 1](0);
  }

  VectorType DerLogSingle(VisibleConstType v, const any & /*lookup*/) override {
    Forward(v);
    return DerLogSingleImpl(v);
  }

  VectorType DerLogSingleImpl(VisibleConstType v) {
    VectorType der(npar_);

    Index start_idx = npar_;
    Index num_of_pars;

    // Backpropagation
    if (nlayer_ > 1) {
      num_of_pars = layers_[nlayer_ - 1]->Npar();
      start_idx -= num_of_pars;
      // Last Layer
      layers_[nlayer_ - 1]->Backprop(
          Vforward_[nlayer_ - 2], Vforward_[nlayer_ - 1], din_.back(),
          din_[nlayer_ - 1], der.segment(start_idx, num_of_pars));
      // Middle Layers
      for (int i = nlayer_ - 2; i > 0; --i) {
        num_of_pars = layers_[i]->Npar();
        start_idx -= num_of_pars;
        layers_[i]->Backprop(Vforward_[i - 1], Vforward_[i], din_[i + 1],
                             din_[i], der.segment(start_idx, num_of_pars));
      }
      // First Layer
      layers_[0]->Backprop(v, Vforward_[0], din_[1], din_[0],
                           der.segment(0, layers_[0]->Npar()));
    } else {
      // Only 1 layer
      layers_[0]->Backprop(v, Vforward_[0], din_.back(), din_[0], der);
    }
    return der;
  }

  void Save(std::string const &filename) const override {
    json state;
    state["Name"] = "FFNN";
    state["Layers"] = {};
    for (int i = 0; i < nlayer_; ++i) {
      layers_[i]->to_json(state);
    }
    WriteJsonToFile(state, filename);
  }

  void Load(const std::string &filename) override {
    auto const pars = ReadJsonFromFile(filename);
    json layers_par;
    if (FieldExists(pars, "Layers")) {
      layers_par = pars["Layers"];
      nlayer_ = layers_par.size();
    } else {
      throw InvalidInputError(
          "Field (Layers) not defined for Machine (FFNN) in initfile");
    }

    for (int i = 0; i < nlayer_; ++i) {
      layers_[i]->from_json(layers_par[i]);
    }
  }

  bool IsHolomorphic() const noexcept override { return true; }
};  // namespace netket

}  // namespace netket

#endif


#include "gl_core_4_3.h"
#include "SDL.h"
#include "SDL_opengl.h"
#include "SDL_timer.h"
#include <Eigen/Dense>

#include "loadshaders_4_3.h"

#include <cmath>
#include <iostream>
#include <stdint.h>

namespace {

constexpr GLubyte *bufferOffset(size_t bytes)
{
    return static_cast<GLubyte *>(0) + bytes;
}

const int kWindowWidth = 800;
const int kWindowHeight = 600;

enum VAO_IDs { Triangles, NumVAOs };
enum Buffer_IDs { ArrayBuffer, NumBuffers };
enum Attrib_IDs { vPosition = 0, mTransform = 1 };

GLuint VAOs[NumVAOs];
GLuint Buffers[NumBuffers];

float rotationAngle = 0;
uint32_t oldTime = 0;
uint32_t currentTime = 0;

const GLuint NumVertices = 6;

void init()
{
    glGenVertexArrays(NumVAOs, VAOs);
    glBindVertexArray(VAOs[Triangles]);

    Eigen::Vector2f vertices[NumVertices] = {
        { -0.90, -0.90 },
        {  0.85, -0.90 },
        { -0.90,  0.85 },
        {  0.90, -0.85 },
        {  0.90,  0.90 },
        { -0.85,  0.90 }
    };

    glGenBuffers(NumBuffers, Buffers);
    glBindBuffer(GL_ARRAY_BUFFER, Buffers[ArrayBuffer]);
    glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW);

    ShaderInfo shaders[] = {
        { GL_VERTEX_SHADER, "../tests/hello/hello_4_3.vert" },
        { GL_FRAGMENT_SHADER, "../tests/hello/hello_4_3.frag" },
        { GL_NONE, NULL }
    };

    GLuint program = loadShaders(shaders);
    glUseProgram(program);

    glVertexAttribPointer(vPosition, 2, GL_FLOAT, GL_FALSE, 0, bufferOffset(0));
    glEnableVertexAttribArray(vPosition);
}

static const float PI = 2*3.1415926;
static const float TWO_PI = 2*PI;

// http://www.opengl.org/sdk/docs/man/xhtml/glBindFragDataLocation.xml
// http://www.opengl.org/sdk/docs/man/xhtml/glBindAttribLocation.xml

void display()
{
    glClear(GL_COLOR_BUFFER_BIT);

    oldTime = currentTime;
    currentTime = SDL_GetTicks();
    float deltaTime = (currentTime - oldTime) / 1000.0f;

    rotationAngle = fmod(rotationAngle + (deltaTime * (PI / 60.0f)), TWO_PI);

    Eigen::Matrix2f rotation;
    rotation << cos(rotationAngle), -sin(rotationAngle),
                sin(rotationAngle),  cos(rotationAngle);
    glUniformMatrix2fv(mTransform, 1, GL_FALSE, rotation.data());

    glBindVertexArray(VAOs[Triangles]);
    glDrawArrays(GL_TRIANGLES, 0, NumVertices);
}

} // namespace

int main(int argc, char *argv[])
{
    if (SDL_Init(SDL_INIT_VIDEO) != 0) {
        std::cerr << SDL_GetError() << std::endl;
        return -1;
    }

    SDL_GL_SetAttribute(SDL_GL_CONTEXT_MAJOR_VERSION, 4);
    SDL_GL_SetAttribute(SDL_GL_CONTEXT_MINOR_VERSION, 3);
    SDL_GL_SetAttribute(SDL_GL_CONTEXT_PROFILE_MASK, SDL_GL_CONTEXT_PROFILE_CORE);

    // Create an OpenGL capable window
    SDL_Window *window = SDL_CreateWindow(
        "Hello World",
        SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED,
        800, 600,
        SDL_WINDOW_OPENGL | SDL_WINDOW_RESIZABLE
    );
    if (!window) {
        std::cerr << SDL_GetError() << std::endl;
        SDL_Quit();
        return -1;
    }

    // Create an OpenGL context associated with the window.
    SDL_GLContext glcontext = SDL_GL_CreateContext(window);
    if (!glcontext) {
        std::cerr << SDL_GetError() << std::endl;
        SDL_Quit();
        return -1;
    }

    // Load extensions
    if (ogl_LoadFunctions() != ogl_LOAD_SUCCEEDED) {
        SDL_DestroyWindow(window);
        SDL_Quit();
        return -1;
    }

    // Initialize displayables
    init();

    SDL_Event event;
    bool keepRunning = true;
    while (keepRunning) {
        while (SDL_PollEvent(&event)) {
            if (event.type == SDL_QUIT) {
                keepRunning = false;
            }
        }

        // Render scene
        display();

        // Swap buffers
        SDL_GL_SwapWindow(window);
    }

    // Once finished with OpenGL functions, the SDL_GLContext can be deleted.
    SDL_GL_DeleteContext(glcontext);

    // Done! Close the window, clean-up and exit the program.
    SDL_DestroyWindow(window);
    SDL_Quit();

    return 0;
}


#include "drake/multibody/contact_solvers/pgs_solver.h"

#include <memory>

#include <Eigen/SparseCore>
#include <gtest/gtest.h>

#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/multibody/contact_solvers/sparse_linear_operator.h"

namespace drake {
namespace multibody {
namespace contact_solvers {
namespace internal {
namespace {

using SparseMatrixd = Eigen::SparseMatrix<double>;
using Eigen::VectorXd;
using Triplet = Eigen::Triplet<double>;
constexpr double kTol = 1e-14;
constexpr int kMaxIterations = 50;

/* A test against analytic solution for a simple contact problem:
 A single point in 3D is in contact with velocity (-1, -1, -1) in the contact
 space without contact force, where the first two components are in arbtrary
 orthogonal directions in the contact plane and the last component is the in the
 normal direction to the contact plane. The friction coefficient is set to be
 large enough to prevent any slipping. Hence, the expected post-solve contact
 velocity is (0, 0, 0). With an arbitrarily prescribed SPD tangent matrix A, the
 contact impulse can also be analytically calculated. The test confirms that the
 results from the PGS solver matches the analytically calculated contact
 velocity and impulse. */
class PgsTest : public ::testing::Test {
 protected:
  void SetUp() override {
    SetSystemDynamicsData();

    PgsSolverParameters params;
    params.abs_tolerance = kTol;
    params.rel_tolerance = kTol;
    params.max_iterations = kMaxIterations;
    pgs_.set_parameters(params);
  }

  // Set the point contact data for a situation with no contact points.
  void SetZeroPointContactData() {
    jacobian_.resize(0, 0);
    Jc_ = std::make_unique<SparseLinearOperator<double>>("Jc", &jacobian_);
    stiffness_.resize(0);
    dissipation_.resize(0);
    penetration_depth_.resize(0);
    mu_.resize(0);
    point_data_ = std::make_unique<PointContactData<double>>(
        &penetration_depth_, Jc_.get(), &stiffness_, &dissipation_, &mu_);
  }

  // Set the point contact data with a single contact point with the given
  // friction coefficient.
  void SetSinglePointContactData(double friction_coeff) {
    jacobian_.resize(3, 3);
    {
      std::vector<Triplet> triplets;
      triplets.emplace_back(0, 0, 1.0);
      triplets.emplace_back(1, 1, 1.0);
      triplets.emplace_back(2, 2, 1.0);
      jacobian_.setFromTriplets(triplets.begin(), triplets.end());
      jacobian_.makeCompressed();
    }
    Jc_ = std::make_unique<SparseLinearOperator<double>>("Jc", &jacobian_);
    // Stiffness dissipation and penetration depth are not used in the PGS
    // solver and are thus set to NAN.
    stiffness_.resize(1);
    stiffness_(0) = NAN;
    dissipation_.resize(1);
    dissipation_(0) = NAN;
    penetration_depth_.resize(1);
    penetration_depth_(0) = NAN;
    mu_ = friction_coeff * VectorXd::Ones(1);
    point_data_ = std::make_unique<PointContactData<double>>(
        &penetration_depth_, Jc_.get(), &stiffness_, &dissipation_, &mu_);
  }

  void SetSystemDynamicsData() {
    v_star_ = -1.0 * VectorXd::Ones(3);
    Ainv_tmp_.resize(3, 3);
    {
      std::vector<Triplet> triplets;
      // Arbitrary entries for the tangent matrix.
      triplets.emplace_back(0, 0, 12.0);
      triplets.emplace_back(1, 1, 4.0);
      triplets.emplace_back(2, 2, 3.0);
      Ainv_tmp_.setFromTriplets(triplets.begin(), triplets.end());
      Ainv_tmp_.makeCompressed();
    }
    Ainv_ = std::make_unique<SparseLinearOperator<double>>("Ainv", &Ainv_tmp_);
    dynamics_data_ =
        std::make_unique<SystemDynamicsData<double>>(Ainv_.get(), &v_star_);
  }

  // Check the solver status to verify the solver has converged as expected.
  void VerifySolverStats() const {
    const PgsSolverStats& solver_stats = pgs_.get_solver_stats();
    EXPECT_TRUE(solver_stats.iterations < kMaxIterations);
    // ‖vc‖∞ <= 1, so the velocity error is smaller than
    // abs_tolerance + 1 * rel_tolerance = 2 * kTol.
    EXPECT_LT(solver_stats.vc_err, 2 * kTol);
    // The ‖W‖ᵣₘₛ > 1 and ‖gamma‖∞ <= 1/3, so the impulse error is smaller than
    // abs_tolerance / ‖W‖ᵣₘₛ + ‖gamma‖∞ * rel_tolerance < 2 * kTol.
    EXPECT_LT(solver_stats.gamma_err, 2 * kTol);
  }

  VectorXd v_star_;
  PgsSolver<double> pgs_;
  std::unique_ptr<SparseLinearOperator<double>> Ainv_;
  std::unique_ptr<SystemDynamicsData<double>> dynamics_data_;
  std::unique_ptr<PointContactData<double>> point_data_;
  VectorXd penetration_depth_;
  std::unique_ptr<SparseLinearOperator<double>> Jc_;
  SparseMatrixd jacobian_;
  SparseMatrixd Ainv_tmp_;
  VectorXd stiffness_;
  VectorXd dissipation_;
  VectorXd mu_;
};

/* The friction coefficient is set to be large enough to prevent any slipping.
 Hence, the expected post-solve contact velocity is (0, 0, 0). With the
 arbitrarily prescribed SPD tangent matrix A, the contact impulse can also be
 analytically calculated. The test confirms that the results from the PGS solver
 matches the analytically calculated contact velocity and impulse for a contact
 in stiction. */
TEST_F(PgsTest, Stiction) {
  // Set up a large friction coefficient to ensure stiction.
  SetSinglePointContactData(1000);
  // Abitrary initial guess.
  Vector3<double> v_guess(0.12, 0.34, 0.56);
  // dt is unused in PGS and is thus set to NAN.
  const double dt = NAN;
  ContactSolverResults<double> result;
  EXPECT_EQ(
      pgs_.SolveWithGuess(dt, *dynamics_data_, *point_data_, v_guess, &result),
      ContactSolverStatus::kSuccess);
  EXPECT_TRUE(
      CompareMatrices(result.ft, Vector2<double>(1. / 12., 1. / 4.), kTol));
  EXPECT_TRUE(CompareMatrices(result.fn, Vector1<double>(1. / 3.), kTol));
  EXPECT_TRUE(CompareMatrices(result.v_next, Vector3<double>(0, 0, 0), kTol));
  EXPECT_TRUE(CompareMatrices(
      result.tau_contact, Vector3<double>(1. / 12., 1. / 4., 1. / 3.), kTol));
  VerifySolverStats();
}

/* The friction coefficient is set to 0 so that the tangential velocity doesn't
 change in during contact. Hence, the expected post-solve contact velocity is
 (-1.0, -1.0, 0). With the arbitrarily prescribed SPD tangent matrix A, the
 contact impulse can also be analytically calculated. The test confirms that the
 results from the PGS solver matches the analytically calculated contact
 velocity and impulse for a sliding contact. */
TEST_F(PgsTest, Sliding) {
  SetSinglePointContactData(0);
  // Abitrary initial guess.
  Vector3<double> v_guess(0.12, 0.34, 0.56);
  // dt is unused in PGS and is thus set to NAN.
  const double dt = NAN;
  ContactSolverResults<double> result;
  EXPECT_EQ(
      pgs_.SolveWithGuess(dt, *dynamics_data_, *point_data_, v_guess, &result),
      ContactSolverStatus::kSuccess);
  EXPECT_TRUE(CompareMatrices(result.ft, Vector2<double>(0, 0), kTol));
  EXPECT_TRUE(CompareMatrices(result.fn, Vector1<double>(1. / 3.), kTol));
  EXPECT_TRUE(
      CompareMatrices(result.v_next, Vector3<double>(-1.0, -1.0, 0), kTol));
  EXPECT_TRUE(CompareMatrices(result.tau_contact,
                              Vector3<double>(0, 0, 1. / 3.), kTol));
  VerifySolverStats();
}

/* The solver exits early if there the number of contact is zero. Tests that the
 contact solver result in this case is as expected. */
TEST_F(PgsTest, NoContact) {
  // Solve a non trivial problem first.
  const Vector3<double> v_guess(0.12, 0.34, 0.56);
  SetSinglePointContactData(0);
  const double dt = NAN;
  ContactSolverResults<double> result;
  pgs_.SolveWithGuess(dt, *dynamics_data_, *point_data_, v_guess, &result);

  // Now solve a problem with no contact.
  SetZeroPointContactData();
  EXPECT_EQ(
      pgs_.SolveWithGuess(dt, *dynamics_data_, *point_data_, v_guess, &result),
      ContactSolverStatus::kSuccess);
  EXPECT_EQ(result.ft.size(), 0);
  EXPECT_EQ(result.fn.size(), 0);
  EXPECT_TRUE(
      CompareMatrices(result.v_next, Vector3<double>(-1.0, -1.0, -1.0), kTol));
  EXPECT_TRUE(
      CompareMatrices(result.tau_contact, Vector3<double>::Zero(), kTol));
  VerifySolverStats();
}
}  // namespace
}  // namespace internal
}  // namespace contact_solvers
}  // namespace multibody
}  // namespace drake


#include <boost/random/ranlux.hpp>


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

#include "Framework/TestingFramework.hpp"

#include <array>
#include <boost/optional.hpp>
#include <cmath>
#include <memory>
#include <pup.h>
#include <random>

#include "DataStructures/Tensor/Tensor.hpp"
#include "Domain/CoordinateMaps/Frustum.hpp"
#include "Domain/OrientationMap.hpp"
#include "ErrorHandling/Error.hpp"
#include "Framework/TestHelpers.hpp"
#include "Helpers/Domain/CoordinateMaps/TestMapHelpers.hpp"
#include "Utilities/TypeTraits.hpp"

namespace domain {
namespace {
void test_suite_for_frustum(const bool with_equiangular_map) {
  INFO("Suite for frustum");
  // Set up random number generator
  MAKE_GENERATOR(gen);
  std::uniform_real_distribution<> lower_bound_dis(-14, -2);
  std::uniform_real_distribution<> upper_bound_dis(2, 14);

  const double lower_x_lower_base = lower_bound_dis(gen);
  CAPTURE_PRECISE(lower_x_lower_base);
  const double lower_y_lower_base = lower_bound_dis(gen);
  CAPTURE_PRECISE(lower_y_lower_base);
  const double upper_x_lower_base = upper_bound_dis(gen);
  CAPTURE_PRECISE(upper_x_lower_base);
  const double upper_y_lower_base = upper_bound_dis(gen);
  CAPTURE_PRECISE(upper_y_lower_base);
  const double lower_x_upper_base = lower_bound_dis(gen);
  CAPTURE_PRECISE(lower_x_upper_base);
  const double lower_y_upper_base = lower_bound_dis(gen);
  CAPTURE_PRECISE(lower_y_upper_base);
  const double upper_x_upper_base = upper_bound_dis(gen);
  CAPTURE_PRECISE(upper_x_upper_base);
  const double upper_y_upper_base = upper_bound_dis(gen);
  CAPTURE_PRECISE(upper_y_upper_base);

  for (OrientationMapIterator<3> map_i{}; map_i; ++map_i) {
    const std::array<std::array<double, 2>, 4> face_vertices{
        {{{lower_x_lower_base, lower_y_lower_base}},
         {{upper_x_lower_base, upper_y_lower_base}},
         {{lower_x_upper_base, lower_y_upper_base}},
         {{upper_x_upper_base, upper_y_upper_base}}}};
    const CoordinateMaps::Frustum frustum_map(face_vertices, -1.0, 2.0, map_i(),
                                              with_equiangular_map, 1.01);
    test_suite_for_map_on_unit_cube(frustum_map);
  }
}

void test_frustum_fail() noexcept {
  INFO("Frustum fail");
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{2.0, 2.0}}, {{-4.0, -4.0}}, {{4.0, 4.0}}}};
  const CoordinateMaps::Frustum map(face_vertices, 2.0, 5.0,
                                    OrientationMap<3>{});

  // For the choice of params above, any point with z<=-1 should fail.
  const std::array<double, 3> test_mapped_point1{{3.0, 3.0, -1.0}};
  const std::array<double, 3> test_mapped_point2{{6.0, -7.0, -1.0}};
  const std::array<double, 3> test_mapped_point3{{6.0, -7.0, -3.0}};

  // This is outside the mapped frustum, so inverse should either
  // return the correct inverse (which happens to be computable for
  // this point) or it should return boost::none.
  const std::array<double, 3> test_mapped_point4{{0.0, 0.0, 9.0}};

  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point1)));
  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point2)));
  CHECK_FALSE(static_cast<bool>(map.inverse(test_mapped_point3)));
  if (map.inverse(test_mapped_point4)) {
    CHECK_ITERABLE_APPROX(map(map.inverse(test_mapped_point4).get()),
                          test_mapped_point4);
  }
}

void test_alignment() {
  INFO("Alignment");
  // This test tests that the logical axes point along the expected directions
  // in physical space

  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{2.0, 2.0}}, {{-4.0, -4.0}}, {{4.0, 4.0}}}};
  const double lower_bound = 2.0;
  const double upper_bound = 5.0;

  const auto wedge_directions = all_wedge_directions();
  const CoordinateMaps::Frustum map_upper_zeta(
      face_vertices, lower_bound, upper_bound,
      wedge_directions[0]);  // Upper Z frustum
  const CoordinateMaps::Frustum map_upper_eta(
      face_vertices, lower_bound, upper_bound,
      wedge_directions[2]);  // Upper Y frustum
  const CoordinateMaps::Frustum map_upper_xi(
      face_vertices, lower_bound, upper_bound,
      wedge_directions[4]);  // Upper X Frustum
  const CoordinateMaps::Frustum map_lower_zeta(
      face_vertices, lower_bound, upper_bound,
      wedge_directions[1]);  // Lower Z frustum
  const CoordinateMaps::Frustum map_lower_eta(
      face_vertices, lower_bound, upper_bound,
      wedge_directions[3]);  // Lower Y frustum
  const CoordinateMaps::Frustum map_lower_xi(
      face_vertices, lower_bound, upper_bound,
      wedge_directions[5]);  // Lower X frustum
  const std::array<double, 3> lowest_corner{{-1.0, -1.0, -1.0}};
  const std::array<double, 3> along_xi{{1.0, -1.0, -1.0}};
  const std::array<double, 3> along_eta{{-1.0, 1.0, -1.0}};
  const std::array<double, 3> along_zeta{{-1.0, -1.0, 1.0}};

  const std::array<double, 3> lowest_physical_corner_in_map_upper_zeta{
      {-2.0, -2.0, 2.0}};
  const std::array<double, 3> lowest_physical_corner_in_map_upper_eta{
      {-2.0, 2.0, -2.0}};
  const std::array<double, 3> lowest_physical_corner_in_map_upper_xi{
      {2.0, -2.0, -2.0}};
  const std::array<double, 3> lowest_physical_corner_in_map_lower_zeta{
      {-2.0, 2.0, -2.0}};
  const std::array<double, 3> lowest_physical_corner_in_map_lower_eta{
      {-2.0, -2.0, 2.0}};
  const std::array<double, 3> lowest_physical_corner_in_map_lower_xi{
      {-2.0, 2.0, -2.0}};

  // Test that this map's logical axes point along +X, +Y, +Z:
  CHECK(map_upper_zeta(along_xi)[0] == approx(2.0));
  CHECK(map_upper_zeta(along_eta)[1] == approx(2.0));
  CHECK(map_upper_zeta(along_zeta)[2] == approx(5.0));
  CHECK_ITERABLE_APPROX(map_upper_zeta(lowest_corner),
                        lowest_physical_corner_in_map_upper_zeta);

  // Test that this map's logical axes point along +Z, +X, +Y:
  CHECK(map_upper_eta(along_xi)[2] == approx(2.0));
  CHECK(map_upper_eta(along_eta)[0] == approx(2.0));
  CHECK(map_upper_eta(along_zeta)[1] == approx(5.0));
  CHECK_ITERABLE_APPROX(map_upper_eta(lowest_corner),
                        lowest_physical_corner_in_map_upper_eta);

  // Test that this map's logical axes point along +Y, +Z, +X:
  CHECK(map_upper_xi(along_xi)[1] == approx(2.0));
  CHECK(map_upper_xi(along_eta)[2] == approx(2.0));
  CHECK(map_upper_xi(along_zeta)[0] == approx(5.0));
  CHECK_ITERABLE_APPROX(map_upper_xi(lowest_corner),
                        lowest_physical_corner_in_map_upper_xi);

  // Test that this map's logical axes point along +X, -Y, -Z:
  CHECK(map_lower_zeta(along_xi)[0] == approx(2.0));
  CHECK(map_lower_zeta(along_eta)[1] == approx(-2.0));
  CHECK(map_lower_zeta(along_zeta)[2] == approx(-5.0));
  CHECK_ITERABLE_APPROX(map_lower_zeta(lowest_corner),
                        lowest_physical_corner_in_map_lower_zeta);

  // Test that this map's logical axes point along -Z, +X, -Y:
  CHECK(map_lower_eta(along_xi)[2] == approx(-2.0));
  CHECK(map_lower_eta(along_eta)[0] == approx(2.0));
  CHECK(map_lower_eta(along_zeta)[1] == approx(-5.0));
  CHECK_ITERABLE_APPROX(map_lower_eta(lowest_corner),
                        lowest_physical_corner_in_map_lower_eta);

  // Test that this map's logical axes point along -Y, +Z, -X:
  CHECK(map_lower_xi(along_xi)[1] == approx(-2.0));
  CHECK(map_lower_xi(along_eta)[2] == approx(2.0));
  CHECK(map_lower_xi(along_zeta)[0] == approx(-5.0));
  CHECK_ITERABLE_APPROX(map_lower_xi(lowest_corner),
                        lowest_physical_corner_in_map_lower_xi);
}

void test_auto_projective_scale_factor() {
  INFO("Auto projective scale factor");
  // This test tests that the correct suggested projective scale factor
  // is computed based on the side lengths of the frustum.

  // Set up random number generator
  MAKE_GENERATOR(gen);
  std::uniform_real_distribution<> lower_bound_dis(-14, -2);
  std::uniform_real_distribution<> upper_bound_dis(2, 14);
  std::uniform_real_distribution<> logical_dis(-1, 1);

  const double lower_x_lower_base = lower_bound_dis(gen);
  CAPTURE_PRECISE(lower_x_lower_base);
  const double lower_y_lower_base = lower_bound_dis(gen);
  CAPTURE_PRECISE(lower_y_lower_base);
  const double upper_x_lower_base = upper_bound_dis(gen);
  CAPTURE_PRECISE(upper_x_lower_base);
  const double upper_y_lower_base = upper_bound_dis(gen);
  CAPTURE_PRECISE(upper_y_lower_base);
  const double lower_x_upper_base = lower_bound_dis(gen);
  CAPTURE_PRECISE(lower_x_upper_base);
  const double lower_y_upper_base = lower_bound_dis(gen);
  CAPTURE_PRECISE(lower_y_upper_base);
  const double upper_x_upper_base = upper_bound_dis(gen);
  CAPTURE_PRECISE(upper_x_upper_base);
  const double upper_y_upper_base = upper_bound_dis(gen);
  CAPTURE_PRECISE(upper_y_upper_base);
  const double xi = logical_dis(gen);
  CAPTURE_PRECISE(xi);
  const double eta = logical_dis(gen);
  CAPTURE_PRECISE(eta);
  const double zeta = logical_dis(gen);
  CAPTURE_PRECISE(zeta);

  const std::array<double, 3> logical_coord{{0.0, 0.0, zeta}};
  const double lower_bound = 2.0;
  const double upper_bound = 5.0;
  const double sigma_z = 0.5 * (upper_bound + lower_bound);
  const double delta_z = 0.5 * (upper_bound - lower_bound);
  const double w_delta = sqrt(((upper_x_lower_base - lower_x_lower_base) *
                               (upper_y_lower_base - lower_y_lower_base)) /
                              ((upper_x_upper_base - lower_x_upper_base) *
                               (upper_y_upper_base - lower_y_upper_base)));
  const double projective_zeta = (w_delta - 1.0 + zeta * (w_delta + 1.0)) /
                                 (w_delta + 1.0 + zeta * (w_delta - 1.0));
  const double expected_physical_z = sigma_z + delta_z * (projective_zeta);
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{lower_x_lower_base, lower_y_lower_base}},
       {{upper_x_lower_base, upper_y_lower_base}},
       {{lower_x_upper_base, lower_y_upper_base}},
       {{upper_x_upper_base, upper_y_upper_base}}}};

  const CoordinateMaps::Frustum map(face_vertices, lower_bound, upper_bound, {},
                                    false, 1.0, true);  // Upper Z frustum
  CHECK(map(logical_coord)[2] == approx(expected_physical_z));
}

void test_is_identity() {
  INFO("Is identity");
  check_if_map_is_identity(CoordinateMaps::Frustum{
      std::array<std::array<double, 2>, 4>{
          {{{-1.0, -1.0}}, {{1.0, 1.0}}, {{-1.0, -1.0}}, {{1.0, 1.0}}}},
      -1.0, 1.0, OrientationMap<3>{}, false, 1.0});
  CHECK(not CoordinateMaps::Frustum{
      std::array<std::array<double, 2>, 4>{
          {{{-1.0, -1.0}}, {{2.0, 1.0}}, {{-1.0, -3.0}}, {{1.0, 1.0}}}},
      -1.0, 1.0, OrientationMap<3>{}, false, 1.5}
                .is_identity());
}
}  // namespace

SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum", "[Domain][Unit]") {
  test_frustum_fail();
  test_suite_for_frustum(false);  // Equidistant
  test_suite_for_frustum(true);   // Equiangular
  test_alignment();
  test_auto_projective_scale_factor();
  test_is_identity();
}

// [[OutputRegex, A projective scale factor of zero maps all coordinates to
// zero! Set projective_scale_factor to unity to turn off projective scaling.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Assert0",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{2.0, 2.0}}, {{-4.0, -4.0}}, {{4.0, 4.0}}}};
  const double lower_bound = 2.0;
  const double upper_bound = 5.0;
  const double projective_scale_factor = 0.0;
  const bool with_equiangular_map = false;

  auto failed_frustum = CoordinateMaps::Frustum(
      face_vertices, lower_bound, upper_bound, OrientationMap<3>{},
      with_equiangular_map, projective_scale_factor);
  static_cast<void>(failed_frustum);

  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The lower bound for a coordinate must be numerically less
// than the upper bound for that coordinate.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Assert1",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{-3.0, 2.0}}, {{-4.0, -4.0}}, {{4.0, 4.0}}}};
  const double lower_bound = 2.0;
  const double upper_bound = 5.0;

  auto failed_frustum = CoordinateMaps::Frustum(
      face_vertices, lower_bound, upper_bound, OrientationMap<3>{});
  static_cast<void>(failed_frustum);

  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The lower bound for a coordinate must be numerically less
// than the upper bound for that coordinate.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Assert2",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{2.0, -3.0}}, {{-4.0, -4.0}}, {{4.0, 4.0}}}};
  const double lower_bound = 2.0;
  const double upper_bound = 5.0;

  auto failed_frustum = CoordinateMaps::Frustum(
      face_vertices, lower_bound, upper_bound, OrientationMap<3>{});
  static_cast<void>(failed_frustum);

  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The lower bound for a coordinate must be numerically less
// than the upper bound for that coordinate.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Assert3",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{2.0, 2.0}}, {{-4.0, -4.0}}, {{-5.0, 4.0}}}};
  const double lower_bound = 2.0;
  const double upper_bound = 5.0;

  auto failed_frustum = CoordinateMaps::Frustum(
      face_vertices, lower_bound, upper_bound, OrientationMap<3>{});
  static_cast<void>(failed_frustum);

  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The lower bound for a coordinate must be numerically less
// than the upper bound for that coordinate.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Assert4",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{2.0, 2.0}}, {{-4.0, -4.0}}, {{4.0, -5.0}}}};
  const double lower_bound = 2.0;
  const double upper_bound = 5.0;

  auto failed_frustum = CoordinateMaps::Frustum(
      face_vertices, lower_bound, upper_bound, OrientationMap<3>{});
  static_cast<void>(failed_frustum);

  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, The lower bound for a coordinate must be numerically less
// than the upper bound for that coordinate.]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.Domain.CoordinateMaps.Frustum.Assert5",
                               "[Domain][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  const std::array<std::array<double, 2>, 4> face_vertices{
      {{{-2.0, -2.0}}, {{2.0, 2.0}}, {{-4.0, -4.0}}, {{4.0, 4.0}}}};
  const double lower_bound = 2.0;
  const double upper_bound = -2.0;

  auto failed_frustum = CoordinateMaps::Frustum(
      face_vertices, lower_bound, upper_bound, OrientationMap<3>{});
  static_cast<void>(failed_frustum);

  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}
}  // namespace domain


//=======================================================================
// Copyright 1997-2001 University of Notre Dame.
// Authors: Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#error "This example appears to be incorrect; it uses edge weights that are smaller than 0 using the comparison operator passed to Dijkstra's algorithm, which is not allowed."

#include <boost/config.hpp>
#include <iostream>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/transpose_graph.hpp>

/* Output:

  distances from start vertex:
  distance(a) = 0
  distance(b) = 3
  distance(c) = 1
  distance(d) = 3
  distance(e) = 3

  min-max paths tree
  a --> c
  b -->
  c --> d
  d --> e
  e --> b

*/

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

  typedef adjacency_list<listS, vecS, directedS,
    no_property, property<edge_weight_t, int> > Graph;
  typedef graph_traits<Graph>::vertex_descriptor Vertex;

  typedef std::pair<int,int> E;

  const char name[] = "abcdef";

  const int num_nodes = 6;
  E edges[] = { E(0,2), E(1,1), E(1,3), E(1,4), E(2,1), E(2,3),
                E(3,4), E(4,0), E(4,1) };
  int weights[] = { 1, 2, 1, 2, 7, 3, 1, 1, 1};
  const int n_edges = sizeof(edges)/sizeof(E);
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  // VC++ can't handle iterator constructors
  Graph G(num_nodes);
  property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, G);
  for (std::size_t j = 0; j < sizeof(edges) / sizeof(E); ++j) {
    graph_traits<Graph>::edge_descriptor e; bool inserted;
    boost::tie(e, inserted) = add_edge(edges[j].first, edges[j].second, G);
    weightmap[e] = weights[j];
  }
#else
  Graph G(edges, edges + n_edges, weights, num_nodes);
  property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, G);
#endif

  std::vector<Vertex> p(num_vertices(G));
  std::vector<int> d(num_vertices(G));

  Vertex s = *(vertices(G).first);

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  dijkstra_shortest_paths
    (G, s, &p[0], &d[0], weightmap, get(vertex_index, G),
     std::greater<int>(), closed_plus<int>(), (std::numeric_limits<int>::max)(), 0,
     default_dijkstra_visitor());
#else
  dijkstra_shortest_paths
    (G, s, distance_map(&d[0]).
     predecessor_map(&p[0]).
     distance_compare(std::greater<int>()));
#endif

  std::cout << "distances from start vertex:" << std::endl;
  graph_traits<Graph>::vertex_iterator vi, vend;
  for(boost::tie(vi,vend) = vertices(G); vi != vend; ++vi)
    std::cout << "distance(" << name[*vi] << ") = " << d[*vi] << std::endl;
  std::cout << std::endl;

  std::cout << "min-max paths tree" << std::endl;
  adjacency_list<> tree(num_nodes);

  for(boost::tie(vi,vend) = vertices(G); vi != vend; ++vi)
    if (*vi != p[*vi])
      add_edge(p[*vi], *vi, tree);

  print_graph(tree, name);

  return 0;
}


//
//  RandomHomology.hpp
//  BitTests
//
//  Created by Sayan Mandal on 2/24/16.
//  Copyright © 2016 Sayan Mandal. All rights reserved.
//

#ifndef RandomHomology_hpp
#define RandomHomology_hpp
#include "Utilities.hpp"
#include <gudhi/Alpha_complex.h>
// to construct a simplex_tree from alpha complex
#include <gudhi/Simplex_tree.h>
#include <CGAL/Epick_d.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Regular_triangulation_3.h>
#include <CGAL/Alpha_shape_cell_base_3.h>
#include <CGAL/Alpha_shape_vertex_base_3.h>
#include <CGAL/Alpha_shape_3.h>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_io.hpp>
#include <iostream>
#include <string>
#include <vector>
#include <limits>  // for numeric limits
#include <fstream>
#include <list>
#include <cassert>
//using Kernel = CGAL::Epick_d< CGAL::Dimension_tag<3> >;
//using Point = Kernel::Point_d;
//using Vector_of_points = std::vector<Point>;
//typedef CGAL::Exact_predicates_inexact_constructions_kernel Gt;
//typedef Gt::Point_3                                  Ppoint;

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Regular_triangulation_vertex_base_3<K> Vbb;
typedef CGAL::Alpha_shape_vertex_base_3<K, Vbb>         Vb;
typedef CGAL::Regular_triangulation_cell_base_3<K> Rcb;
typedef CGAL::Alpha_shape_cell_base_3<K, Rcb>           Fb;

typedef CGAL::Triangulation_data_structure_3<Vb,Fb> Tds;
typedef CGAL::Regular_triangulation_3<K,Tds>       Triangulation_3;
typedef CGAL::Alpha_shape_3<Triangulation_3>        Alpha_shape_3;
typedef Alpha_shape_3::Cell_handle          Cell_handle;
typedef Alpha_shape_3::Vertex_handle        Vertex_handle;
typedef Alpha_shape_3::Facet                Facet;
typedef Alpha_shape_3::Edge                 Edge;
typedef Triangulation_3::Weighted_point                 Weighted_point;
typedef Triangulation_3::Bare_point                         Bare_point;

using std::set;
using namespace std;
class RandomHomology {
public:
    int dim;
    MortonCode *mc;
    ANNkd_tree *kd_tree;
    set<int>removed_ids;
    vector<vector<int>> indices;
    vector<vector<float>> initial_points;
    RandomHomology(vector<vector<float>> &points,
                   vector<float> &min_bounds,
                   vector<float> &max_bounds, int dim);
    RandomHomology(GIC &g);
    ~RandomHomology() {};
    void run(double alpha);
    void run(double alpha, string input_points_file, vector<Operation*> &c);
    void collapseToClosest(MortonPoint p, vector<Operation*> &collapses);
};

#endif /* RandomHomology_hpp */


/**
 * \file dcs/math/stats/distribution/map.hpp
 *
 * \brief Markov Arrival Process (MAP).
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 *

 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_STATS_DISTRIBUTION_MAP_HPP
#define DCS_MATH_STATS_DISTRIBUTION_MAP_HPP

#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/operation/abs.hpp>
#include <boost/numeric/ublasx/operation/cumsum.hpp>
#include <boost/numeric/ublasx/operation/diag.hpp>
#include <boost/numeric/ublasx/operation/inv.hpp>
#include <boost/numeric/ublasx/operation/lu.hpp>
#include <boost/numeric/ublasx/operation/max.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/sum.hpp>
#include <boost/numeric/ublasx/operation/which.hpp>
#include <cmath>
#include <cstddef>
#include <dcs/assert.hpp>
#include <dcs/debug.hpp>
#include <dcs/math/policies/policy.hpp>
#include <dcs/math/stats/distribution/base_distribution.hpp>
#include <dcs/math/stats/distribution/exponential.hpp>
#include <dcs/math/stats/function/rand.hpp>
#include <dcs/math/random/any_generator.hpp>
#include <dcs/math/random/base_generator.hpp>
#include <dcs/math/random/uniform_01_adaptor.hpp>
//#include <dcs/math/random/uniform_int_adaptor.hpp>
#include <functional>
#include <iostream>
#include <vector>
#include <stdexcept>

namespace dcs { namespace math { namespace stats {

namespace detail { namespace /*<unnamed>*/ {

/**
 * \brief Equilibrium distribution of a continuous-time Markov chain (CTMC).
 * \param Q Infinitesimal generator of the CTMC.
 * \return The equilibrium distribution of the CTMC.
 *
 * Solve the following system:
 * \f{align}
 * \mathbf{\pi}\mathbf{Q}=\mathbf{0} \\
 * \mathbf{\pi}\mathbf{1}^T=1
 * \f}
 * where:
 * - the first equation is the global balance equation,
 * - the * second one is the normalization condition,
 * - and \f$\mathbf{\pi}\f$ is the steady state probabilities vector
 *   (expressed as a row vector).
 * .
 *
 * \note
 *  Actually, we solve the CTMC by means of the direct method (see
 *  [1,2,3] for possible issues and other existing methods).
 *  From the point of view of implementation, there are two main problems:
 *  -# Maths routing usually assumes that vectors are expressed as column
 *     vector.
 *     So, the global balance equation comes in the "wrong" form: the
 *     unknowns are given as a row vector not a column vector.
 *     This problem is resolved by transposing the equation, i.e.
 *     \f[
 *      \mathbf{Q}^T\mathbf{\pi}^T = \mathbf{0}^T
 *     \f]
 *     where the right hand side is now a column vector of zeros, rather than a
 *     row vector.
 *  -# The redundancy amongst the global balance equations.
 *     To solve this problem we need the additional information of the
 *     normalization condition.
 *     This problem is resolved by replacing one of the global balance equations
 *     by the normalization condition.
 *     In the transposed matrix this corresponds to replacing one row by a row
 *     of 1's.
 *     We usually choose the last row and denote the modified matrix Q_n^T .
 *     Similarly we change the constant terms vector, which was all zeros to be
 *     a column vector with 1 in the last row, and zeros everywhere else.
 *     We denote such a vector, \f$e_n^T\f$.
 *  .
 *
 * References:
 * -# W.J. Stewart.
 *    "Introduction to the Numerical Solution of Markov Chains",
 *    Princeton University Press, 1994.
 * -# W.J. Stewart.
 *    "Performance Modeling and Markov Chains",
 *    Formal Methods of Performance Evaluation, Springer, 2007.
 * -# W.J. Stewart.
 *    "Probability, Markov Chains, Queues, and Simulation",
 *    Princeton University Press, 2009.
 * .
 */
template <typename MatrixExprT>
::boost::numeric::ublas::vector<
	typename ::boost::numeric::ublas::matrix_traits<MatrixExprT>::value_type
> ctmc_solve(::boost::numeric::ublas::matrix_expression<MatrixExprT> const& Q)
{
	namespace ublas = ::boost::numeric::ublas;
	namespace ublasx = ::boost::numeric::ublasx;

	typedef typename ublas::matrix_traits<MatrixExprT>::value_type value_type;
	typedef typename ublas::matrix_traits<MatrixExprT>::size_type size_type;
	typedef ublas::vector<size_type> size_vector_type;
	typedef ublas::vector<value_type> vector_type;

	size_type nr = ublasx::num_rows(Q);
	//size_type nc = ublasx::num_columns(Q);

	size_vector_type z;

	// Is the CTMC irreducible?

	z = ublasx::which(
		ublasx::sum<1>(
			ublasx::abs(Q)
		),
		::std::bind2nd(
			::std::equal_to<value_type>(),
			value_type(0)
		)
	);

	size_type nz;
	nz = ublasx::size(z);

	if (nz > 1)
	{
		DCS_DEBUG_TRACE("Warning: Q is a reducible infinitesimal generator.");
//		b = ublas::zero_vector<value_type>(nr);
	}
	else if (nz == 0)
	{
		z.resize(1, false);
		z(0) = nr-1;
		nz = 1;
	}

	// Create the constant terms vecotr b=e_n^T such that Q_n^T\pi^T=e_n^T
	vector_type b;
	b = ublas::zero_vector<value_type>(nr);

	ublas::matrix<value_type> tmp_Q(Q);

	// normalization condition
	for (size_type j = 0; j < nz; ++j)
	{
		size_type k(z(j));

		b(k) = 1;

		for (size_type i = 0; i < nr; ++i)
		{
			tmp_Q(i,k) = value_type(1);
		}
	}

	ublasx::lu_solve_inplace(ublas::trans(tmp_Q), b);

	return ublas::trans(b);
}

/**
 * \brief Equilibrium distribution of a discrete-time Markov chain (DTMC).
 * \param P Stochastic transition matrix of the DTMC.
 * \return The equilibrium distribution of the DTMC.
 */
template <typename MatrixExprT>
::boost::numeric::ublas::vector<
	typename ::boost::numeric::ublas::matrix_traits<MatrixExprT>::value_type
> dtmc_solve(::boost::numeric::ublas::matrix_expression<MatrixExprT> const& P)
{
	namespace ublas = ::boost::numeric::ublas;
	namespace ublasx = ::boost::numeric::ublasx;

	typedef MatrixExprT matrix_type;
	typedef typename ublas::matrix_traits<matrix_type>::size_type size_type;
	typedef typename ublas::matrix_traits<matrix_type>::value_type value_type;

	size_type nr = ublasx::num_rows(P);
	size_type nc = ublasx::num_columns(P);

	ublas::identity_matrix<value_type> I(nr,nc);

	return ctmc_solve(P-I);
}

/**
 * \brief Embedded discrete-time process of the given MAP.
 *
 * \param D0 The \f$D_0\f$ matrix of the MAP.
 * \param D1 The \f$D_1\f$ matrix of the MAP.
 * \return The probability transition matrix of the embedded process.
 *
 * Compute the stochastic transition matrix \f$P\f$ of the embedded phase
 * process at arrival instants.
 * Specifically, \f$P_{ij} is the probability that the MAP restarts in phase
 * \f$j\f$ if the last absorption occurred in phase \f$i\f$.
 *
 * \note If the MAP is feasible, then the probability transition matrix must be
 *  an irreducible stochastic matrix.
 *  In this case, the stationary distribution at arrival instants denoted by
 *  \f$\pi\f$ is given by \f$\piP=\pi$ and \f$\pie^T=1\f$.
 *  After an event has been generated, the distribution of the MAP is given by
 *  \f$\pi\f$.
 */
template <typename D0MatrixExprT, typename D1MatrixExprT>
::boost::numeric::ublas::matrix<
	typename ::boost::numeric::ublas::promote_traits<
		typename ::boost::numeric::ublas::matrix_traits<D0MatrixExprT>::value_type,
		typename ::boost::numeric::ublas::matrix_traits<D1MatrixExprT>::value_type
	>::promote_type
> embedded_dtmc(::boost::numeric::ublas::matrix_expression<D0MatrixExprT> const& D0,
				::boost::numeric::ublas::matrix_expression<D1MatrixExprT> const& D1)
{

//	typedef typename ::boost::numeric::ublas::promote_traits<
//				typename ::boost::numeric::ublas::matrix_traits<D0MatrixExprT>::value_type,
//				typename ::boost::numeric::ublas::matrix_traits<D1MatrixExprT>::value_type
//			>::promote_type value_type;
//	typedef ::boost::numeric::ublas::matrix<value_type> matrix_type;

	// Compute P=(-D0)^{-1})*D1
	return ::boost::numeric::ublas::prod(
				::boost::numeric::ublasx::inv(-D0),
				D1
			);
}

/// Compute the equilibrium distribution of the embedded discrete-time process.
template <typename D0MatrixExprT, typename D1MatrixExprT>
::boost::numeric::ublas::vector<
	typename ::boost::numeric::ublas::promote_traits<
		typename ::boost::numeric::ublas::matrix_traits<D0MatrixExprT>::value_type,
		typename ::boost::numeric::ublas::matrix_traits<D1MatrixExprT>::value_type
	>::promote_type
> equilibrium_distribution(::boost::numeric::ublas::matrix_expression<D0MatrixExprT> const& D0,
						   ::boost::numeric::ublas::matrix_expression<D1MatrixExprT> const& D1)
{
	return dtmc_solve(embedded_dtmc(D0, D1));
}

template <typename ValueT, typename URNG>
inline
ValueT generate_probability(URNG& rng)
{
	::dcs::math::random::uniform_01_adaptor<URNG&,ValueT> u01_rng(rng);

	return u01_rng();
}

template <typename VectorExprT, typename URNG>
typename ::boost::numeric::ublas::vector_traits<VectorExprT>::size_type generate_initial_state(::boost::numeric::ublas::vector_expression<VectorExprT> const& pi, URNG& rng)
{
	namespace ublas = ::boost::numeric::ublas;
	namespace ublasx = ::boost::numeric::ublasx;

	typedef typename ublas::vector_traits<VectorExprT>::size_type size_type;
	typedef typename ublas::vector_traits<VectorExprT>::value_type value_type;

	size_type n = ublasx::size(pi);

	value_type p = generate_probability<value_type>(rng);

	size_type ret(n); //default to last state

	//NOTE: states start from 1!

	ublas::vector<value_type> cdf = ublasx::cumsum(pi);
	for (size_type i = 0; i < n; ++i)
	{
		if (p <= cdf(i))
		{
			ret = i+1;
			break;
		}
	}
// Alternative (but possibly more time-consuming)
//	ret = ublasx::min(
//			ublasx::which(
//				ublasx::cumsum(pi),
//				::std::bind1st(
//					::std::less_equal<value_type>(),
//					p
//				)
//			)
//		);

	return ret;
}

template <typename VectorExprT, typename URNG>
typename ::boost::numeric::ublas::vector_traits<VectorExprT>::size_type generate_next_state(::boost::numeric::ublas::vector_expression<VectorExprT> const& cdf, URNG& rng)
{
	namespace ublas = ::boost::numeric::ublas;
	namespace ublasx = ::boost::numeric::ublasx;

	typedef typename ublas::vector_traits<VectorExprT>::size_type size_type;
	typedef typename ublas::vector_traits<VectorExprT>::value_type value_type;

	value_type p = generate_probability<value_type>(rng);

	ublas::vector<size_type> ix;

	ix = ublasx::which(
			cdf,
			::std::bind2nd(::std::greater_equal<value_type>(), p)
		);

	if (ix.size() == 0)
	{
		//TODO
		throw ::std::runtime_error("Unexpected failure");
	}

	return ix(0);
}

template <typename ValueT, typename URNG>
ValueT generate_sojourn_time(ValueT rate, URNG& rng)
{
	::dcs::math::stats::exponential_distribution<ValueT> exp(rate);

	return ::dcs::math::stats::rand(exp, rng);
}

}} // Namespace detail::<unnamed>

template <
	typename RealT = double,
	typename PolicyT = ::dcs::math::policies::policy<>
>
class map_distribution//: public base_distribution<RealT>
{
//	private: typedef base_distribution<RealT> base_type;
//	public: typedef typename base_type::value_type value_type;
//	public: typedef typename base_type::support_type support_type;
	public: typedef RealT value_type;
	public: typedef value_type support_type;
	public: typedef ::boost::numeric::ublas::matrix<support_type> matrix_type;
	public: typedef ::std::size_t size_type;
	private: typedef ::boost::numeric::ublas::vector<support_type> value_vector_type;
	private: typedef ::boost::numeric::ublas::vector<size_type> index_vector_type;

	public: template <typename D0MatrixExprT, typename D1MatrixExprT>
		map_distribution(::boost::numeric::ublas::matrix_expression<D0MatrixExprT> const& D0,
						 ::boost::numeric::ublas::matrix_expression<D1MatrixExprT> const& D1)
	: //base_type(),
	  D0_(D0),
	  D1_(D1)
	{
		// pre: num_rows(D0) == num_rows(D1)
		DCS_ASSERT(
			::boost::numeric::ublasx::num_rows(D0) == ::boost::numeric::ublasx::num_rows(D1),
			throw ::std::invalid_argument("[dcs::math::stats::map_distribution::ctor] Matrices D0 and D1 have different number of rows.")
		);
		// pre: num_cols(D0) == num_cols(D1)
		DCS_ASSERT(
			::boost::numeric::ublasx::num_columns(D0) == ::boost::numeric::ublasx::num_columns(D1),
			throw ::std::invalid_argument("[dcs::math::stats::map_distribution::ctor] Matrices D0 and D1 have different number of columns.")
		);
	}

	// Compiler-generated copy-constructor, copy-assignment, and destructor
	// are fine.

	public: matrix_type const& D0() const
	{
		return D0_;
	}

	public: matrix_type const& D1() const
	{
		return D1_;
	}

/*
	private: value_type do_rand(::dcs::math::random::any_generator<value_type>& rng) const
	{
		return rand_sample(rng);
	}

	private: value_type do_rand(::dcs::math::random::base_generator<value_type>& rng) const
	{
		return rand_sample(rng);
	}
*/

	public: template <typename URNG>
		value_type rand(URNG& rng) const
	{
		::std::vector<value_type> samples;
		samples = rand(rng, 1);

		if (samples.size() == 0)
		{
			throw ::std::runtime_error("[dcs::math::stats::map_distribution::rand] Unexpected failure.");
		}

		return samples[0];
	}

	/**
	 * The following algorithm is an adaptation of the one found in the MAP-QN
	 * toolbox (by G. Casale et al, http://www.cs.wm.edu/MAPQN/).
	 */
	public: template <typename URNG>
		::std::vector<value_type> rand(URNG& rng, size_type n) const
	{
		namespace ublas = ::boost::numeric::ublas;
		namespace ublasx = ::boost::numeric::ublasx;

		const size_type max_path_len0(20); // initial max path length
		const size_type path_len_incr(5); // increment in the max path length

		size_type ns(ublasx::num_rows(D0_)); // # stages
		size_type ns2(ns << 1); // 2*ns

		value_vector_type pi;

		pi = detail::equilibrium_distribution(D0_,  D1_);

		size_type s0; // initial stage

		// Randomly generate the initial stage.
		s0 = detail::generate_initial_state(pi, rng);

		matrix_type P(ns,ns2,0);

		for (size_type b=0; b < 2; ++b)
		{
			for (size_type i = 0; i < ns; ++i)
			{
				for (size_type j = 0; j < ns; ++j)
				{
					P(i,b*ns+j) = (b==0 ? D0_(i,j) : D1_(i,j)) / ::std::abs(D0_(i,i));
				}
			}
		}
		for (size_type i=0; i < ns; ++i)
		{
			P(i,i) = 0;
		}

		// Create CDF
		matrix_type C(ns,ns2);
		for (size_type i = 0; i < ns; ++i)
		{
			ublas::row(C, i) = ublasx::cumsum(ublas::row(P, i));
		}
		C = ublasx::abs(C);

		size_type src_s(s0);
		matrix_type V(ublas::zero_matrix<value_type>(n, max_path_len0));
		size_type max_path_len(max_path_len0);
		for (size_type i = 0; i < n; ++i)
		{
			bool arrival(false);
			size_type last(1);
			V(i,0) = src_s;

			while (!arrival)
			{
				size_type dst_s;
				dst_s = detail::generate_next_state(
							ublas::row(C, src_s-1),
							rng
					);
				++dst_s; // make sure that stage id starts from 1
				if (dst_s > ns)
				{
					arrival = true;
					dst_s -= ns;
					src_s = dst_s;
				}
				else
				{
					V(i,last) = dst_s;
					src_s = dst_s;
					++last;
				}
				if (last >= max_path_len)
				{
					max_path_len += path_len_incr;
					V.resize(n, max_path_len, true);
					for (size_type c = last; c < max_path_len; ++c)
					{
						ublas::column(V, c) = ublas::zero_vector<size_type>(n);
					}
				}
			}
		}

//XXX: not used
//		// Create the vector of absorbing stages (for each sample)
//		value_vector_type L(n);
//
//		for (size_type i = 0; i < n; ++i)
//		{
//			size_type ix;
//
//			ix = ublasx::max(
//					ublasx::which(
//						ublas::row(V, i)
//					)
//				);
//			L(i) = V(i,ix);
//		}

//XXX: not used
//		// Create the vector of initial stages (for each sample)
//		value_vector_type F;
//
//		F = ublas::column(V, 1);

		value_vector_type hold_rates(-ublasx::diag(D0_));

		size_type nv = ublasx::num_columns(V);
		matrix_type H(V); // matrix of times to absorptions
		for (size_type i = 0; i < ns; ++i)
		{
			size_type s = i+1;
			for (size_type j = 0; j < n; ++j)
			{
				for (size_type k = 0; k < nv; ++k)
				{
					if (V(j,k) == s)
					{
						H(j,k) = hold_rates(i);
					}
				}
			}
		}

		::std::vector<value_type> samples(n, 0);

		for (size_type i = 0; i < nv; ++i)
		{
			for (size_type k = 0; k < n; ++k)
			{
				if (H(k,i) > 0)
				{
					samples[k] += detail::generate_sojourn_time<value_type>(H(k,i), rng);
				}
			}
		}

		return samples;
	}

	private: matrix_type D0_;
	private: matrix_type D1_;
};

template <
	typename CharT,
	typename CharTraitsT,
	typename RealT,
	typename PolicyT
>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, map_distribution<RealT,PolicyT> const& dist)
{
	return os << "MAP("
			  << "D0=" <<  dist.D0()
			  << ",D1=" <<  dist.D1()
			  << ")";
}

}}} // Namespace dcs:math::stats

#endif // DCS_MATH_STATS_DISTRIBUTION_MAP_HPP


#include <iostream>
#include <cstdlib>
#include <vector>
#include "include/PRM/graphPRM.h"
#include "include/utils/utils.h"
#include "include/astar/astar.h"
#include <boost/graph/random.hpp>

int main(int argc, char **argv)
{
    std::cout << "Running the bug trap demo..." << std::endl;

    //k-dimensional config. space
    int space_dim(2);
    int graph_size(100000);
    int num_landmarks(100);
    //Create a k dimensional environment with default size
    PRM::bugTrap* E = new PRM::bugTrap(50);
    //initialize PRM graph with environment E
    PRM::ProbabilisticRoadMap G(E);

    //Create a PRM on the environment
    G.buildGraph(graph_size);
    //Save the graph to a csv file
    G.write_to_file();

    std::cout << "Number of distance evals: " << G.kd_tree->d_count << std::endl;

    //Query

    PRM::vctr start({5.0,4.0});
    PRM::vctr goal({5.0,1.0});
    std::pair<PRM::ProbabilisticRoadMap::vertex_t,PRM::ProbabilisticRoadMap::vertex_t> query = G.path_query(start,goal);

    //Landmark A*
    std::cout << "\nShortest path query with landmark heuristic " << std::endl;
    ASTAR::LandmarkHeuristics LH(num_landmarks,&G);
    LH.setGoal(query.second);
    ASTAR::astarSearcher A1(&G,&LH,query.first,query.second);
    A1.displayData();
    A1.pathToFile("landmark_shortest_path.csv");
    A1.verticesToFile("landmark_vertices_visited.csv");

    //Euclidean Distance A*
    std::cout << "\nShortest path query with Euclidean distance heuristic " << std::endl;
    ASTAR::EuclidianHeuristics EH(&G,query.second);
    ASTAR::astarSearcher A2(&G,&EH,query.first,query.second);
    A2.displayData();
    A2.pathToFile("euclidean_shortest_path.csv");
    A2.verticesToFile("euclidean_vertices_visited.csv");

    //Dijkstra
    std::cout << "\nShortest path query with a uniform cost search" << std::endl;
    ASTAR::NoHeuristics NH;
    ASTAR::astarSearcher A3(&G,&NH,query.first,query.second);
    A3.displayData();
    A3.pathToFile("dijkstra_shortest_path.csv");
    A3.verticesToFile("dijkstra_vertices_visited.csv");

    return 0;
}


#ifndef TMXPP_UNIT_INTERVAL_HPP
#define TMXPP_UNIT_INTERVAL_HPP

#include <boost/hana/integral_constant.hpp>
#include <tmxpp/Constrained.hpp>

namespace tmxpp {

using Unit_interval =
    Closed_interval<double, boost::hana::int_<0>, boost::hana::int_<1>>;

} // namespace tmxpp

#endif // TMXPP_UNIT_INTERVAL_HPP


/*
@copyright Louis Dionne 2015
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/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/tuple.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_CHECK(
        demux(make<Tuple>)(
            _ + _,
            _ - _,
            _ * _,
            _ / _
        )(10, 4)
        ==
        make<Tuple>(10 + 4, 10 - 4, 10 * 4, 10 / 4)
    );
    //! [main]
}


#ifndef _APPLICATIONS_UTILS_HPP_
#define _APPLICATIONS_UTILS_HPP_

#include <Eigen/Core>

namespace utils {

Eigen::VectorXd create_class_weights(const Eigen::VectorXi & y);

}  // namespace utils

#endif // _APPLICATIONS_UTILS_HPP_


//////////////////////////////////////////////////////////////////////////////
///   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_EULER_FUNCTION_SCALAR_GAMMALN_HPP_INCLUDED
#define NT2_TOOLBOX_EULER_FUNCTION_SCALAR_GAMMALN_HPP_INCLUDED

#include <nt2/toolbox/euler/details/math.hpp>
#include <boost/math/special_functions/gamma.hpp>

#include <nt2/sdk/constant/real.hpp>
#include <nt2/sdk/constant/digits.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type is fundamental_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::gammaln_, tag::cpu_,
                         (A0),
                         (fundamental_<A0>)
                        )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::gammaln_(tag::fundamental_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)> :
      std::tr1::result_of<meta::floating(A0)>{};

    NT2_FUNCTOR_CALL(1)
    {
      if (a0 < Zero<A0>()) return Nan<A0>();
      typedef typename NT2_RETURN_TYPE(1)::type type;
    #ifdef NT2_TOOLBOX_EULER_HAS_LGAMMA
      return ::lgamma(type(a0));
    #else
      return boost::math::lgamma(type(a0));
    #endif
    }

  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type is float_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::gammaln_, tag::cpu_,
                         (A0),
                         (float_<A0>)
                        )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::gammaln_(tag::float_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)> :
      std::tr1::result_of<meta::floating(A0)>{};

    NT2_FUNCTOR_CALL(1)
    {
      if (a0 < Zero<A0>()) return Nan<A0>();
      typedef typename NT2_RETURN_TYPE(1)::type type;
    #ifdef NT2_TOOLBOX_EULER_HAS_LGAMMAF
      return ::lgammaf(type(a0));
    #else
      return boost::math::lgamma(type(a0));
    #endif
    }

  };
} }

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


/*
@copyright Louis Dionne 2015
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/hana/assert.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/functional/infix.hpp>
#include <boost/hana/logical.hpp>
#include <boost/hana/tuple.hpp>

#include <test/injection.hpp>
using namespace boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto implies = infix([](auto p, auto q) {
    return or_(not_(p), q);
});

int main() {
    using test::x;
    BOOST_HANA_CONSTEXPR_LAMBDA auto f = test::injection([]{});
    BOOST_HANA_CONSTEXPR_LAMBDA auto g = test::injection([]{});

    // injectivity
    {
        BOOST_HANA_CONSTEXPR_LAMBDA auto is_injection = [=](auto x, auto y) {
            return equal(f(x), f(y)) ^implies^ equal(x, y);
        };

        BOOST_HANA_CONSTANT_CHECK(all(
            ap(make<Tuple>(is_injection),
                make<Tuple>(x<0>, x<1>, x<2>, x<3>),
                make<Tuple>(x<0>, x<1>, x<2>, x<3>))
        ));
    }

    // various number of arguments
    {
        BOOST_HANA_CONSTANT_CHECK(not_equal(
            f(), g()
        ));

        BOOST_HANA_CONSTANT_CHECK(equal(
            f(), f()
        ));

        BOOST_HANA_CONSTANT_CHECK(not_equal(
            f(x<0>),
            g(x<0>)
        ));

        BOOST_HANA_CONSTANT_CHECK(not_equal(
            f(x<0>, x<1>),
            f(x<0>)
        ));

        BOOST_HANA_CONSTANT_CHECK(equal(
            f(x<0>, x<1>),
            f(x<0>, x<1>)
        ));

        BOOST_HANA_CONSTANT_CHECK(not_equal(
            f(x<0>, x<1>),
            f(x<1>, x<0>)
        ));
    }
}


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/*
 * @file eqtesting.cpp
 * @brief Useful fucntions for equality testing...
 */
#include <NTL/lzz_pXFactoring.h>
NTL_CLIENT
#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"

#include <cassert>
#include <cstdio>

// Map all non-zero slots to 1, leaving zero slots as zero.
// Assumes that r=1, and that all the slot contain elements from GF(p^d).
//
// We compute x^{p^d-1} = x^{(1+p+...+p^{d-1})*(p-1)} by setting y=x^{p-1}
// and then outputting y * y^p * ... * y^{p^{d-1}}, with exponentiation to
// powers of p done via Frobenius.
void mapTo01(const EncryptedArray& ea, Ctxt& ctxt)
{
  long p = ctxt.getPtxtSpace();
  if (p != ctxt.getContext().zMStar.getP()) // ptxt space is p^r for r>1
    std::logic_error("mapTo01 not implemented for r>1");

  if (p>2)
    ctxt.power(p-1); // set y = x^{p-1}

  long d = ea.getDegree();
  if (d>1) { // compute the product of the d automorphisms
    std::vector<Ctxt> v(d, ctxt);
    for (long i=1; i<d; i++)
      v[i].frobeniusAutomorph(i);
    totalProduct(ctxt, v);
  }
}

// computes ctxt^{2^d-1} using a method that takes
// O(log d) automorphisms and multiplications
void fastPower(Ctxt& ctxt, long d)
{
  assert(ctxt.getPtxtSpace()==2);
  if (d <= 1) return;

  Ctxt orig = ctxt;

  long k = NumBits(d);
  long e = 1;

  for (long i = k-2; i >= 0; i--) {
    Ctxt tmp1 = ctxt;
    tmp1.smartAutomorph(1L << e);
    ctxt.multiplyBy(tmp1);
    e = 2*e;

    if (bit(d, i)) {
      ctxt.smartAutomorph(2);
      ctxt.multiplyBy(orig);
      e += 1;
    }
  }
}

// ===> This function only works for p=2, r=1 <===
// Test if prefixes of bits in slots are all zero: Set slot j of res[i] to 0
// if bits 0..i of j'th slot in ctxt are all zero, else it is set to 1
// It is assumed that res and the res[i]'s are initialized by the caller.
// Complexity: O(d + n log d) smart automorphisms
//             O(n d)
void incrementalZeroTest(Ctxt* res[], const EncryptedArray& ea,
			 const Ctxt& ctxt, long n)
{
  FHE_TIMER_START;
  long nslots = ea.size();
  long d = ea.getDegree();

  // compute linearized polynomial coefficients

  vector< vector<ZZX> > Coeff;
  Coeff.resize(n);

  for (long i = 0; i < n; i++) {
    // coeffients for mask on bits 0..i
    // L[j] = X^j for j = 0..i, L[j] = 0 for j = i+1..d-1

    vector<ZZX> L;
    L.resize(d);

    for (long j = 0; j <= i; j++)
      SetCoeff(L[j], j);

    vector<ZZX> C;

    ea.buildLinPolyCoeffs(C, L);

    Coeff[i].resize(d);
    for (long j = 0; j < d; j++) {
      // Coeff[i][j] = to the encoding that has C[j] in all slots
      // FIXME: maybe encrtpted array should have this functionality
      //        built in
      vector<ZZX> T;
      T.resize(nslots);
      for (long s = 0; s < nslots; s++) T[s] = C[j];
      ea.encode(Coeff[i][j], T);
    }
  }

  vector<Ctxt> Conj(d, ctxt);
  // initialize Cong[j] to ctxt^{2^j}
  for (long j = 0; j < d; j++) {
    Conj[j].smartAutomorph(1L << j);
  }

  for (long i = 0; i < n; i++) {
    res[i]->clear();
    for (long j = 0; j < d; j++) {
      Ctxt tmp = Conj[j];
      tmp.multByConstant(Coeff[i][j]);
      *res[i] += tmp;
    }

    // *res[i] now has 0..i in each slot
    // next, we raise to the power 2^d-1

    fastPower(*res[i], d);
  }
  FHE_TIMER_STOP;
}

#ifdef DEBUG_TEST
/************************** debugging code below *********************/
void printBits(const vector<ZZX>& v, long n)
{
  long len = v.size();
  if (n>50 || len>32) return;
  for (long i = 0; i < len; i++) {
    for (long j = n-1; j >= 0; j--)
      cout << coeff(v[i], j);
    cout << " ";
  }
  cout << "\n";
}

void  TestIt(long c, long k, long w, long L, long m, long n)
{
  FHEcontext context(m, 2, 1); // p = 2, r = 1
  long d = context.zMStar.getOrdP();

  buildModChain(context, L, c);

  context.zMStar.printout();
  cerr << endl;
#ifdef DEBUG
  cerr << context << endl;
#endif

  FHESecKey secretKey(context);
  const FHEPubKey& publicKey = secretKey;
  secretKey.GenSecKey(w); // A Hamming-weight-w secret key

  ZZX G;

  G = makeIrredPoly(2, d);
  // G = context.alMod.getFactorsOverZZ()[0];

  cerr << "generating key-switching matrices... ";
  addFrbMatrices(secretKey);
  addSome1DMatrices(secretKey);
  cerr << "done\n";

  cerr << "computing masks and tables for rotation...";
  EncryptedArray ea(context, G);
  cerr << "done\n";

  long nslots = ea.size();

  if (n <= 0 || n > d) n = d;

  vector<ZZX> v;
  v.resize(nslots);
  for (long i = 0; i < nslots; i++) {
    GF2X f;
    random(f, n);
    conv(v[i], f);
  }

  printBits(v, n);

  Ctxt ctxt(publicKey);
  ea.encrypt(ctxt, publicKey, v);
  // ctxt encrypts a vector where each slots is a random
  // polynomial of degree < n

  Ctxt* res[n];
  for (long j = 0; j < n; j++) res[j] = new Ctxt(publicKey); // allocate

  resetAllTimers();

  incrementalZeroTest(res, ea, ctxt, n);

  for (long j = 0; j < n; j++) {
    vector<ZZX> v1;
    ea.decrypt(*res[j], secretKey, v1);
    printBits(v1, n);
  }

  for (long j = 0; j < n; j++) delete res[j]; // cleanup
}

void usage(char *prog)
{
  cerr << "Usage: "<<prog<<" [ optional parameters ]...\n";
  cerr << "  optional parameters have the form 'attr1=val1 attr2=val2 ...'\n";
  cerr << "  e.g, 'R=4 L=9 k=80'\n\n";
  cerr << "  c is number of columns in the key-switching matrices [default=2]\n";
  cerr << "  k is the security parameter [default=80]\n";
  cerr << "  L is the # of primes in the modulus chain [default=20]\n";
  cerr << "  m is a specific modulus\n";
  cerr << "  n is the number of masks (defaults to all)\n";
  exit(0);
}

int main(int argc, char *argv[])
{
  argmap_t argmap;
  argmap["c"] = "2";
  argmap["k"] = "80";
  argmap["L"] = "20";
  argmap["m"] = "0";
  argmap["n"] = "0";

  // get parameters from the command line
  if (!parseArgs(argc, argv, argmap)) usage(argv[0]);

  long c = atoi(argmap["c"]);
  long k = atoi(argmap["k"]);
  long L = atoi(argmap["L"]);
  long chosen_m = atoi(argmap["m"]);
  long n = atoi(argmap["n"]);

  long w = 64; // Hamming weight of secret key
  //  long L = z*R; // number of levels

  long m = FindM(k, L, c, 2, 1, 0, chosen_m, true);

  setTimersOn();
  TestIt(c, k, w, L, m, n);

  cerr << endl;
  printAllTimers();
  cerr << endl;

}
// call to get our running test case:
// eqtesting_x m=20485
// eqtesting_x m=105 for quick testing

#endif // #ifdef DEBUG_TEST


#include "cloud_persistence.h"
#include <fstream>

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Eigen>
#include <Eigen/Sparse>

#define TOK(expected_tok) \
  file >> tok; \
  ASSERT(not file.fail(), \
      "failed to read expected token '" expected_tok "'"); \
  ASSERT_EQ(tok, expected_tok);

#define PARSE(type, variable) \
  type variable; \
  file >> variable; \
  ASSERT(not file.fail(), \
      "failed to parse variable '" #variable "' of type " #type);

namespace Cloud
{

//----( persistence )---------------------------------------------------------

static const int FILE_VERSION = 6;

void write_matrix (const MatrixSf & A, ostream & o)
{
  LOG("  writing " << A.nonZeros() << " entries of "
      << A.rows() << " x " << A.cols() << " sparse matrix");

  o << "\n  SparseMatrix<float>"
    << "\n   shape = " << A.rows() << " " << A.cols()
    << "\n   " << A.nonZeros() << " entries (row,col,value)";

  for (int i = 0; i < A.outerSize(); ++i) {

    for (MatrixSf::InnerIterator iter(A,i); iter; ++iter) {

      o << "\n   " << iter.row() << " " << iter.col() << " " << iter.value();
    }
  }
}

void read_matrix (MatrixSf & A, istream & file)
{
  string tok;

  TOK("SparseMatrix<float>")
  TOK("shape") TOK("=") PARSE(int, I) PARSE(int, J);
  PARSE(int, N); TOK("entries"); TOK("(row,col,value)");

  LOG("  reading " << N << " entries of "
      << I << " x " << J << " sparse matrix");

  A.resize(I,J);
  for (int n = 0; n < N; ++n) {

    PARSE(int, row)
    PARSE(int, col)
    PARSE(float, value)

    A.insert(row, col) = value;
  }

  A.finalize();
}

void Persistent::save (string filename) const
{
  LOG("saving to file " << filename);
  filestem = filename;

  std::ofstream o(filename);

  o << "kazoo cloud file";

  o << "\n""version " << FILE_VERSION;

  write(o);

  o << "\n"
    << "\n""end"
    << "\n";
}

void Persistent::load (string filename)
{
  LOG("loading from file " << filename);

  std::ifstream file(filename);
  ASSERT(file, "failed to open " << filename);

  string tok;

  TOK("kazoo") TOK("cloud") TOK("file")

  TOK("version") PARSE(int, version)
  ASSERT_EQ(version, FILE_VERSION);

  read(file);

  TOK("end")
}

} // namespace Cloud