/*
 *  This file is a part of HAXX
 *
 *  Copyright (c) 2017 David Williams-Young
 *  All rights reserved.
 *
 *  See LICENSE.txt
 */

#ifdef BOOST_TEST_MODULE
  #undef BOOST_TEST_MODULE
#endif

#define BOOST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/iterator/counting_iterator.hpp>

#include "haxx.hpp"

#include <random>
#include <iterator>
#include <iostream>
#include <limits>
#include <chrono>

// Length constants
#define HBLAS1_VECLEN 100
#define HBLAS2_MATLEN (HBLAS1_VECLEN) * (HBLAS1_VECLEN)
#define HBLAS1_RAND_MIN -20
#define HBLAS1_RAND_MAX 54

// Setup Random Number generator
static std::random_device rd;
static std::mt19937 gen(rd());
static std::uniform_real_distribution<> dis(HBLAS1_RAND_MIN,HBLAS1_RAND_MAX);

template <typename _F> _F genRandom();
template<> inline double genRandom<double>(){ return double(dis(gen)); }
template<> inline std::complex<double> genRandom<std::complex<double>>(){
  return std::complex<double>(dis(gen),dis(gen));
}
template<> inline HAXX::quaternion<double> genRandom<HAXX::quaternion<double>>(){
  return HAXX::quaternion<double>(dis(gen),dis(gen),dis(gen),dis(gen));
}

// Index list for HBLAS1 UT conformation
static std::vector<int> indx(boost::counting_iterator<int>(0),
  boost::counting_iterator<int>(HBLAS1_VECLEN));

// Strides to be tested
static std::vector<size_t> strides = {1,2,3,5,9};

#define COMPARE_TOL 1e-12
#define CMP_Q(a,b) ( HAXX::norm(((a) * HAXX::inv(b))- 1.) < COMPARE_TOL )


/* This file is part of the Tomographer project, which is distributed under the
 * terms of the MIT license.
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2016 ETH Zurich, Institute for Theoretical Physics, Philippe Faist
 * Copyright (c) 2017 Caltech, Institute for Quantum Information and Matter, Philippe Faist
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <cmath>

#include <string>
#include <iostream>
#include <random>

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

// definitions for Tomographer test framework -- this must be included before any
// <Eigen/...> or <tomographer/...> header
#include "test_tomographer.h"

#include <tomographer/tools/eigenutil.h>
#include <tomographer/tools/cxxutil.h>

// -----------------------------------------------------------------------------
// fixture(s)

// -----------------------------------------------------------------------------
// test suites

BOOST_AUTO_TEST_SUITE(test_mathtools_eigenutil)

BOOST_AUTO_TEST_CASE(denseRandom)
{
  std::mt19937 rng;
  std::uniform_real_distribution<double> dist(0.0, 1.0);
  std::uniform_real_distribution<float> distf(0.f, 1.f);

  constexpr int N = 10000;

  Eigen::VectorXd v(Tomographer::Tools::denseRandom<Eigen::VectorXd>(rng, dist, N));
  MY_BOOST_CHECK_FLOATS_EQUAL(v.sum(), 0.5*N, 2.0/std::sqrt(N)) ;

  Eigen::VectorXd v2(Tomographer::Tools::denseRandom<Eigen::Matrix<double,N,1> >(rng, dist, N));
  MY_BOOST_CHECK_FLOATS_EQUAL(v2.sum(), 0.5*N, 2.0/std::sqrt(N)) ;

  Eigen::Matrix<float,N,1> v3(Tomographer::Tools::denseRandom<Eigen::Matrix<float,Eigen::Dynamic,1> >(rng, distf, N));
  MY_BOOST_CHECK_FLOATS_EQUAL(v3.sum(), 0.5*N, 2.f/std::sqrt((float)N)) ;
}

BOOST_AUTO_TEST_CASE(canonicalBasisVec_1)
{
  auto v1 = Tomographer::Tools::canonicalBasisVec<Eigen::VectorXd>(3, 10);
  Eigen::VectorXd v2(10); v2 << 0,0,0,1,0,0,0,0,0,0;
  MY_BOOST_CHECK_EIGEN_EQUAL(v1, v2, tol);
}
BOOST_AUTO_TEST_CASE(canonicalBasisVec_2)
{
  auto v1 = Tomographer::Tools::canonicalBasisVec<Eigen::Matrix<double,10,1> >(3, 10);
  Eigen::VectorXd v2(10); v2 << 0,0,0,1,0,0,0,0,0,0;
  MY_BOOST_CHECK_EIGEN_EQUAL(v1, v2, tol);
}
BOOST_AUTO_TEST_CASE(canonicalBasisVec_mat)
{
  auto m1 = Tomographer::Tools::canonicalBasisVec<Eigen::Matrix<double,3,3> >(1,2, 3,3);
  Eigen::Matrix3d m2; m2 << 0,0,0, 0,0,1, 0,0,0 ;
  MY_BOOST_CHECK_EIGEN_EQUAL(m1, m2, tol);
}

BOOST_AUTO_TEST_SUITE(powersOfTwo)

BOOST_AUTO_TEST_CASE(basic)
{
  Eigen::VectorXd v1 = Tomographer::Tools::powersOfTwo<Eigen::VectorXd>(10);
  Eigen::VectorXd v2(10); v2 << 1, 2, 4, 8, 16, 32, 64, 128, 256, 512;
  MY_BOOST_CHECK_EIGEN_EQUAL(v1, v2, tol);
}
BOOST_AUTO_TEST_CASE(mat)
{
  Eigen::Matrix3d m1 = Tomographer::Tools::powersOfTwo<Eigen::Matrix3d>();
  Eigen::Matrix3d m2;
  m2 << 1,  8, 64,
        2, 16, 128,
        4, 32, 256;
  MY_BOOST_CHECK_EIGEN_EQUAL(m1, m2, tol);
}
BOOST_AUTO_TEST_CASE(fixed)
{
  Eigen::Array<double,1,9> twopows = Tomographer::Tools::powersOfTwo<Eigen::Array<double,1,9> >().transpose();
  Eigen::Array<double,1,9> correct_twopows;
  correct_twopows << 1, 2, 4, 8, 16, 32, 64, 128, 256;

  MY_BOOST_CHECK_EIGEN_EQUAL(twopows, correct_twopows, tol);
}

BOOST_AUTO_TEST_CASE(dyn_vector)
{
  Eigen::VectorXd twopows(6);
  twopows = Tomographer::Tools::powersOfTwo<Eigen::VectorXd>(6);
  BOOST_MESSAGE("twopows = " << twopows);
  Eigen::VectorXd correct_twopows(6);
  (correct_twopows << 1, 2, 4, 8, 16, 32).finished();

  MY_BOOST_CHECK_EIGEN_EQUAL(twopows, correct_twopows, tol);
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


//
// Copyright 2021 Prathamesh Tagore <prathameshtagore@gmail.com>
//
// 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)
//

#include <boost/gil/extension/io/jpeg.hpp>
#include <boost/gil.hpp>

namespace gil = boost::gil;

// Demonstrates the use of a rasterizer to generate an image of an ellipse
// The various rasterizers available are defined in include/boost/gil/rasterization/circle.hpp,
// include/boost/gil/rasterization/ellipse.hpp and include/boost/gil/rasterization/line.hpp
// The rasterizer used is a generalisation of the midpoint algorithm often used for drawing circle.
// This examples also shows how to create images with various pixel depth, as well as the behaviour
// in case of the rasterization of a curve that doesn't fit in a view.
// See also:
// rasterizer_circle.cpp - Demonstrates the use of a rasterizer to generate an image of a circle
// rasterizer_line.cpp - Demonstrates the use of a rasterizer to generate an image of a line

int main()
{
    // Syntax for usage :-
    // auto rasterizer = gil::midpoint_elliptical_rasterizer{};
    // rasterizer(img_view, colour, center, semi-axes_length);
    // Where
    // img_view : gil view of the image on which ellipse is to be drawn.
    // colour : Vector containing channel intensity values for img_view. Number of colours
    // provided must be equal to the number of channels present in img_view.
    // center : Array containing positive integer x co-ordinate and y co-ordinate of the center
    // respectively.
    // semi-axes_length : Array containing positive integer lengths of horizontal semi-axis
    // and vertical semi-axis respectively.

    gil::gray8_image_t gray_buffer_image(256, 256);
    auto gray_elliptical_rasterizer = gil::midpoint_elliptical_rasterizer{};
    gray_elliptical_rasterizer(view(gray_buffer_image), {128}, {128, 128}, {100, 50});

    gil::rgb8_image_t rgb_buffer_image(256, 256);
    auto rgb_elliptical_rasterizer = gil::midpoint_elliptical_rasterizer{};
    rgb_elliptical_rasterizer(view(rgb_buffer_image), {0, 0, 255}, {128, 128}, {50, 100});

    gil::rgb8_image_t rgb_buffer_image_out_of_bound(256, 256);
    auto rgb_elliptical_rasterizer_out_of_bound = gil::midpoint_elliptical_rasterizer{};
    rgb_elliptical_rasterizer_out_of_bound(view(rgb_buffer_image_out_of_bound), {255, 0, 0},
        {100, 100}, {160, 160});

    gil::write_view("rasterized_ellipse_gray.jpg", view(gray_buffer_image), gil::jpeg_tag{});
    gil::write_view("rasterized_ellipse_rgb.jpg", view(rgb_buffer_image), gil::jpeg_tag{});
    gil::write_view("rasterized_ellipse_rgb_out_of_bound.jpg", view(rgb_buffer_image_out_of_bound),
        gil::jpeg_tag{});
}


/*********************************************************************************
 *
 * Inviwo - Interactive Visualization Workshop
 *
 * Copyright (c) 2017-2020 Inviwo Foundation
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *********************************************************************************/

#include <modules/eigenutils/processors/eigennormalize.h>

#include <Eigen/Geometry>

namespace inviwo {

// The Class Identifier has to be globally unique. Use a reverse DNS naming scheme
const ProcessorInfo EigenNormalize::processorInfo_{
    "org.inviwo.EigenNormalize",  // Class identifier
    "Matrix Normalization",       // Display name
    "Eigen",                      // Category
    CodeState::Experimental,      // Code state
    "Eigen",                      // Tags
};
const ProcessorInfo EigenNormalize::getProcessorInfo() const { return processorInfo_; }

EigenNormalize::EigenNormalize()
    : Processor()
    , in_("in")
    , out_("out")

    , method_("method", "Method",
              {{"maxelement", "Max Element", Method::MaxElement},
               {"minmaxelement", "Min/Max Element", Method::MinMaxElement},
               {"normalize", "Normalize", Method::Normalize}}) {

    addPort(in_);
    addPort(out_);

    addProperty(method_);
}

void EigenNormalize::process() {
    auto m = in_.getData();
    switch (method_.get()) {
        case Method::MaxElement: {
            auto maxV = m->maxCoeff();
            out_.setData(std::make_shared<Eigen::MatrixXf>((*m) / maxV));
            break;
        }
        case Method::MinMaxElement: {
            auto minV = m->minCoeff();
            auto maxV = m->maxCoeff();
            auto m2 = std::make_shared<Eigen::MatrixXf>(*m);
            m2->array() -= minV;
            m2->array() /= maxV - minV;
            out_.setData(m2);
            break;
        }
        case Method::Normalize:
            out_.setData(std::make_shared<Eigen::MatrixXf>(m->normalized()));
            break;
    }
}

}  // namespace inviwo


//
// Created by Ragesh on 5/17/18.
//

#ifndef PROJECT_RAY_TRACE_ITERATOR_HPP
#define PROJECT_RAY_TRACE_ITERATOR_HPP

/**
 * This is the header file which defines the prototype of class and functions for performing
 * ray tracing operation on laser beam rays. The main class is a iterator which is derived from
 * iterator_facade class which a part of the iterator module in the boost library.
 */

// Boost libraries
#include <boost/iterator/iterator_facade.hpp>

// C libraries
#include <cassert>
#include <cmath>
#include <cstdlib>
#include <cstdio>

// C++ libraries
#include <limits>
#include <stdexcept>
#include <utility>

namespace NS_occupancy_grid {

template<typename T>
inline int signum(T val) {
  /// A template based implementation signum function.
  /// T is the template type
  /**
   * \param val : the input to the signum function
   *
   */
  return ((T(0) < val) - (val < T(0)));
}

template<typename real_t, typename int_t>
class ray_trace_iterator : public boost::iterator_facade<ray_trace_iterator<real_t, int_t>, std::pair<int_t, int_t>,
                           boost::forward_traversal_tag, std::pair<int_t, int_t>>
{

private:

typedef typename boost::iterator_facade<ray_trace_iterator<real_t, int_t>, std::pair<int_t, int_t>,
boost::forward_traversal_tag, std::pair<int_t, int_t>> super_t;
// Input arguments
real_t px_, py_, dx_, dy_, origin_x, origin_y, cell_size_x, cell_size_y;

// Intermediate variables for faster computation

/// integral steps (direction) (-1, 0, 1) in both x and y
int_t dir_x, dir_y;
/// distance to the nearest grid line
real_t ex_, ey_;
/// Maximum time to collision (from one grid line to next)
real_t Tx_, Ty_;

// State of iterator
/// Grid index
int_t i_, j_;
/// time to collision to next grid line
real_t tx_, ty_;

public:

// constructor
ray_trace_iterator(real_t px, real_t py, real_t dx, real_t dy, real_t origin_x_, real_t origin_y_,
    real_t cell_size_x_, real_t cell_size_y_) :
px_{px}, py_{py},
dx_{dx}, dy_{dy},
origin_x{origin_x_}, origin_y{origin_y_},
cell_size_x{cell_size_x_}, cell_size_y{cell_size_y_} {
/**
 * The constructor for the ray trace iterator.
 *
 * \param px : the start point x coordinate
 * \param py : the start point y coordinate
 * \param dx : the increment along x axis
 * \param dy : the increment along y axis
 * \param origin_x_ : the x coordinate of the map bottom left corner
 * \param origin_y_ : the y coordinate of the map bottom left corner
 * \param cell_size_x_ : the size of the cell along x
 * \param cell_size_y_ : the size of the cell along y
 *
 */

// shift the coordinates to zero the origin
px = px - origin_x_;
py = py - origin_y_;

// grid cell containing (px,py)
i_ = static_cast<int_t>(std::floor(px / cell_size_x_));
j_ = static_cast<int_t>(std::floor(py / cell_size_y_));

dir_x = signum(dx);
dir_y = signum(dy);

// whether the grid line we are going to hit is floor() or ceil()
// depends on the direction in which the ray is moving
// using the fact that ceil() = floor() + 1
int_t floor_or_ceil_x = (dir_x > 0) ? 1 : 0;
int_t floor_or_ceil_y = (dir_y > 0) ? 1 : 0;

// uncomment lines below for debugging
//printf("\n Cell: (%i, %i), dx dy: (%f,%f) \n", i_, j_, dx, dy);

// distance to the nearest grid line
ex_ = std::fabs((i_ + floor_or_ceil_x) * cell_size_x_ - px);
ey_ = std::fabs((j_ + floor_or_ceil_y) * cell_size_y_ - py);

// (max) time to collision from one grid line to another
Tx_ = (dx == 0) ? std::numeric_limits<real_t>::infinity() : cell_size_x_ / std::fabs(dx);
Ty_ = (dy == 0) ? std::numeric_limits<real_t>::infinity() : cell_size_y_ / std::fabs(dy);

// time to collision from this position
tx_ = (dx == 0) ? std::numeric_limits<real_t>::infinity() : ex_ / fabs(dx);
ty_ = (dy == 0) ? std::numeric_limits<real_t>::infinity() : ey_ / fabs(dy);

if (!((tx_ >= 0) && (ty_ >= 0))) {
printf("t:(%f, %f), direction:(%f, %f), position:(%f, %f), cell:(%d, %d), cell size:(%f, %f)\n",
tx_, ty_, dx, dy, px, py, i_, j_, cell_size_x, cell_size_y);
throw std::logic_error("tx < 0 or ty < 0");
}

// time is always positive
assert(tx_ >= 0);
assert(ty_ >= 0);

}

typename super_t::reference dereference() const {
  return std::make_pair(i_, j_);
}

bool equal(ray_trace_iterator it) const {
  /// function to check if the objects are equal
  return ((it.i_ == i_) && (it.j_ == j_) &&
      (it.tx_ == tx_) && (it.ty_ == ty_) &&
      (it.Tx_ == Tx_) && (it.Ty_ == Ty_) &&
      (it.dir_x == dir_x) && (it.dir_y == dir_y));
}

/// overload the == operator
//      friend bool operator== (const ray_trace_iterator<real_t, int_t>& it1,
//                              const ray_trace_iterator<real_t, int_t>& it2);
friend bool operator==(const ray_trace_iterator<real_t, int_t> &it1, const ray_trace_iterator<real_t, int_t> &it2) {
  return ((it1.i_ == it2.i_) && (it1.j_ == it2.j_) &&
      (it1.tx_ == it2.tx_) && (it1.ty_ == it2.ty_) &&
      (it1.Tx_ == it2.Tx_) && (it1.Ty_ == it2.Ty_) &&
      (it1.dir_x == it2.dir_x) && (it1.dir_y == it2.dir_y));
}

void increment();

std::pair<real_t, real_t> real_position() const;

};

template<typename real_t, typename int_t>
void ray_trace_iterator<real_t, int_t>::increment() {
  if (tx_ < ty_) {
    i_ += dir_x;
    ty_ = ty_ - tx_;
    tx_ = Tx_;
  } else {
    j_ += dir_y;
    tx_ = tx_ - ty_;
    ty_ = Ty_;
  }
}

template<typename real_t, typename int_t>
std::pair<real_t, real_t> ray_trace_iterator<real_t, int_t>::real_position() const {

  // whether the grid line we are going to hit is floor() or ceil()
  // depends on the direction ray is moving
  int_t floor_or_ceil_x = (dir_x > 0) ? 1 : 0;
  int_t floor_or_ceil_y = (dir_y > 0) ? 1 : 0;

  real_t ex = (dx_ == 0) ? ex_ // error is same as starting point
                         : tx_ * fabs(dx_);
  real_t ey = (dy_ == 0) ? ey_
                         : ty_ * fabs(dy_);

  real_t px = (i_ + floor_or_ceil_x) * cell_size_x - ex * dir_x;
  real_t py = (j_ + floor_or_ceil_y) * cell_size_y - ey * dir_y;

  // shift coordinates
  px = px + origin_x;
  py = py + origin_y;

  return std::make_pair(px, py);
}

}

#endif //PROJECT_RAY_TRACE_ITERATOR_HPP


#ifndef __SCHEME_UTILITY_RATIONALTYPE
#define __SCHEME_UTILITY_RATIONALTYPE
#include "utility/bigint.hpp"
#include <boost/operators.hpp>
#include <iosfwd>

class RationalType: public
                    boost::totally_ordered<RationalType,
                    boost::arithmetic<RationalType>
                    >
{
    BigInt up_, down_;
    void reduce();
    void lazyreduce();
    public:
    RationalType();
    RationalType(const BigInt& num);
    RationalType(const BigInt& up, const BigInt& down);
    explicit RationalType(double a);
    RationalType& operator += (const RationalType& b);
    RationalType& operator -= (const RationalType& b);
    RationalType& operator *= (const RationalType& b);
    RationalType& operator /= (const RationalType& b);
    bool operator == (const RationalType& b) const;
    bool operator < (const RationalType& b) const;
    friend std::istream& operator >>(std::istream& i, RationalType& a);
    friend std::ostream& operator <<(std::ostream& o, const RationalType& a);
    operator long double() const;
    RationalType operator -();
    bool getSign() const;
    bool isInt() const;
    BigInt getUp() const;
    BigInt getDown() const;
    BigInt toInt() const;
};

#endif


/*
 * Chaos
 *
 * Copyright 2015 Operating Systems Laboratory EPFL
 *
 * 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.
 */

#include <cstdio>
#include <boost/thread/thread.hpp>
#include "defs.h"
#include "options.h"
#include "util.h"
#include "output.h"
#include "prng/splittable_mrg.h"
#include "prng/utils.h"

static void generate_edge(mrg_state *pstate, const int scale,
                          double a, double b, double c, double d,
                          struct edge_struct *edge) {
  vertex_t i = 0, j = 0;
  vertex_t bit = (vertex_t) 1 << (scale - 1);

  while (true) {
    double r = mrg_get_double_orig(pstate);
    if (r > a) {               /* outside quadrant 1 */
      if (r <= a + b)          /* in quadrant 2 */
        j |= bit;
      else if (r <= a + b + c) /* in quadrant 3 */
        i |= bit;
      else {                   /* in quadrant 4 */
        j |= bit;
        i |= bit;
      }
    }

    if (1 == bit) break;

    /*
     * Noise is introduced by modifying the probabilites by +/- 5%
     * and normalising them.
     */
#ifdef NOISE
    r  = mrg_get_double_orig(pstate);
    a *= 0.95 + r/10;
    r  = mrg_get_double_orig(pstate);
    b *= 0.95 + r/10;
    r  = mrg_get_double_orig(pstate);
    c *= 0.95 + r/10;
    r  = mrg_get_double_orig(pstate);
    d *= 0.95 + r/10;

    double norm = 1.0 / (a + b + c + d);
    a *= norm;
    b *= norm;
    c *= norm;
    d = 1.0 - (a + b + c);
#endif

    /* Iterates scale times. */
    bit >>= 1;
  }

  edge->src = i;
  edge->dst = j;
#ifdef WEIGHT
  edge->weight = (value_t)mrg_get_double_orig(pstate);
#endif
}

// ugly, but max 10 args to functions allowed in boost::thread
static unsigned int xscale_interval;
static unsigned int xscale_node;

static void generate(thread_buffer *buffer, const mrg_state &state, const int
scale, const edge_t start, const edge_t end,
                     const double a, const double b, const double c, /*const
                     double d,*/ const bool symmetric) {
  const double d = 1 - (a + b + c);
  for (edge_t ei = start; ei < end; ++ei) {
    if (ei % xscale_interval != xscale_node) { continue; }
    mrg_state new_state = state;
    mrg_skip(&new_state, 0, ei, 0);
    struct edge_struct *edge = buffer->edge_struct();
    generate_edge(&new_state, scale, a, b, c, d, edge);
    if (symmetric) {
      struct edge_struct *reverse_edge = buffer->edge_struct();
      reverse_edge->src = edge->dst;
      reverse_edge->dst = edge->src;
#ifdef WEIGHT
      reverse_edge->weight = edge->weight;
#endif
    }
  }
  buffer->flush();
}

int main(int argc, char **argv) {
  struct options options;
  if (process_options(argc, argv, true, &options) != 0)
    return 0;

  if (options.rmat.a + options.rmat.b + options.rmat.c >= 1) {
    printf("Error: The sum of probabilities must equal 1\n");
    return 0;
  }
  double d = 1 - (options.rmat.a + options.rmat.b + options.rmat.c);
  xscale_node = options.rmat.xscale_node;
  xscale_interval = options.rmat.xscale_interval;
  uint_fast32_t seed[5];
  make_mrg_seed(options.rng.userseed1, options.rng.userseed2, seed);
  mrg_state state;
  mrg_seed(&state, seed);
  //mrg_skip(&new_state, 50, 7, 0); // Do an initial skip?

  edge_t total_edges = options.rmat.edges;
  if ((total_edges % options.rmat.xscale_interval) > options.rmat.xscale_node) {
    total_edges /= options.rmat.xscale_interval;
    total_edges++;
  }
  else {
    total_edges /= options.rmat.xscale_interval;
  }

  if (options.global.symmetric) {
    total_edges *= 2;
  }

  printf("Generator type: R-MAT\n");
  printf("Scale: %d (%" PRIu64 " vertices)\n", options.rmat.scale, ((uint64_t) 1 << options.rmat.scale));
  printf("Edges: %" PRIet "\n", total_edges);
  printf("Probabilities: A=%4.2f, B=%4.2f, C=%4.2f, D=%4.2f\n", options.rmat.a, options.rmat.b, options.rmat.c, d);

  double start = get_time();

  // io thread
  size_t buffer_size = calculate_buffer_size(options.global.buffer_size);
  buffer_queue flushq;
  buffer_manager manager(&flushq, options.global.buffers_per_thread, buffer_size);
  io_thread_func io_func(options.global.graphname.c_str(), total_edges, &flushq, &manager, buffer_size);
  boost::thread io_thread(boost::ref(io_func));

  // worker threads
  int nthreads = options.global.nthreads;
  edge_t edges_per_thread = options.rmat.edges / nthreads;
  threadid_t *workers[nthreads];
  boost::thread *worker_threads[nthreads];
  for (int i = 0; i < nthreads; i++) {
    workers[i] = new threadid_t(i);
    thread_buffer *buffer = manager.register_thread(*workers[i]);
    // last thread gets the remainder (if any)
    edge_t start = i * edges_per_thread;
    edge_t end = (i == nthreads - 1) ? (options.rmat.edges) : ((i + 1) * edges_per_thread);
    worker_threads[i] = new boost::thread(generate, buffer,
                                          state, options.rmat.scale, start, end,
                                          options.rmat.a, options.rmat.b, options.rmat.c, /*d,*/
                                          options.global.symmetric);
  }

  // Wait until work completes
  for (int i = 0; i < nthreads; i++) {
    worker_threads[i]->join();
  }
  io_func.stop();
  io_thread.join();

  // cleanup
  for (int i = 0; i < nthreads; i++) {
    manager.unregister_thread(*workers[i]);
    delete worker_threads[i];
    delete workers[i];
  }

  double elapsed = get_time() - start;
  printf("Generation time: %fs\n", elapsed);

  make_ini_file(options.global.graphname.c_str(), (uint64_t) 1 << options.rmat.scale, total_edges);

  return 0;
}


/*
@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/pair.hpp>
using namespace boost::hana;

int main() {

{

//! [make<Pair>]
BOOST_HANA_CONSTEXPR_CHECK(first(make<Pair>(1, 'x')) == 1);
BOOST_HANA_CONSTEXPR_CHECK(second(make<Pair>(1, 'x')) == 'x');
//! [make<Pair>]

}{

//! [make_pair]
BOOST_HANA_CONSTEXPR_CHECK(make_pair(1, 'x') == make<Pair>(1, 'x'));
//! [make_pair]

}{

//! [comparable]
BOOST_HANA_CONSTEXPR_CHECK(make<Pair>(1, 'x') == make<Pair>(1, 'x'));
BOOST_HANA_CONSTEXPR_CHECK(make<Pair>(2, 'x') != make<Pair>(1, 'x'));
BOOST_HANA_CONSTEXPR_CHECK(make<Pair>(1, 'y') != make<Pair>(1, 'x'));
//! [comparable]

}{

//! [orderable]
BOOST_HANA_CONSTEXPR_CHECK(make<Pair>(1, 'x') < make<Pair>(1, 'y'));
BOOST_HANA_CONSTEXPR_CHECK(make<Pair>(1, 'x') < make<Pair>(10, 'x'));
BOOST_HANA_CONSTEXPR_CHECK(make<Pair>(1, 'y') < make<Pair>(10, 'x'));
//! [orderable]

}{

//! [foldable]
BOOST_HANA_CONSTEXPR_CHECK(foldl(make<Pair>(1, 3), 0, plus) == 4);
BOOST_HANA_CONSTEXPR_CHECK(foldr(make<Pair>(1, 3), 0, minus) == -2);
//! [foldable]

}{

//! [product]
BOOST_HANA_CONSTEXPR_CHECK(first(make<Pair>(1, 'x')) == 1);
BOOST_HANA_CONSTEXPR_CHECK(second(make<Pair>(1, 'x')) == 'x');
//! [product]

}

}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map.hpp>
#include <boost/ref.hpp>
#include <vector>

#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace boost;

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

  typedef adjacency_list
    < vecS,
      vecS,
      undirectedS,
      property<vertex_index_t, int>,
      property<edge_index_t, int>
    >
    graph;

  // Create a maximal planar graph on 6 vertices
  graph g(6);

  add_edge(0,1,g);
  add_edge(1,2,g);
  add_edge(2,3,g);
  add_edge(3,4,g);
  add_edge(4,5,g);
  add_edge(5,0,g);

  add_edge(0,2,g);
  add_edge(0,3,g);
  add_edge(0,4,g);

  add_edge(1,3,g);
  add_edge(1,4,g);
  add_edge(1,5,g);

  // Initialize the interior edge index
  property_map<graph, edge_index_t>::type e_index = get(edge_index, g);
  graph_traits<graph>::edges_size_type edge_count = 0;
  graph_traits<graph>::edge_iterator ei, ei_end;
  for(tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
    put(e_index, *ei, edge_count++);

  // Test for planarity - we know it is planar, we just want to
  // compute the planar embedding as a side-effect
  typedef std::vector< graph_traits<graph>::edge_descriptor > vec_t;
  std::vector<vec_t> embedding(num_vertices(g));
  if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
                                   boyer_myrvold_params::embedding =
                                       &embedding[0]
                                   )
      )
    std::cout << "Input graph is planar" << std::endl;
  else
    std::cout << "Input graph is not planar" << std::endl;

  typedef std::vector<graph_traits<graph>::vertex_descriptor>
    ordering_storage_t;

  ordering_storage_t ordering;
  planar_canonical_ordering(g, &embedding[0], std::back_inserter(ordering));

  ordering_storage_t::iterator oi, oi_end;
  oi_end = ordering.end();
  std::cout << "The planar canonical ordering is: ";
  for(oi = ordering.begin(); oi != oi_end; ++oi)
    std::cout << *oi << " ";
  std::cout << std::endl;

  return 0;
}


// Copyright (C) 2006-2009 Dmitry Bufistov and Andrey Parfenov

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

#include <cassert>
#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <boost/graph/howard_cycle_ratio.hpp>

/**
 * @author Dmitry Bufistov
 * @author Andrey Parfenov
 */

using namespace boost;
typedef adjacency_list<
    listS, listS, directedS,
    property<vertex_index_t, int>,
    property<
        edge_weight_t, double, property<edge_weight2_t, double>
    >
> grap_real_t;

template <typename TG>
void gen_rand_graph(TG &g, size_t nV, size_t nE)
{
    g.clear();
    mt19937 rng;
    rng.seed(uint32_t(time(0)));
    boost::generate_random_graph(g, nV, nE, rng, true, true);
    boost::uniform_real<> ur(-1,10);
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> >   ew1rg(rng, ur);
    randomize_property<edge_weight_t>(g, ew1rg);
    boost::uniform_int<size_t> uint(1,5);
    boost::variate_generator<boost::mt19937&, boost::uniform_int<size_t> >      ew2rg(rng, uint);
    randomize_property<edge_weight2_t>(g, ew2rg);
}

int main(int argc, char* argv[])
{
    using std::cout;
    using std::endl;
    const double epsilon = 0.0000001;
    double min_cr, max_cr; ///Minimum and maximum cycle ratio
    typedef std::vector<graph_traits<grap_real_t>::edge_descriptor> ccReal_t;
    ccReal_t cc; ///critical cycle

    grap_real_t tgr;
    property_map<grap_real_t, vertex_index_t>::type vim = get(vertex_index, tgr);
    property_map<grap_real_t, edge_weight_t>::type ew1 = get(edge_weight, tgr);
    property_map<grap_real_t, edge_weight2_t>::type ew2 = get(edge_weight2, tgr);

    gen_rand_graph(tgr, 1000, 30000);
    cout << "Vertices number: " << num_vertices(tgr) << endl;
    cout << "Edges number: " << num_edges(tgr) << endl;
    int i = 0;
    graph_traits<grap_real_t>::vertex_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = vertices(tgr); vi != vi_end; vi++) {
        vim[*vi] = i++; ///Initialize vertex index property
    }
    max_cr = maximum_cycle_ratio(tgr, vim, ew1, ew2);
    cout << "Maximum cycle ratio is " << max_cr << endl;
    min_cr = minimum_cycle_ratio(tgr, vim, ew1, ew2, &cc);
    cout << "Minimum cycle ratio is " << min_cr << endl;
    std::pair<double, double> cr(.0,.0);
    cout << "Critical cycle:\n";
    for (ccReal_t::iterator itr = cc.begin(); itr != cc.end(); ++itr)
    {
        cr.first += ew1[*itr];
        cr.second += ew2[*itr];
        std::cout << "(" << vim[source(*itr, tgr)] << "," <<
            vim[target(*itr, tgr)] << ") ";
    }
    cout << endl;
    assert(std::abs(cr.first / cr.second - min_cr) < epsilon);
    return EXIT_SUCCESS;
}


/*

Copyright (c) 2013  Ghassen Hamrouni

Abstract:

This module provides a weighted undirected graph G (possibly with loops)

Author:

Ghassen Hamrouni <ghamrouni.iptech@gmail.com> 23-07-2013

Revision History:

*/

#ifndef R_1_PUBLIC_GRAPH_H_
#define R_1_PUBLIC_GRAPH_H_

#include <Eigen/Dense>
#include "MinCutClusterAnalyzer.hpp"
#include "ClusterCollection.hpp"

#include <random>
#include <fstream>
#include <string>
#include <set>
#include <unordered_set>

namespace R1 {

	//
	// Graph: A weighted undirected graph G (possibly with loops)
	//
	class Graph {

	public:

		virtual ~Graph() {}

		Graph() {}

		Graph(int NVertex) : n_vertex(NVertex), n_edges(0) {
			adjacency_matrix = Eigen::MatrixXf(NVertex, NVertex);
			adjacency_matrix.setZero();
		}

		void setSize(int NVertex) {
			n_vertex = (NVertex);
			n_edges  = (0);
			adjacency_matrix = Eigen::MatrixXf(NVertex, NVertex);
			adjacency_matrix.setZero();
		}

		void connect(int u, int v) {
			if (u >= n_vertex) {
				return;
			}

			if (v >= n_vertex) {
				return;
			}

			adjacency_matrix(u, v) = 1.0;
			adjacency_matrix(v, u) = 1.0;

			n_edges = n_edges + 1;
		}

		void connect(int u, int v, float weight) {
			if (u >= n_vertex) {
				return;
			}

			if (v >= n_vertex) {
				return;
			}

			adjacency_matrix(u, v) = weight;
			adjacency_matrix(v, u) = weight;

			n_edges = n_edges + 1;
		}

		float degree(int v) {
			float n = 0.0f;

			if (v >= n_vertex) {
				return 0;
			}

			for (int u = 0; u < n_vertex; u++) {
				n += adjacency_matrix(u, v);
			}

			return n;
		}

		bool is_connected(int u, int v) {
			if (u >= n_vertex) {
				return false;
			}

			if (v >= n_vertex) {
				return false;
			}

			return (adjacency_matrix(u, v) >= 1.0);
		}

		// A graph is said to be nontrivial if it contains at least
		// one edge.
		bool is_trivial() {
			return n_edges == 0;
		}

		void generateRandomSpanningTree()
		{
			std::random_device rd;
			std::mt19937 gen(rd());
			std::uniform_int_distribution<> vdist(0, n_vertex - 1);
			int current_vertex = vdist(gen);

			std::vector<int> P;
			std::vector<int> V;

			for (int i = 0; i < n_vertex; i++)
			{
				if (i != current_vertex)
					P.push_back(i);
			}

			V.push_back(current_vertex);

			while (!P.empty())
			{
				std::uniform_int_distribution<> distP(0, P.size() - 1);

				int j_index = distP(gen);
				int j = P[j_index];

				std::swap(P[j_index], P.back());
				P.pop_back();

				std::uniform_int_distribution<> distV(0, V.size() - 1);

				int u_index = distV(gen);
				int u = V[u_index];

				connect(u, j);

				V.push_back(j);
			}
		}

		void generateUniformRandomSpanningTree()
		{
			std::random_device rd;
			std::mt19937 gen(rd());
			std::uniform_int_distribution<> vdist(0, n_vertex - 1);

			int current_vertex = vdist(gen);
			std::vector<int> V;

			int n_edge = n_vertex - 1;

			for (int i = 0; i < n_vertex; i++)
			{
				V.push_back(0);
			}

			V[current_vertex] = 1;

			while (n_edge)
			{
				int j = vdist(gen);

				if (V[j] == 0)
				{
					V[j] = 1;
					connect(current_vertex, j);
					n_edge--;
				}

				current_vertex = j;
			}
		}

		void generateRandomGraph(int n_random_edges)
		{
			std::random_device rd;
			std::mt19937 gen(rd());
			std::uniform_int_distribution<> vdist(0, n_vertex - 1);

			while (n_random_edges > 0)
			{
				int i = vdist(gen);
				int j = vdist(gen);

				connect(i, j);

				n_random_edges--;
			}
		}

		ClusterCollection cluster(double alpha = 0.5)
		{
			return MinCutClusterAnalyzer::cluster(adjacency_matrix, alpha);
		}

		std::vector<ClusterCollection> hierarchicalCluster(double alpha = 0.5, double decay = 0.5)
		{
			std::vector<ClusterCollection> clusters;
			int prev_cluster_nb = 0;

			for (int i = 0; ; i++)
			{
				ClusterCollection cl = MinCutClusterAnalyzer::cluster(adjacency_matrix, alpha);

				if (cl.size != prev_cluster_nb)
				{
					prev_cluster_nb = cl.size;
					clusters.push_back(cl);
				}

				if (cl.size == 1) // One cluster
					break;

				if (alpha <= 0.0)
					break;

				alpha *= decay;
			}

			return clusters;
		}

	protected:
		Eigen::MatrixXf adjacency_matrix;

		int		n_vertex;
		int		n_edges;

	};
}
#endif


//==============================================================================
//         Copyright 2015 J.T. Lapreste
//
//          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
//==============================================================================
#include <nt2/exponential/include/functions/significants.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_integer.hpp>

NT2_TEST_CASE_TPL ( significants,  BOOST_SIMD_REAL_TYPES)
{

  using nt2::significants;
  using nt2::tag::significants_;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<significants_(T, iT)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

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


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

//[distance
//` Shows calculation of distance of point to some other geometries

#include <iostream>
#include <list>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/multi/geometries/multi_point.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>

#include <boost/foreach.hpp>

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point_type;
    typedef boost::geometry::model::polygon<point_type> polygon_type;
    typedef boost::geometry::model::linestring<point_type> linestring_type;
    typedef boost::geometry::model::multi_point<point_type> multi_point_type;

    point_type p(1,2);
    polygon_type poly;
    linestring_type line;
    multi_point_type mp;

    boost::geometry::read_wkt(
        "POLYGON((2 1.3,2.4 1.7,2.8 1.8,3.4 1.2,3.7 1.6,3.4 2,4.1 3,5.3 2.6,5.4 1.2,4.9 0.8,2.9 0.7,2 1.3)"
            "(4.0 2.0, 4.2 1.4, 4.8 1.9, 4.4 2.2, 4.0 2.0))", poly);
    line.push_back(point_type(0,0));
    line.push_back(point_type(0,3));
    mp.push_back(point_type(0,0));
    mp.push_back(point_type(3,3));

    std::cout
        << "Point-Poly: " << boost::geometry::distance(p, poly) << std::endl
        << "Point-Line: " << boost::geometry::distance(p, line) << std::endl
        << "Point-MultiPoint: " << boost::geometry::distance(p, mp) << std::endl;

    return 0;
}

//]

//[distance_output
/*`
Output:
[pre
Point-Poly: 1.22066
Point-Line: 1
Point-MultiPoint: 2.23607
]
*/
//]


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   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
//==============================================================================
#include <nt2/include/functions/sqrt1pm1.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/sqrt_2.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/i.hpp>

NT2_TEST_CASE_TPL ( sqrt1pm1_real,  NT2_REAL_TYPES)
{
  using nt2::sqrt1pm1;
  using nt2::tag::sqrt1pm1_;
  typedef typename std::complex<T> cT;
  typedef typename nt2::meta::call<sqrt1pm1_(cT)>::type r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, cT);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
    NT2_TEST_ULP_EQUAL(sqrt1pm1(nt2::Inf<cT>()),  cT(nt2::Inf<T>()), 0);
    NT2_TEST_ULP_EQUAL(sqrt1pm1(nt2::Minf<cT>()),  cT(-1, nt2::Inf<T>()), 0);
    NT2_TEST_ULP_EQUAL(sqrt1pm1(nt2::Nan<cT>()),  cT(nt2::Nan<T>(), nt2::Nan<T>()), 0);
#endif
    NT2_TEST_ULP_EQUAL(sqrt1pm1(nt2::Mone<cT>()),  cT(nt2::Mone<T>()), 0);
    NT2_TEST_ULP_EQUAL(sqrt1pm1(nt2::One<cT>()),  cT(nt2::Sqrt_2<T>()-nt2::One<T>()), 2);
    NT2_TEST_ULP_EQUAL(sqrt1pm1(cT(nt2::Eps<T>())), cT(nt2::Eps<T>()*nt2::Half<T>()), 2);
    NT2_TEST_ULP_EQUAL(sqrt1pm1(cT(-1, 2)), cT(0, 1), 2);
    NT2_TEST_ULP_EQUAL(sqrt1pm1(nt2::Zero<cT>()),  cT(nt2::Zero<T>()), 0);
}


//=========================================================================
//
// Copyright 2018 Kitware, Inc.
// Author: Guilbert Pierre (spguilbert@gmail.com)
//         Laurenson Nick (nlaurenson5@gmail.com)
// Date: 03-27-2018
//
// 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.
//=========================================================================
#include "SpinningSensorKeypointExtractor.h"

#include <numeric>

#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace {
//-----------------------------------------------------------------------------
template <typename T>
std::vector<size_t> sortIdx(const std::vector<T> &v)
{
  // initialize original index locations
  std::vector<size_t> idx(v.size());
  std::iota(idx.begin(), idx.end(), 0);

  // sort indexes based on comparing values in v
  std::sort(idx.begin(), idx.end(),
       [&v](size_t i1, size_t i2) {return v[i1] > v[i2];});

  return idx;
}

//-----------------------------------------------------------------------------
class LineFitting
{
public:
  // Fitting using PCA
  bool FitPCA(std::vector<Eigen::Vector3d >& points);

  // Futting using very local line and
  // check if this local line is consistent
  // in a more global neighborhood
  bool FitPCAAndCheckConsistency(std::vector<Eigen::Vector3d >& points);

  // Poor but fast fitting using
  // extremities of the distribution
  void FitFast(std::vector<Eigen::Vector3d >& points);

  // Direction and position
  Eigen::Vector3d Direction;
  Eigen::Vector3d Position;
  Eigen::Matrix3d SemiDist;
  Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity();
  double MaxDistance = 0.02;
  double MaxSinAngle = 0.65;
};

//-----------------------------------------------------------------------------
bool LineFitting::FitPCA(std::vector<Eigen::Vector3d >& points)
{
  // Compute PCA to determine best line approximation
  // of the points distribution
  Eigen::MatrixXd data(points.size(), 3);

  for (unsigned int k = 0; k < points.size(); k++)
  {
    data.row(k) = points[k];
  }
  // Position
  this->Position = data.colwise().mean();
  Eigen::MatrixXd centered = data.rowwise() - this->Position.transpose();
  Eigen::Matrix3d varianceCovariance = centered.transpose() * centered;
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig(varianceCovariance);

  // Direction
  this->Direction = eig.eigenvectors().col(2).normalized();

  // Semi distance matrix
  // (polar form associated to
  // a bilineare symmetric positive
  // semi-definite matrix)
  this->SemiDist = (this->I3 - this->Direction * this->Direction.transpose());

  bool isLineFittingAccurate = true;

  // if a point of the neighborhood is too far from
  // the fitting line we considere the neighborhood as
  // non flat
  double squaredMaxDistance = std::pow(this->MaxDistance, 2);
  for (unsigned int k = 0; k < points.size(); k++)
  {
    double d = (points[k] - this->Position).transpose() * this->SemiDist * (points[k] - this->Position);
    if (d > squaredMaxDistance)
    {
      isLineFittingAccurate = false;
    }
  }
  return isLineFittingAccurate;
}

//-----------------------------------------------------------------------------
bool LineFitting::FitPCAAndCheckConsistency(std::vector<Eigen::Vector3d >& points)
{
  bool isLineFittingAccurate = true;

  // first check if the neighborhood is straight
  Eigen::Vector3d U, V;
  U = (points[1] - points[0]).normalized();
  for (unsigned int index = 1; index < points.size() - 1; index++)
  {
    V = (points[index + 1] - points[index]).normalized();
    double sinAngle = (U.cross(V)).norm();
    if (sinAngle > this->MaxSinAngle)
    {
      isLineFittingAccurate = false;
    }
  }

  // Then fit with PCA
  isLineFittingAccurate &= this->FitPCA(points);
  return isLineFittingAccurate;
}

//-----------------------------------------------------------------------------
void LineFitting::FitFast(std::vector<Eigen::Vector3d >& points)
{
  // Take the two extrems points of the neighborhood
  // i.e the farest and the closest to the current point
  Eigen::Vector3d U = points[0];
  Eigen::Vector3d V = points[points.size() - 1];

  // direction
  this->Direction = (V - U).normalized();

  // position
  this->Position = U;

  // Semi distance matrix
  // (polar form associated to
  // a bilineare symmetric positive
  // semi-definite matrix)
  this->SemiDist = (this->I3 - this->Direction * this->Direction.transpose());
  this->SemiDist = this->SemiDist.transpose() * this->SemiDist;
}
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::PrepareDataForNextFrame()
{
  // Reset the pcl format pointcloud to store the new frame
  this->pclCurrentFrameByScan.resize(this->NLasers);
  for (unsigned int k = 0; k < this->NLasers; ++k)
  {
    this->pclCurrentFrameByScan[k].reset(new pcl::PointCloud<Point>());
  }

  this->EdgesPoints.reset(new pcl::PointCloud<Point>());
  this->PlanarsPoints.reset(new pcl::PointCloud<Point>());
  this->BlobsPoints.reset(new pcl::PointCloud<Point>());

  this->Angles.clear();
  this->Angles.resize(this->NLasers);
  this->SaillantPoint.clear();
  this->SaillantPoint.resize(this->NLasers);
  this->DepthGap.clear();
  this->DepthGap.resize(this->NLasers);
  this->IntensityGap.clear();
  this->IntensityGap.resize(this->NLasers);
  this->IsPointValid.clear();
  this->IsPointValid.resize(this->NLasers);
  this->Label.clear();
  this->Label.resize(this->NLasers);
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::ConvertAndSortScanLines()
{
  int nbPoints = this->pclCurrentFrame->size();
  double frameStartTime = this->pclCurrentFrame->points[0].time;
  double frameDuration = this->pclCurrentFrame->points[nbPoints-1].time - frameStartTime;

  for (size_t index = 0; index < nbPoints; ++index)
  {
    const Point& oldPoint = this->pclCurrentFrame->points[index];
    int id = this->LaserIdMapping[oldPoint.laserId];
    // modify the point so that:
    // - laserId is corrected with the laserIdMapping
    // - time become a relative advancement time (between 0 and 1)
    Point newPoint(oldPoint);
    newPoint.laserId = id;
    newPoint.time = (oldPoint.time - frameStartTime) / frameDuration;

    // add the current point to its corresponding laser scan
    this->pclCurrentFrameByScan[id]->push_back(newPoint);
  }
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::ComputeKeyPoints(pcl::PointCloud<Point>::Ptr pc, std::vector<size_t> laserIdMapping)
{
  if (this->LaserIdMapping.empty())
  {
    this->NLasers = laserIdMapping.size();
    this->LaserIdMapping = laserIdMapping;
  }
  this->pclCurrentFrame = pc;
  this->PrepareDataForNextFrame();
  this->ConvertAndSortScanLines();
  // Initialize the vectors with the correct length
  for (unsigned int k = 0; k < this->NLasers; ++k)
  {
    size_t nbPoint = this->pclCurrentFrameByScan[k]->size();
    this->IsPointValid[k].resize(nbPoint, 1);
    this->Label[k].resize(nbPoint, 0);
    this->Angles[k].resize(nbPoint, 0);
    this->SaillantPoint[k].resize(nbPoint, 0);
    this->DepthGap[k].resize(nbPoint, 0);
    this->IntensityGap[k].resize(nbPoint, 0);
  }

  // Invalid points with bad criteria
  this->InvalidPointWithBadCriteria();

  // compute keypoints scores
  this->ComputeCurvature();

  // labelize keypoints
  this->SetKeyPointsLabels();
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::ComputeCurvature()
{
  double squaredDistToLineThreshold = std::pow(this->DistToLineThreshold, 2);
  double squaredDepthDistCoeff = 0.25;
  // loop over scans lines
  for (unsigned int scanLine = 0; scanLine < this->NLasers; ++scanLine)
  {
    Point currentPoint, nextPoint, previousPoint;
    Eigen::Vector3d X, centralPoint;
    LineFitting leftLine, rightLine, farNeighborsLine;

    // We will compute the line that fit the neighbors located
    // previously the current. We will do the same for the
    // neighbors located after the current points. We will then
    // compute the angle between these two lines as an approximation
    // of the "sharpness" of the current point.
    std::vector<Eigen::Vector3d> leftNeighbor(this->NeighborWidth);
    std::vector<Eigen::Vector3d> rightNeighbor(this->NeighborWidth);
    std::vector<Eigen::Vector3d> farNeighbors;
    farNeighbors.reserve(3 * this->NeighborWidth);

    // loop over points in the current scan line
    int Npts = this->pclCurrentFrameByScan[scanLine]->size();

    // if the line is almost empty, skip it
    if (Npts < 2 * this->NeighborWidth + 1)
    {
      continue;
    }

    for (int index = this->NeighborWidth; (index + this->NeighborWidth) < Npts; ++index)
    {
      // Skip curvature computation for invalid points
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // central point
      currentPoint = this->pclCurrentFrameByScan[scanLine]->points[index];
      centralPoint << currentPoint.x, currentPoint.y, currentPoint.z;

      // compute intensity gap
      nextPoint = this->pclCurrentFrameByScan[scanLine]->points[index + 1];
      previousPoint = this->pclCurrentFrameByScan[scanLine]->points[index - 1];
      this->IntensityGap[scanLine][index] = std::abs(nextPoint.intensity - previousPoint.intensity);

      // Fill right and left neighborhood
      // /!\ The way the neighbors are added
      // to the vectors matters. Especially when
      // computing the saillancy
      for (int j = index - this->NeighborWidth; j < index; ++j)
      {
        currentPoint = this->pclCurrentFrameByScan[scanLine]->points[j];
        leftNeighbor[j -index + this->NeighborWidth] << currentPoint.x, currentPoint.y, currentPoint.z;
      }
      for (int j = index + 1; j <= index + this->NeighborWidth; ++j)
      {
        currentPoint = this->pclCurrentFrameByScan[scanLine]->points[j];
        rightNeighbor[j - index - 1] << currentPoint.x, currentPoint.y, currentPoint.z;
      }

      // Fit line on the neighborhood and
      // Indicate if the left and right side
      // neighborhood of the current point is flat or not
      bool leftFlat = leftLine.FitPCAAndCheckConsistency(leftNeighbor);
      bool rightFlat = rightLine.FitPCAAndCheckConsistency(rightNeighbor);

      // Measurement of the gap
      double dist1 = 0; double dist2 = 0;

      // if both neighborhood are flat we can compute
      // the angle between them as an approximation of the
      // sharpness of the current point
      if (rightFlat && leftFlat)
      {
        // We check that the current point is not too far from its
        // neighborhood lines. This is because we don't want a point
        // to be considered as a angles point if it is due to gap
        dist1 = (centralPoint - leftLine.Position).transpose() * leftLine.SemiDist * (centralPoint - leftLine.Position);
        dist2 = (centralPoint - rightLine.Position).transpose() * rightLine.SemiDist * (centralPoint - rightLine.Position);

        if ((dist1 < squaredDistToLineThreshold) && (dist2 < squaredDistToLineThreshold))
          this->Angles[scanLine][index] = std::abs((leftLine.Direction.cross(rightLine.Direction)).norm()); // sin of angle actually
      }
      // Here one side of the neighborhood is non flat
      // Hence it is not worth to estimate the sharpness.
      // Only the gap will be considered here.
      else if (rightFlat && !leftFlat)
      {
        dist1 = std::numeric_limits<double>::max();
        for (unsigned int neighIndex = 0; neighIndex < leftNeighbor.size(); ++neighIndex)
        {
          dist1 = std::min(dist1,
                  ((leftNeighbor[neighIndex] - rightLine.Position).transpose() * rightLine.SemiDist * (leftNeighbor[neighIndex] - rightLine.Position))(0));
        }
        dist1 = squaredDepthDistCoeff * dist1;
      }
      else if (!rightFlat && leftFlat)
      {
        dist2 = std::numeric_limits<double>::max();
        for (unsigned int neighIndex = 0; neighIndex < leftNeighbor.size(); ++neighIndex)
        {
          dist2 = std::min(dist2,
                  ((rightNeighbor[neighIndex] - leftLine.Position).transpose() * leftLine.SemiDist * (rightNeighbor[neighIndex] - leftLine.Position))(0));
        }
        dist2 = squaredDepthDistCoeff * dist2;
      }
      else
      {
        // Compute saillant point score
        double currDepth = centralPoint.norm();
        unsigned int diffDepth = 0;
        bool canLeftBeAdded = true; bool hasLeftEncounteredDepthGap = false;
        bool canRightBeAdded = true; bool hasRightEncounteredDepthGap = false;

        // The saillant point score is the distance between the current point
        // and the points that have a depth gap with the current point
        farNeighbors.resize(0);
        for (unsigned int neighIndex = 0; neighIndex < leftNeighbor.size(); ++neighIndex)
        {
          // Left neighborhood depth gap computation
          if ((std::abs(leftNeighbor[leftNeighbor.size() - 1 - neighIndex].norm() - currDepth) > 1.5) && canLeftBeAdded)
          {
            hasLeftEncounteredDepthGap = true;
            diffDepth++;
            farNeighbors.emplace_back(leftNeighbor[neighIndex]);
          }
          else
          {
            if (hasLeftEncounteredDepthGap)
            {
              canLeftBeAdded = false;
            }
          }
          // Right neigborhood depth gap computation
          if ((std::abs(rightNeighbor[neighIndex].norm() - currDepth) > 1.5) && canRightBeAdded)
          {
            hasRightEncounteredDepthGap = true;
            diffDepth++;
            farNeighbors.emplace_back(rightNeighbor[neighIndex]);
          }
          else
          {
            if (hasRightEncounteredDepthGap)
            {
              canRightBeAdded = false;
            }
          }
        }

        // If there is enought neighbors with a big depth gap
        // we propose to compute the saillancy of the current
        // as the distance between the line that fits the neighbors
        // with a depth gap and the current point
        if (static_cast<double>(diffDepth) / (2.0 * this->NeighborWidth) > 0.5)
        {
          farNeighborsLine.FitPCA(farNeighbors);
          this->SaillantPoint[scanLine][index] =
            (centralPoint - farNeighborsLine.Position).transpose() * farNeighborsLine.SemiDist * (centralPoint - farNeighborsLine.Position);
        }
      }
      this->DepthGap[scanLine][index] = std::max(dist1, dist2);
    }
  }
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::InvalidPointWithBadCriteria()
{
  // Temporary variables used in the next loop
  Eigen::Vector3d dX, X, Xn, Xp, Xproj, dXproj;
  Eigen::Vector3d Y, Yn, Yp, dY;
  double L, Ln, expectedLength, dLn, dLp;
  Point currentPoint, nextPoint, previousPoint;
  Point temp;

  // loop over scan lines
  for (unsigned int scanLine = 0; scanLine < this->NLasers; ++scanLine)
  {
    int Npts = this->pclCurrentFrameByScan[scanLine]->size();

    // if the line is almost empty, skip it
    if (Npts < 3 * this->NeighborWidth)
    {
      continue;
    }
    // invalidate first and last points
    for (int index = 0; index <= this->NeighborWidth; ++index)
    {
      this->IsPointValid[scanLine][index] = 0;
    }
    for (int index = Npts - 1 - this->NeighborWidth - 1; index < Npts; ++index)
    {
      this->IsPointValid[scanLine][index] = 0;
    }

    // loop over points into the scan line
    for (int index = this->NeighborWidth; index <  Npts - this->NeighborWidth - 1; ++index)
    {
      currentPoint = this->pclCurrentFrameByScan[scanLine]->points[index];
      nextPoint = this->pclCurrentFrameByScan[scanLine]->points[index + 1];
      previousPoint = this->pclCurrentFrameByScan[scanLine]->points[index - 1];
      X << currentPoint.x, currentPoint.y, currentPoint.z;
      Xn << nextPoint.x, nextPoint.y, nextPoint.z;
      Xp << previousPoint.x, previousPoint.y, previousPoint.z;
      dX = Xn - X;
      L = X.norm();
      Ln = Xn.norm();
      dLn = dX.norm();

      // the expected length between two firing of the same laser
      // depend on the distance and the angular resolution of the
      // sensor.
      expectedLength = 2.0 *  std::tan(this->AngleResolution / 2.0) * L;
      double ratioExpectedLength = 10.0;

      // if the length between the two firing
      // is more than n-th the expected length
      // it means that there is a gap. We now must
      // determine if the gap is due to the geometry of
      // the scene or if the gap is due to an occluded area
      if (dLn > ratioExpectedLength * expectedLength)
      {
        // Project the next point onto the
        // sphere of center 0 and radius =
        // norm of the current point. If the
        // gap has disappeared it means that
        // the gap was due to an occlusion
        Xproj = L / Ln * Xn;
        dXproj = Xproj - X;
        // it is a depth gap, invalidate the part which belong
        // to the occluded area (farest)
        // invalid next part
        if (L < Ln)
        {
          for (int i = index + 1; i <= index + this->NeighborWidth; ++i)
          {
            if (i > index + 1)
            {
              temp = this->pclCurrentFrameByScan[scanLine]->points[i - 1];
              Yp << temp.x, temp.y, temp.z;
              temp = this->pclCurrentFrameByScan[scanLine]->points[i];
              Y << temp.x, temp.y, temp.z;
              dY = Y - Yp;
              // if there is a gap in the neihborhood
              // we do not invalidate the rest of neihborhood
              if (dY.norm() > ratioExpectedLength * expectedLength)
              {
                break;
              }
            }
            this->IsPointValid[scanLine][i] = 0;
          }
        }
        // invalid previous part
        else
        {
          for (int i = index - this->NeighborWidth; i <= index; ++i)
          {
            if (i < index)
            {
              temp = this->pclCurrentFrameByScan[scanLine]->points[i + 1];
              Yn << temp.x, temp.y, temp.z;
              temp = this->pclCurrentFrameByScan[scanLine]->points[i];
              Y << temp.x, temp.y, temp.z;
              dY = Yn - Y;
              // if there is a gap in the neihborhood
              // we do not invalidate the rest of neihborhood
              if (dY.norm() > ratioExpectedLength * expectedLength)
              {
                break;
              }
            }
            this->IsPointValid[scanLine][i] = 0;
          }
        }
      }
      // Invalid points which are too close from the sensor
      if (L < this->MinDistanceToSensor)
      {
        this->IsPointValid[scanLine][index] = 0;
      }

      // Invalid points which are on a planar
      // surface nearly parallel to the laser
      // beam direction
      dLp = (X - Xp).norm();
      if ((dLp > 1 / 4.0 * ratioExpectedLength * expectedLength) && (dLn > 1 / 4.0 * ratioExpectedLength * expectedLength))
      {
        this->IsPointValid[scanLine][index] = 0;
      }
    }
  }
}

//-----------------------------------------------------------------------------
void SpinningSensorKeypointExtractor::SetKeyPointsLabels()
{
  this->EdgesIndex.clear(); this->EdgesIndex.resize(0);
  this->PlanarIndex.clear(); this->PlanarIndex.resize(0);
  this->BlobIndex.clear(); this->BlobIndex.resize(0);
  double squaredEdgeDepthGapThreshold = std::pow(this->EdgeDepthGapThreshold, 2);

  // loop over the scan lines
  for (unsigned int scanLine = 0; scanLine < this->NLasers; ++scanLine)
  {
    int Npts = this->pclCurrentFrameByScan[scanLine]->size();
    unsigned int nbrEdgePicked = 0;
    unsigned int nbrPlanarPicked = 0;

    // We split the validity of points between the edges
    // keypoints and planar keypoints. This allows to take
    // some points as planar keypoints even if they are close
    // to an edge keypoint.
    std::vector<double> IsPointValidForPlanar = this->IsPointValid[scanLine];

    // if the line is almost empty, skip it
    if (Npts < 3 * this->NeighborWidth)
    {
      continue;
    }

    // Sort the curvature score in a decreasing order
    std::vector<size_t> sortedDepthGapIdx = sortIdx<double>(this->DepthGap[scanLine]);
    std::vector<size_t> sortedAnglesIdx = sortIdx<double>(this->Angles[scanLine]);
    std::vector<size_t> sortedSaillancyIdx = sortIdx<double>(this->SaillantPoint[scanLine]);
    std::vector<size_t> sortedIntensityGap = sortIdx<double>(this->IntensityGap[scanLine]);

    double depthGap, sinAngle, saillancy, intensity;
    int index = 0;

    // Edges using depth gap
    for (int k = 0; k < Npts; ++k)
    {
      index = sortedDepthGapIdx[k];
      depthGap = this->DepthGap[scanLine][index];

      // thresh
      if (depthGap < squaredEdgeDepthGapThreshold)
      {
        break;
      }

      // if the point is invalid continue
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // else indicate that the point is an edge
      this->Label[scanLine][index] = 4;
      this->EdgesIndex.push_back(std::pair<int, int>(scanLine, index));
      nbrEdgePicked++;
      //IsPointValidForPlanar[index] = 0;

      // invalid its neighborhod
      int indexBegin = index - this->NeighborWidth + 1;
      int indexEnd = index + this->NeighborWidth - 1;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        this->IsPointValid[scanLine][j] = 0;
      }
    }

    // Edges using angles
    for (int k = 0; k < Npts; ++k)
    {
      index = sortedAnglesIdx[k];
      sinAngle = this->Angles[scanLine][index];

      // thresh
      if (sinAngle < this->EdgeSinAngleThreshold)
      {
        break;
      }

      // if the point is invalid continue
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // else indicate that the point is an edge
      this->Label[scanLine][index] = 4;
      this->EdgesIndex.push_back(std::pair<int, int>(scanLine, index));
      nbrEdgePicked++;
      //IsPointValidForPlanar[index] = 0;

      // invalid its neighborhod
      int indexBegin = index - this->NeighborWidth;
      int indexEnd = index + this->NeighborWidth;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        this->IsPointValid[scanLine][j] = 0;
      }
    }

    // Edges using saillancy
    for (int k = 0; k < Npts; ++k)
    {
      index = sortedSaillancyIdx[k];
      saillancy = this->SaillantPoint[scanLine][index];

      // thresh
      if (saillancy < this->SaillancyThreshold)
      {
        break;
      }

      // if the point is invalid continue
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // else indicate that the point is an edge
      this->Label[scanLine][index] = 4;
      this->EdgesIndex.push_back(std::pair<int, int>(scanLine, index));
      nbrEdgePicked++;
      //IsPointValidForPlanar[index] = 0;

      // invalid its neighborhod
      int indexBegin = index - this->NeighborWidth + 1;
      int indexEnd = index + this->NeighborWidth - 1;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        this->IsPointValid[scanLine][j] = 0;
      }
    }

    // Edges using intensity
    for (int k = 0; k < Npts; ++k)
    {
      index = sortedIntensityGap[k];
      intensity = this->IntensityGap[scanLine][index];

      // thresh
      if (intensity < 50.0)
      {
        break;
      }

      // if the point is invalid continue
      if (this->IsPointValid[scanLine][index] == 0)
      {
        continue;
      }

      // else indicate that the point is an edge
      this->Label[scanLine][index] = 4;
      this->EdgesIndex.push_back(std::pair<int, int>(scanLine, index));
      nbrEdgePicked++;
      //IsPointValidForPlanar[index] = 0;

      // invalid its neighborhood
      int indexBegin = index - 1;
      int indexEnd = index + 1;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        this->IsPointValid[scanLine][j] = 0;
      }
    }

    // Blobs Points
//    if (!this->FastSlam)
//    {
      for (int k = 0; k < Npts; k = k + 3)
      {
        this->BlobIndex.push_back(std::pair<int, int>(scanLine, k));
      }
//    }

    // Planes
    for (int k = Npts - 1; k >= 0; --k)
    {
      index = sortedAnglesIdx[k];
      sinAngle = this->Angles[scanLine][index];

      // thresh
      if (sinAngle > this->PlaneSinAngleThreshold)
      {
        break;
      }

      // if the point is invalid continue
      if (IsPointValidForPlanar[index] == 0)
      {
        continue;
      }

      // else indicate that the point is a planar one
      if ((this->Label[scanLine][index] != 4) && (this->Label[scanLine][index] != 3))
        this->Label[scanLine][index] = 2;
      this->PlanarIndex.push_back(std::pair<int, int>(scanLine, index));
      IsPointValidForPlanar[index] = 0;
      this->IsPointValid[scanLine][index] = 0;

      // Invalid its neighbor so that we don't have too
      // many planar keypoints in the same region. This is
      // required because of the k-nearest search + plane
      // approximation realized in the odometry part. Indeed,
      // if all the planar points are on the same scan line the
      // problem is degenerated since all the points are distributed
      // on a line.
      int indexBegin = index - 4;
      int indexEnd = index + 4;
      indexBegin = std::max(0, indexBegin);
      indexEnd = std::min(Npts - 1, indexEnd);
      for (int j = indexBegin; j <= indexEnd; ++j)
      {
        IsPointValidForPlanar[j] = 0;
      }
      nbrPlanarPicked++;
    }
  }

  // add keypoints in increasing scan id order
  std::sort(this->EdgesIndex.begin(), this->EdgesIndex.end());
  std::sort(this->PlanarIndex.begin(), this->PlanarIndex.end());
  std::sort(this->BlobIndex.begin(), this->BlobIndex.end());

  // fill the keypoints vectors and compute the max dist keypoints
  this->FarestKeypointDist = 0.0;
  Point p;
  for (unsigned int k = 0; k < this->EdgesIndex.size(); ++k)
  {
    p = this->pclCurrentFrameByScan[this->EdgesIndex[k].first]->points[this->EdgesIndex[k].second];
    this->EdgesPoints->push_back(p);
    this->FarestKeypointDist = std::max(this->FarestKeypointDist, static_cast<double>(std::pow(p.x, 2) + std::pow(p.y, 2) + std::pow(p.z, 2)));
  }
  for (unsigned int k = 0; k < this->PlanarIndex.size(); ++k)
  {
    p = this->pclCurrentFrameByScan[this->PlanarIndex[k].first]->points[this->PlanarIndex[k].second];
    this->PlanarsPoints->push_back(p);
    this->FarestKeypointDist = std::max(this->FarestKeypointDist, static_cast<double>(std::pow(p.x, 2) + std::pow(p.y, 2) + std::pow(p.z, 2)));
  }
  for (unsigned int k = 0; k < this->BlobIndex.size();  ++k)
  {
    p = this->pclCurrentFrameByScan[this->BlobIndex[k].first]->points[this->BlobIndex[k].second];
    this->BlobsPoints->push_back(p);
    this->FarestKeypointDist = std::max(this->FarestKeypointDist, static_cast<double>(std::pow(p.x, 2) + std::pow(p.y, 2) + std::pow(p.z, 2)));
  }
  this->FarestKeypointDist = std::sqrt(this->FarestKeypointDist);

  // keypoints extraction informations
  std::cout << "Extracted Edges: " << this->EdgesPoints->size() << " Planars: "
            << this->PlanarsPoints->size() << " Blobs: "
            << this->BlobsPoints->size() << std::endl;
}

//-----------------------------------------------------------------------------
std::unordered_map<std::string, std::vector<double> >
SpinningSensorKeypointExtractor::GetDebugArray()
{
  auto get1DVector =  [this](std::vector<std::vector<double>> array) {
    std::vector<double> v (this->pclCurrentFrame->size());
    std::vector<int> indexPerByScanLine(this->NLasers, 0);
    for (int i = 0; i < this->pclCurrentFrame->size(); i++)
    {
      double laserId = this->LaserIdMapping[this->pclCurrentFrame->points[i].laserId];
      v[i] = array[laserId][indexPerByScanLine[laserId]];
      indexPerByScanLine[laserId]++;
    }
    return v;
  }; // end of lambda expression

  std::unordered_map<std::string, std::vector<double> > map;
  map["angles_line"]    = get1DVector(this->Angles);
  map["saillant_point"] = get1DVector(this->SaillantPoint);
  map["depth_gap"]      = get1DVector(this->DepthGap);
  map["intensity_gap"]  = get1DVector(this->IntensityGap);
  map["is_point_valid"] = get1DVector(this->IsPointValid);
  map["keypoint_label"] = get1DVector(this->Label);
  return map;
}


/**
 *  testReachset.cpp
 *
 *  Test the reachable set computation of a two-link scara manipulator.
 *
 *  Created by Yinan Li on July 05, 2020.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <cmath>

#include <array>
#include <boost/numeric/odeint.hpp>

#include "src/system.hpp"
#include "src/csolver.h"

#include "scara.h"

typedef std::array<double, 4> state_type;
struct scara_vf {
    rocs::Rn _u;
    scara_vf(rocs::Rn u) : _u(u) {}
    void operator()(state_type &x, state_type &dxdt, double t) const {
	double z1 = I1+I2+m1*r1*r1+m2*(l1*l1+r2*r2);
	double z2 = m2*l1*r2;
	double z3 = I2+m2*r2*r2;
	double detM = z3*(z1-z3) - z2*z2*std::cos(x[1])*std::cos(x[1]);
	double a = z2*std::sin(x[1])*(2*x[2]+x[3])*x[3];
	double b = z2*std::cos(x[1]);
	double c = z2*x[2]*std::sin(x[1])-_u[1];

	dxdt[0] = x[2];
	dxdt[1] = x[3];
	dxdt[2] = (z3*_u[0] + z3*a + (z3+b)*c) / detM;
	dxdt[3] = ((z1+2*b)*(-c) - (z3+b)*(_u[0]+a)) / detM;
    }
};

int main() {

    /* set the state space */
    double xlb[4] = {0, -M_PI, -0.5, -0.5};
    double xub[4] = {M_PI, M_PI, 0.5, 0.5};

    /* set the control values */
    double ulb[2] = {-0.001, -0.001};
    double uub[2] = {0.001, 0.001};
    double mu[2] = {0.0002, 0.0002};

    /* set the sampling time and disturbance */
    double t = 0.05;
    double delta = 0.01;
    /* parameters for computing the flow */
    int kmax = 5;
    double tol = 0.01;
    double alpha = 0.5;
    double beta = 2;
    rocs::params controlparams(kmax, tol, alpha, beta);

    /* define the control system */
    rocs::CTCntlSys<scaraode> scara("inverted pendulum", t,
				    scaraode::n, scaraode::nu,
				    delta, &controlparams);
    scara.init_workspace(xlb, xub);
    scara.init_inputset(mu, ulb, uub);
    scara.allocate_flows();

    /* test if reachable set covers the nominal trajectory */
    /* compute reachable set */
    // rocs::ivec x0 = {rocs::interval(0.09, 0.11),
    // 		     rocs::interval(-0.01, 0.01),
    // 		     rocs::interval(-0.01, 0.01),
    // 		     rocs::interval(-0.01, 0.01)};
    rocs::ivec x0 = {rocs::interval(0.6172, 0.63),
    		     rocs::interval(1.61, 1.63),
    		     rocs::interval(-0.005, 0.005),
		     rocs::interval(-0.005, 0.005)};
    std::vector<rocs::ivec> x(scara._ugrid._nv, rocs::ivec(4));
    std::cout << "The initial interval: " << x0 << '\n';
    std::cout << "The integrating time: " << t << '\n';
    scara.get_reach_set(x, x0);

    // double dt{0.001};
    // for (size_t i = 0; i < x.size(); ++i) {
    // 	/* integrate the nominal trajectory */
    // 	state_type y{0.1, 0, 0, 0};
    // 	rocs::Rn u(scara._ugrid._data[i]);
    // 	boost::numeric::odeint::runge_kutta_cash_karp54<state_type> rk45;
    // 	boost::numeric::odeint::integrate_const(rk45, scara_vf(u),
    // 						y, 0.0, t, dt);
    // 	std::cout << "x(t)= [" << y[0] << ','<< y[1] << ',' << y[2] << ',' << y[3] << "]\n";
    // 	std::cout << "R(t, x0, [" << scara._ugrid._data[i][0] << ','
    // 		  << scara._ugrid._data[i][1] << "])= ";
    // 	std::cout << x[i] <<'\n';
    // 	std::cout << "Check: ";
    // 	for (int j = 0; j < 4; ++j) {
    // 	    if (y[j] > x[i][j].getinf() && y[j] < x[i][j].getsup())
    // 		std::cout << true << ' ';
    // 	    else
    // 		std::cout << false << ' ';
    // 	}
    // 	std::cout << '\n';
    // }

    scara.release_flows();

    return 0;
}


//
// author: Ed Valeev (eduard@valeyev.net)
// date  : July 8, 2014
// the use of this software is permitted under the conditions GNU General Public License (GPL) version 2
//

// standard C++ headers
#include <cmath>
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <vector>
#include <chrono>

// OpenMP
#include <omp.h>

// Eigen matrix algebra library
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

// Libint Gaussian integrals library
#include <libint2.hpp>

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
        Matrix;  // import dense, dynamically sized Matrix type from Eigen;
                 // this is a matrix with row-major storage (http://en.wikipedia.org/wiki/Row-major_order)
                 // to meet the layout of the integrals returned by the Libint integral library

struct Atom {
    int atomic_number;
    double x, y, z;
};

std::vector<Atom> read_geometry(const std::string& filename);
std::vector<libint2::Shell> make_sto3g_basis(const std::vector<Atom>& atoms);
size_t nbasis(const std::vector<libint2::Shell>& shells);
std::vector<size_t> map_shell_to_basis_function(const std::vector<libint2::Shell>& shells);
Matrix compute_soad(const std::vector<Atom>& atoms);
Matrix compute_1body_ints(const std::vector<libint2::Shell>& shells,
                          libint2::Operator t,
                          const std::vector<Atom>& atoms = std::vector<Atom>());
Matrix compute_2body_fock(const std::vector<libint2::Shell>& shells,
                                 const Matrix& D);

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

  using std::cout;
  using std::cerr;
  using std::endl;

  using libint2::Shell;
  using libint2::BasisSet;
  using libint2::Engine;
  using libint2::Operator;

  double fock_time = 0.0;
  double start;

  try {

    /*** =========================== ***/
    /*** initialize molecule         ***/
    /*** =========================== ***/

    // read geometry from a file; by default read from h2o.geom, else take filename (.xyz or .geom) from the command line
    const auto filename = (argc > 1) ? argv[1] : "h2o.geom";
    std::vector<Atom> atoms = read_geometry(filename);

    // count the number of electrons
    auto nelectron = 0;
    for (auto i = 0; i < atoms.size(); ++i)
      nelectron += atoms[i].atomic_number;
    const auto ndocc = nelectron / 2;

    // compute the nuclear repulsion energy
    auto enuc = 0.0;
    for (auto i = 0; i < atoms.size(); i++)
      for (auto j = i + 1; j < atoms.size(); j++) {
        auto xij = atoms[i].x - atoms[j].x;
        auto yij = atoms[i].y - atoms[j].y;
        auto zij = atoms[i].z - atoms[j].z;
        auto r2 = xij*xij + yij*yij + zij*zij;
        auto r = sqrt(r2);
        enuc += atoms[i].atomic_number * atoms[j].atomic_number / r;
      }
    cout << "\tNuclear repulsion energy = " << enuc << endl;

    /*** =========================== ***/
    /*** create basis set            ***/
    /*** =========================== ***/

    auto shells = make_sto3g_basis(atoms);
    size_t nao = 0;
    for (auto s=0; s<shells.size(); ++s)
      nao += shells[s].size();

    /*** =========================== ***/
    /*** compute 1-e integrals       ***/
    /*** =========================== ***/

    // initializes the Libint integrals library ... now ready to compute
    libint2::initialize();

    // compute overlap integrals
    auto S = compute_1body_ints(shells, Operator::overlap);

    // compute kinetic-energy integrals
    auto T = compute_1body_ints(shells, Operator::kinetic);

    // compute nuclear-attraction integrals
    Matrix V = compute_1body_ints(shells, Operator::nuclear, atoms);

    // Core Hamiltonian = T + V
    Matrix H = T + V;

    // T and V no longer needed, free up the memory
    T.resize(0,0);
    V.resize(0,0);

    /*** =========================== ***/
    /*** build initial-guess density ***/
    /*** =========================== ***/

    const auto use_hcore_guess = false;  // use core Hamiltonian eigenstates to guess density?
                                         // set to true to match the result of versions 0, 1, and 2 of the code
                                         // HOWEVER !!! even for medium-size molecules hcore will usually fail !!!
                                         // thus set to false to use Superposition-Of-Atomic-Densities (SOAD) guess
    Matrix D;
    if (use_hcore_guess) { // hcore guess
      // solve H C = e S C
      Eigen::GeneralizedSelfAdjointEigenSolver<Matrix> gen_eig_solver(H, S);
      auto eps = gen_eig_solver.eigenvalues();
      auto C = gen_eig_solver.eigenvectors();

      // compute density, D = C(occ) . C(occ)T
      auto C_occ = C.leftCols(ndocc);
      D = C_occ * C_occ.transpose();
    }
    else {  // SOAD as the guess density, assumes STO-nG basis
      D = compute_soad(atoms);
    }

    /*** =========================== ***/
    /*** main iterative loop         ***/
    /*** =========================== ***/

    const auto maxiter = 100;
    const auto conv = 1e-12;
    auto iter = 0;
    auto rmsd = 0.0;
    auto ediff = 0.0;
    auto ehf = 0.0;
    do {
      const auto tstart = std::chrono::high_resolution_clock::now();
      ++iter;

      // Save a copy of the energy and the density
      auto ehf_last = ehf;
      auto D_last = D;

      // build a new Fock matrix
      auto F = H;
      start = omp_get_wtime();
      F += compute_2body_fock(shells, D);
      fock_time += omp_get_wtime() - start;

      // solve F C = e S C
      Eigen::GeneralizedSelfAdjointEigenSolver<Matrix> gen_eig_solver(F, S);
      auto eps = gen_eig_solver.eigenvalues();
      auto C = gen_eig_solver.eigenvectors();

      // compute density, D = C(occ) . C(occ)T
      auto C_occ = C.leftCols(ndocc);
      D = C_occ * C_occ.transpose();

      // compute HF energy
      ehf = 0.0;
      for (auto i = 0; i < nao; i++)
        for (auto j = 0; j < nao; j++)
          ehf += D(i,j) * (H(i,j) + F(i,j));

      // compute difference with last iteration
      ediff = ehf - ehf_last;
      rmsd = (D - D_last).norm();

      const auto tstop = std::chrono::high_resolution_clock::now();
      const std::chrono::duration<double> time_elapsed = tstop - tstart;

      if (iter == 1)
        std::cout <<
        "\n\n Iter        E(elec)              E(tot)               Delta(E)             RMS(D)         Time(s)\n";
      printf(" %02d %20.12f %20.12f %20.12f %20.12f %10.5lf\n", iter, ehf, ehf + enuc,
             ediff, rmsd, time_elapsed.count());

    } while (((fabs(ediff) > conv) || (fabs(rmsd) > conv)) && (iter < maxiter));

    printf("** Hartree-Fock energy = %20.12f\n", ehf + enuc);

    libint2::finalize(); // done with libint

  } // end of try block; if any exceptions occurred, report them and exit cleanly

  catch (const char* ex) {
    cerr << "caught exception: " << ex << endl;
    return 1;
  }
  catch (std::string& ex) {
    cerr << "caught exception: " << ex << endl;
    return 1;
  }
  catch (std::exception& ex) {
    cerr << ex.what() << endl;
    return 1;
  }
  catch (...) {
    cerr << "caught unknown exception\n";
    return 1;
  }

  printf("\n\nFock build time = %.2f\n", fock_time);

  return 0;
}

// this reads the geometry in the same format used in older versions of the code
std::vector<Atom> read_dotgeom(std::istream& is) {
  size_t natom;
  is >> natom;

  std::vector<Atom> atoms(natom);
  for (auto i = 0; i < natom; i++)
    is >> atoms[i].atomic_number >> atoms[i].x >> atoms[i].y >> atoms[i].z;

  return atoms;
}

// this reads the geometry in the standard xyz format supported by most chemistry software
std::vector<Atom> read_dotxyz(std::istream& is) {
  // line 1 = # of atoms
  size_t natom;
  is >> natom;
  // read off the rest of line 1 and discard
  std::string rest_of_line;
  std::getline(is, rest_of_line);

  // line 2 = comment (possibly empty)
  std::string comment;
  std::getline(is, comment);

  std::vector<Atom> atoms(natom);
  for (auto i = 0; i < natom; i++) {
    std::string element_label;
    double x, y, z;
    is >> element_label >> x >> y >> z;

    // .xyz files report element labels, hence convert to atomic numbers
    int Z;
    if (element_label == "H")
      Z = 1;
    else if (element_label == "C")
      Z = 6;
    else if (element_label == "N")
      Z = 7;
    else if (element_label == "O")
      Z = 8;
    else if (element_label == "F")
      Z = 9;
    else if (element_label == "S")
      Z = 16;
    else if (element_label == "Cl")
      Z = 17;
    else {
      std::cerr << "read_dotxyz: element label \"" << element_label << "\" is not recognized" << std::endl;
      throw "Did not recognize element label in .xyz file";
    }

    atoms[i].atomic_number = Z;

    // .xyz files report Cartesian coordinates in angstroms; convert to bohr
    const auto angstrom_to_bohr = 1 / 0.52917721092; // 2010 CODATA value
    atoms[i].x = x * angstrom_to_bohr;
    atoms[i].y = y * angstrom_to_bohr;
    atoms[i].z = z * angstrom_to_bohr;
  }

  return atoms;
}

std::vector<Atom> read_geometry(const std::string& filename) {

  std::cout << "Will read geometry from " << filename << std::endl;
  std::ifstream is(filename);
  assert(is.good());

  // to prepare for MPI parallelization, we will read the entire file into a string that can be
  // broadcast to everyone, then converted to an std::istringstream object that can be used just like std::ifstream
  std::ostringstream oss;
  oss << is.rdbuf();
  // use ss.str() to get the entire contents of the file as an std::string
  // broadcast
  // then make an std::istringstream in each process
  std::istringstream iss(oss.str());

  // check the extension: if .xyz, assume the standard XYZ format, otherwise use the same format used by hf.v1
  if ( filename.rfind(".xyz") != std::string::npos)
    return read_dotxyz(iss);
  else if ( filename.rfind(".geom") != std::string::npos)
    return read_dotgeom(iss);
  else
    throw std::invalid_argument("unknown filename extension");
}

std::vector<libint2::Shell> make_sto3g_basis(const std::vector<Atom>& atoms) {

  using libint2::Shell;

  std::vector<Shell> shells;

  for(auto a=0; a<atoms.size(); ++a) {

    // STO-3G basis set
    // cite: W. J. Hehre, R. F. Stewart, and J. A. Pople, The Journal of Chemical Physics 51, 2657 (1969)
    //       doi: 10.1063/1.1672392
    // obtained from https://bse.pnl.gov/bse/portal
    switch (atoms[a].atomic_number) {
      case 1: // Z=1: hydrogen
        shells.push_back(
            {
              {3.425250910, 0.623913730, 0.168855400}, // exponents of primitive Gaussians
              {  // contraction 0: s shell (l=0), spherical=false, contraction coefficients
                {0, false, {0.15432897, 0.53532814, 0.44463454}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}   // origin coordinates
            }
        );
        break;

      case 6: // Z=6: carbon
        shells.push_back(
            {
              {71.616837000, 13.045096000, 3.530512200},
              {
                {0, false, {0.15432897, 0.53532814, 0.44463454}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        shells.push_back(
            {
              {2.941249400, 0.683483100, 0.222289900},
              {
                {0, false, {-0.09996723, 0.39951283, 0.70011547}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        shells.push_back(
            {
              {2.941249400, 0.683483100, 0.222289900},
              { // contraction 0: p shell (l=1), spherical=false
                {1, false, {0.15591627, 0.60768372, 0.39195739}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        break;

      case 7: // Z=7: nitrogen
        shells.push_back(
            {
              {99.106169000, 18.052312000, 4.885660200},
              {
                {0, false, {0.15432897, 0.53532814, 0.44463454}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        shells.push_back(
            {
              {3.780455900, 0.878496600, 0.285714400},
              {
                {0, false, {-0.09996723, 0.39951283, 0.70011547}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        shells.push_back(
            {
          {3.780455900, 0.878496600, 0.285714400},
              { // contraction 0: p shell (l=1), spherical=false
                {1, false, {0.15591627, 0.60768372, 0.39195739}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        break;

      case 8: // Z=8: oxygen
        shells.push_back(
            {
              {130.709320000, 23.808861000, 6.443608300},
              {
                {0, false, {0.15432897, 0.53532814, 0.44463454}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        shells.push_back(
            {
              {5.033151300, 1.169596100, 0.380389000},
              {
                {0, false, {-0.09996723, 0.39951283, 0.70011547}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        shells.push_back(
            {
              {5.033151300, 1.169596100, 0.380389000},
              { // contraction 0: p shell (l=1), spherical=false
                {1, false, {0.15591627, 0.60768372, 0.39195739}}
              },
              {{atoms[a].x, atoms[a].y, atoms[a].z}}
            }
        );
        break;

      default:
        throw "do not know STO-3G basis for this Z";
    }

  }

  return shells;
}

size_t nbasis(const std::vector<libint2::Shell>& shells) {
  size_t n = 0;
  for (const auto& shell: shells)
    n += shell.size();
  return n;
}

size_t max_nprim(const std::vector<libint2::Shell>& shells) {
  size_t n = 0;
  for (auto shell: shells)
    n = std::max(shell.nprim(), n);
  return n;
}

int max_l(const std::vector<libint2::Shell>& shells) {
  int l = 0;
  for (auto shell: shells)
    for (auto c: shell.contr)
      l = std::max(c.l, l);
  return l;
}

std::vector<size_t> map_shell_to_basis_function(const std::vector<libint2::Shell>& shells) {
  std::vector<size_t> result;
  result.reserve(shells.size());

  size_t n = 0;
  for (auto shell: shells) {
    result.push_back(n);
    n += shell.size();
  }

  return result;
}

// computes Superposition-Of-Atomic-Densities guess for the molecular density matrix
// in minimal basis; occupies subshell by smearing electrons evenly over the orbitals
Matrix compute_soad(const std::vector<Atom>& atoms) {

  // compute number of atomic orbitals
  size_t nao = 0;
  for(const auto& atom: atoms) {
    const auto Z = atom.atomic_number;
    if (Z == 1 || Z == 2) // H, He
      nao += 1;
    else if (Z <= 10) // Li - Ne
      nao += 5;
    else
      throw "SOAD with Z > 10 is not yet supported";
  }

  // compute the minimal basis density
  Matrix D = Matrix::Zero(nao, nao);
  size_t ao_offset = 0; // first AO of this atom
  for(const auto& atom: atoms) {
    const auto Z = atom.atomic_number;
    if (Z == 1 || Z == 2) { // H, He
      D(ao_offset, ao_offset) = Z; // all electrons go to the 1s
      ao_offset += 1;
    }
    else if (Z <= 10) {
      D(ao_offset, ao_offset) = 2; // 2 electrons go to the 1s
      D(ao_offset+1, ao_offset+1) = (Z == 3) ? 1 : 2; // Li? only 1 electron in 2s, else 2 electrons
      // smear the remaining electrons in 2p orbitals
      const double num_electrons_per_2p = (Z > 4) ? (double)(Z - 4)/3 : 0;
      for(auto xyz=0; xyz!=3; ++xyz)
        D(ao_offset+2+xyz, ao_offset+2+xyz) = num_electrons_per_2p;
      ao_offset += 5;
    }
  }

  return D * 0.5; // we use densities normalized to # of electrons/2
}

Matrix compute_1body_ints(const std::vector<libint2::Shell>& shells,
                          libint2::Operator obtype,
                          const std::vector<Atom>& atoms)
{
  using libint2::Shell;
  using libint2::Engine;
  using libint2::Operator;

  const auto n = nbasis(shells);
  Matrix result(n,n);

  // construct the overlap integrals engine
  Engine engine(obtype, max_nprim(shells), max_l(shells), 0);
  // nuclear attraction ints engine needs to know where the charges sit ...
  // the nuclei are charges in this case; in QM/MM there will also be classical charges
  if (obtype == Operator::nuclear) {
    std::vector<std::pair<double,std::array<double,3>>> q;
    for(const auto& atom : atoms) {
      q.push_back( {static_cast<double>(atom.atomic_number), {{atom.x, atom.y, atom.z}}} );
    }
    engine.set_params(q);
  }

  auto shell2bf = map_shell_to_basis_function(shells);

  // buf[0] points to the target shell set after every call  to engine.compute()
  const auto& buf = engine.results();

  // loop over unique shell pairs, {s1,s2} such that s1 >= s2
  // this is due to the permutational symmetry of the real integrals over Hermitian operators: (1|2) = (2|1)
  for(auto s1=0; s1!=shells.size(); ++s1) {

    auto bf1 = shell2bf[s1]; // first basis function in this shell
    auto n1 = shells[s1].size();

    for(auto s2=0; s2<=s1; ++s2) {

      auto bf2 = shell2bf[s2];
      auto n2 = shells[s2].size();

      // compute shell pair
      engine.compute(shells[s1], shells[s2]);
      if (buf[0] == nullptr)
        continue;  // if all integrals screened out, skip to next quartet

      // "map" buffer to a const Eigen Matrix, and copy it to the corresponding blocks of the result
      Eigen::Map<const Matrix> buf_mat(buf[0], n1, n2);
      result.block(bf1, bf2, n1, n2) = buf_mat;
      if (s1 != s2) // if s1 >= s2, copy {s1,s2} to the corresponding {s2,s1} block, note the transpose!
      result.block(bf2, bf1, n2, n1) = buf_mat.transpose();

    }
  }

  return result;
}

Matrix compute_2body_fock(const std::vector<libint2::Shell>& shells,
                          const Matrix& D) {
  using libint2::Shell;
  using libint2::Engine;
  using libint2::Operator;

  auto time_elapsed = std::chrono::duration<double>::zero();

  const auto n = nbasis(shells);
  auto shell2bf = map_shell_to_basis_function(shells);
  Matrix Gtotal = Matrix::Zero(n,n);

#pragma omp parallel
// or try this if you do not want all variables shared by default
//#pragma omp parallel default(none) shared(shells,shell2bf,D,Gtotal,time_elapsed)
  {
    const int tid = omp_get_thread_num();
    const int nthread = omp_get_num_threads();
    long count = 0;

    // construct the 2-electron repulsion integrals engine
    Engine engine(Operator::coulomb, max_nprim(shells), max_l(shells), 0);
    Matrix G = Matrix::Zero(n,n);

    // buf[0] points to the target shell set after every call  to engine.compute()
    const auto &buf = engine.results();

    // The problem with the simple Fock builder is that permutational symmetries of the Fock,
    // density, and two-electron integrals are not taken into account to reduce the cost.
    // To make the simple Fock builder efficient we must rearrange our computation.
    // The most expensive step in Fock matrix construction is the evaluation of 2-e integrals;
    // hence we must minimize the number of computed integrals by taking advantage of their permutational
    // symmetry. Due to the multiplicative and Hermitian nature of the Coulomb kernel (and realness
    // of the Gaussians) the permutational symmetry of the 2-e ints is given by the following relations:
    //
    // (12|34) = (21|34) = (12|43) = (21|43) = (34|12) = (43|12) = (34|21) = (43|21)
    //
    // (here we use chemists' notation for the integrals, i.e in (ab|cd) a and b correspond to
    // electron 1, and c and d -- to electron 2).
    //
    // It is easy to verify that the following set of nested loops produces a permutationally-unique
    // set of integrals:
    // foreach a = 0 .. n-1
    //   foreach b = 0 .. a
    //     foreach c = 0 .. a
    //       foreach d = 0 .. (a == c ? b : c)
    //         compute (ab|cd)
    //
    // The only complication is that we must compute integrals over shells. But it's not that complicated ...
    //
    // The real trick is figuring out to which matrix elements of the Fock matrix each permutationally-unique
    // (ab|cd) contributes. STOP READING and try to figure it out yourself. (to check your answer see below)

    // loop over permutatinally-unique set of shells
    for (auto s1 = 0; s1 != shells.size(); ++s1) {

      auto bf1_first = shell2bf[s1]; // first basis function in this shell
      auto n1 = shells[s1].size();   // number of basis functions in this shell

      for (auto s2 = 0; s2 <= s1; ++s2) {

        auto bf2_first = shell2bf[s2];
        auto n2 = shells[s2].size();

        for (auto s3 = 0; s3 <= s1; ++s3) {

          auto bf3_first = shell2bf[s3];
          auto n3 = shells[s3].size();

          const auto s4_max = (s1 == s3) ? s2 : s3;
          for (auto s4 = 0; s4 <= s4_max; ++s4, ++count) {

            if (tid == (count%nthread)) {
              auto bf4_first = shell2bf[s4];
              auto n4 = shells[s4].size();

              // compute the permutational degeneracy (i.e. # of equivalents) of the given shell set
              auto s12_deg = (s1 == s2) ? 1.0 : 2.0;
              auto s34_deg = (s3 == s4) ? 1.0 : 2.0;
              auto s12_34_deg = (s1 == s3) ? (s2 == s4 ? 1.0 : 2.0) : 2.0;
              auto s1234_deg = s12_deg * s34_deg * s12_34_deg;

              const auto tstart = std::chrono::high_resolution_clock::now();

              engine.compute(shells[s1], shells[s2], shells[s3], shells[s4]);
              const auto *buf_1234 = buf[0];
              if (buf_1234 == nullptr)
                continue; // if all integrals screened out, skip to next quartet

              const auto tstop = std::chrono::high_resolution_clock::now();
              time_elapsed += tstop - tstart;

              // ANSWER
              // 1) each shell set of integrals contributes up to 6 shell sets of the Fock matrix:
              //    F(a,b) += (ab|cd) * D(c,d)
              //    F(c,d) += (ab|cd) * D(a,b)
              //    F(b,d) -= 1/4 * (ab|cd) * D(a,c)
              //    F(b,c) -= 1/4 * (ab|cd) * D(a,d)
              //    F(a,c) -= 1/4 * (ab|cd) * D(b,d)
              //    F(a,d) -= 1/4 * (ab|cd) * D(b,c)
              // 2) each permutationally-unique integral (shell set) must be scaled by its degeneracy,
              //    i.e. the number of the integrals/sets equivalent to it
              // 3) the end result must be symmetrized
              for (auto f1 = 0, f1234 = 0; f1 != n1; ++f1) {
                const auto bf1 = f1 + bf1_first;
                for (auto f2 = 0; f2 != n2; ++f2) {
                  const auto bf2 = f2 + bf2_first;
                  for (auto f3 = 0; f3 != n3; ++f3) {
                    const auto bf3 = f3 + bf3_first;
                    for (auto f4 = 0; f4 != n4; ++f4, ++f1234) {
                      const auto bf4 = f4 + bf4_first;

                      const auto value = buf_1234[f1234];

                      const auto value_scal_by_deg = value * s1234_deg;

                      G(bf1, bf2) += D(bf3, bf4) * value_scal_by_deg;
                      G(bf3, bf4) += D(bf1, bf2) * value_scal_by_deg;
                      G(bf1, bf3) -= 0.25 * D(bf2, bf4) * value_scal_by_deg;
                      G(bf2, bf4) -= 0.25 * D(bf1, bf3) * value_scal_by_deg;
                      G(bf1, bf4) -= 0.25 * D(bf2, bf3) * value_scal_by_deg;
                      G(bf2, bf3) -= 0.25 * D(bf1, bf4) * value_scal_by_deg;
                    }
                  }
                }
              }
            }

          }
        }
      }
    }

#pragma omp critical
    { Gtotal += G; }

  } // OMP parallel section

  // symmetrize the result and return
  Matrix Gtotal_t = Gtotal.transpose();
  return 0.5 * (Gtotal + Gtotal_t);
}