/****************************************************************************
**
** Copyright (C) 2018 TU Wien, ACIN, Vision 4 Robotics (V4R) group
** Contact: v4r.acin.tuwien.ac.at
**
** This file is part of V4R
**
** V4R is distributed under dual licenses - GPLv3 or closed source.
**
** GNU General Public License Usage
** V4R is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published
** by the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** V4R is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** Please review the following information to ensure the GNU General Public
** License requirements will be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
**
** Commercial License Usage
** If GPL is not suitable for your project, you must purchase a commercial
** license to use V4R. Licensees holding valid commercial V4R licenses may
** use this file in accordance with the commercial license agreement
** provided with the Software or, alternatively, in accordance with the
** terms contained in a written agreement between you and TU Wien, ACIN, V4R.
** For licensing terms and conditions please contact
** office<at>acin.tuwien.ac.at.
**
**
** The copyright holder additionally grants the author(s) of the file the right
** to use, copy, modify, merge, publish, distribute, sublicense, and/or
** sell copies of their contributions without any restrictions.
**
****************************************************************************/

/**
 * @file motion_blur_detection.cpp
 * @author Georg Halmetschlager-Funek (gh@acin.tuwien.ac.at)
 * @date 2018
 * @brief
 *
 */

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

#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>

#include <v4r/common/img_loader.h>
#include <v4r/common/motion_blur_detection.h>
namespace po = boost::program_options;

bool parseArgs(int argc, char** argv, std::string& input_folder, std::string& result,
               v4r::MotionBlurDetectionType& method);

int main(int argc, char** argv) {
  std::string img_folder;
  std::string result;
  v4r::MotionBlurDetectionType method;

  if (!parseArgs(argc, argv, img_folder, result, method)) {
    return 0;
  }

  if (std::system("clear") == -1) {
  }
  v4r::ImgContainer img_load(img_folder);
  std::vector<cv::Mat> imgs;
  imgs = img_load.getImages();

  std::vector<cv::String> filenames;
  filenames = img_load.getFileNames();

  if (!(imgs.size() > 0 && filenames.size() > 0)) {
    std::cout << "Wasn't able to load images from " + img_folder << std::endl;
    std::cout << "Aborting ..." << std::endl;
    return -1;
  }

  auto img_ptr = std::make_shared<std::vector<cv::Mat>>(imgs);
  v4r::MotionBlurDetection mb(img_ptr, method);
  mb.calculateMotionBlurForImgVector();
  std::vector<double> bluriness;
  bluriness = mb.getMotionBlurMeasures();

  std::stringstream output;
  for (size_t i = 0; i < bluriness.size(); i++) {
    output << bluriness[i] << "," << filenames[i] << "\n";
  }

  std::cout << "Saving file..." << result << std::endl;

  std::ofstream stddevs;
  stddevs.open(result);
  stddevs << output.str();
  stddevs.close();
}

bool parseArgs(int argc, char** argv, std::string& input_folder, std::string& result,
               v4r::MotionBlurDetectionType& method) {
  std::string method_str;
  po::options_description arguments(
      "Detect Motion Blur. Estimates the bluriness of an image, using either the Variance of Laplacian (VL) or the "
      "Variance of a Modified Laplacian (VML). The higher the variance, the sharper the image.");
  arguments.add_options()("help,h", "");
  arguments.add_options()("img_path,i", po::value<std::string>(&input_folder)->default_value("./"),
                          "Path to image folder");
  arguments.add_options()("result,r", po::value<std::string>(&result)->default_value("./blur.csv"),
                          "Path to result file");
  arguments.add_options()("method,r", po::value<std::string>(&method_str)->default_value("VL"),
                          "Available Methods: Variation of Modified Laplacian (VML), Variation of "
                          "Laplacian (VL) [Default]");

  po::options_description all("");
  all.add(arguments);
  po::options_description visible("");
  visible.add(arguments);
  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(all).run(), vm);
  po::notify(vm);

  std::string usage = "General usage: [-options] []";

  if (vm.count("help")) {
    std::cout << usage << std::endl;
    std::cout << visible;
    return false;
  }

  boost::to_upper(method_str);
  if (method_str == "VL") {
    method = v4r::MotionBlurDetectionType::VL;
  } else if (method_str == "VML") {
    method = v4r::MotionBlurDetectionType::VML;
  } else {
    std::cout << "Unknown method, user provided " << method_str << " . Using VL." << std::endl;
    method = v4r::MotionBlurDetectionType::VL;
  }

  return true;
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_PREDICATES_FUNCTIONS_TABLE_ISROW_HPP_INCLUDED
#define NT2_PREDICATES_FUNCTIONS_TABLE_ISROW_HPP_INCLUDED

#include <nt2/predicates/functions/isrow.hpp>
#include <nt2/include/functions/numel.hpp>
#include <nt2/include/functions/extent.hpp>
#include <boost/fusion/include/at.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( isrow_, tag::cpu_
                            , (A0)
                            , (unspecified_<A0>)
                            )
  {
    typedef bool result_type;

    BOOST_FORCEINLINE
    result_type operator()(const A0& a0) const
    {
      typedef typename meta::call<tag::extent_(A0 const&)>::type extent_t;
      extent_t ex = nt2::extent(a0);
      std::size_t nz = nt2::numel(ex);
      if (nz)
        return boost::fusion::at_c<1>(ex) == nz;
      else
        {
          if (boost::fusion::at_c<0>(ex) !=  1) return false;
          for(size_t i=2; i < ex.size(); ++i)
            {
              if (ex[i] !=  1u) return false;
            }
        }
      return true;
    }
  };
} }

#endif


#include <random>

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

#include <aikido/common/RNG.hpp>
#include <aikido/distance/JointAvoidanceConfigurationRanker.hpp>
#include <aikido/statespace/Rn.hpp>
#include <aikido/statespace/StateSpace.hpp>

#include "eigen_tests.hpp"

using aikido::distance::JointAvoidanceConfigurationRanker;
using aikido::statespace::R1;
using aikido::statespace::dart::MetaSkeletonStateSpace;
using aikido::statespace::dart::MetaSkeletonStateSpacePtr;
using dart::dynamics::BodyNode;
using dart::dynamics::BodyNodePtr;
using dart::dynamics::RevoluteJoint;
using dart::dynamics::Skeleton;
using dart::dynamics::SkeletonPtr;

static constexpr double EPS = 1e-6;

static BodyNode::Properties create_BodyNodeProperties(const std::string& name)
{
  BodyNode::Properties bodyProperties;
  bodyProperties.mName = name;
  return bodyProperties;
}

class JointAvoidanceConfigurationRankerTest : public ::testing::Test
{
protected:
  void SetUp() override
  {
    // Manipulator with 2 revolute joints.
    mManipulator = Skeleton::create("Manipulator");

    // Root joint
    RevoluteJoint::Properties properties1;
    properties1.mAxis = Eigen::Vector3d::UnitY();
    properties1.mName = "Joint1";

    auto pair1 = mManipulator->createJointAndBodyNodePair<RevoluteJoint>(
        nullptr, properties1, create_BodyNodeProperties("root"));
    pair1.first->setPositionLowerLimit(0, 0.0);
    pair1.first->setPositionUpperLimit(0, 2 * M_PI);
    bn1 = pair1.second;

    // joint 2, body 2
    RevoluteJoint::Properties properties2;
    properties2.mAxis = Eigen::Vector3d::UnitY();
    properties2.mName = "Joint2";
    properties2.mT_ParentBodyToJoint.translation() = Eigen::Vector3d(0, 0, 1);
    auto pair2 = mManipulator->createJointAndBodyNodePair<RevoluteJoint>(
        bn1, properties2, create_BodyNodeProperties("leaf"));

    bn2 = pair2.second;
    mStateSpace = std::make_shared<MetaSkeletonStateSpace>(mManipulator.get());

    Eigen::Vector2d defaultPosition(0.0, 0.0);
    mManipulator->setPositions(defaultPosition);
  }

  SkeletonPtr mManipulator;
  MetaSkeletonStateSpacePtr mStateSpace;
  BodyNodePtr bn1, bn2;
};

TEST_F(JointAvoidanceConfigurationRankerTest, Constructor)
{
  EXPECT_THROW(
      JointAvoidanceConfigurationRanker(nullptr, mManipulator),
      std::invalid_argument);

  EXPECT_THROW(
      JointAvoidanceConfigurationRanker(mStateSpace, nullptr),
      std::invalid_argument);

  EXPECT_THROW(
      JointAvoidanceConfigurationRanker(mStateSpace, nullptr),
      std::invalid_argument);

  std::vector<double> negativeWeights{-1, 0};
  EXPECT_THROW(
      JointAvoidanceConfigurationRanker(
          mStateSpace, mManipulator, negativeWeights),
      std::invalid_argument);

  std::vector<double> wrongDimensionWeights{1};
  EXPECT_THROW(
      JointAvoidanceConfigurationRanker(
          mStateSpace, mManipulator, wrongDimensionWeights),
      std::invalid_argument);

  JointAvoidanceConfigurationRanker rankerOne(mStateSpace, mManipulator);
  DART_UNUSED(rankerOne);

  std::vector<double> goodWeights{1, 2};
  JointAvoidanceConfigurationRanker rankerTwo(
      mStateSpace, mManipulator, goodWeights);
  DART_UNUSED(rankerTwo);
}

TEST_F(JointAvoidanceConfigurationRankerTest, OrderTest)
{
  std::vector<Eigen::Vector2d> jointPositions{Eigen::Vector2d(0.3, 0.1),
                                              Eigen::Vector2d(0.1, 0.4),
                                              Eigen::Vector2d(0.2, 0.1)};

  std::vector<aikido::statespace::CartesianProduct::ScopedState> states;
  for (std::size_t i = 0; i < jointPositions.size(); ++i)
  {
    auto state = mStateSpace->createState();
    mStateSpace->convertPositionsToState(jointPositions[i], state);
    states.emplace_back(state.clone());
  }

  JointAvoidanceConfigurationRanker ranker(mStateSpace, mManipulator);
  ranker.rankConfigurations(states);

  Eigen::VectorXd rankedState(2);
  jointPositions[0] = Eigen::Vector2d(0.3, 0.1);
  jointPositions[1] = Eigen::Vector2d(0.2, 0.1);
  jointPositions[2] = Eigen::Vector2d(0.1, 0.4);
  for (std::size_t i = 0; i < states.size(); ++i)
  {
    mStateSpace->convertStateToPositions(states[i], rankedState);
    EXPECT_EIGEN_EQUAL(rankedState, jointPositions[i], EPS);
  }
}

TEST_F(JointAvoidanceConfigurationRankerTest, WeightedOrderTest)
{
  std::vector<Eigen::Vector2d> jointPositions{Eigen::Vector2d(0.3, 0.1),
                                              Eigen::Vector2d(0.1, 0.4),
                                              Eigen::Vector2d(0.2, 0.1)};

  std::vector<aikido::statespace::CartesianProduct::ScopedState> states;
  for (std::size_t i = 0; i < jointPositions.size(); ++i)
  {
    auto state = mStateSpace->createState();
    mStateSpace->convertPositionsToState(jointPositions[i], state);
    states.emplace_back(state.clone());
  }

  std::vector<double> weights{10, 1};
  JointAvoidanceConfigurationRanker ranker(mStateSpace, mManipulator, weights);
  ranker.rankConfigurations(states);

  Eigen::VectorXd rankedState(2);
  jointPositions[0] = Eigen::Vector2d(0.3, 0.1);
  jointPositions[1] = Eigen::Vector2d(0.2, 0.1);
  jointPositions[2] = Eigen::Vector2d(0.1, 0.4);
  for (std::size_t i = 0; i < states.size(); ++i)
  {
    mStateSpace->convertStateToPositions(states[i], rankedState);
    EXPECT_EIGEN_EQUAL(rankedState, jointPositions[i], EPS);
  }
}


/*=============================================================================

  SKSURGERYPCLCPP: Image-guided surgery functions, in C++, using PCL.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "sksGeneralizedIterativeClosestPointWrapper.h"
#include "sksExceptionMacro.h"
#include "sksConversionUtils.h"
#include <sksGeneralizedIterativeClosestPoint.h>
#include <boost/python/extract.hpp>

namespace sks
{

//-----------------------------------------------------------------------------
double GeneralizedIterativeClosestPointWrapper(const np::ndarray& source,
                                               const np::ndarray& target,
                                               float normalSearchRadius,
                                               float angleThresholdInDegreesForNormalCorrespondence,
                                               unsigned int maxNumberOfIterations,
                                               float maxCorrespondenceDistance,
                                               float transformationEpsilon,
                                               float fitnessEpsilon,
                                               np::ndarray& result,
                                               np::ndarray& transformedSource
                                               )
{
  CheckInputsForRegistration(source, target, transformedSource, result);

  pcl::PointCloud<pcl::PointXYZ>::Ptr sourceCloud = ConvertInputToPointCloud(source);
  pcl::PointCloud<pcl::PointXYZ>::Ptr targetCloud = ConvertInputToPointCloud(target);
  pcl::PointCloud<pcl::PointXYZ>::Ptr transformedSourceCloud = ConvertInputToPointCloud(transformedSource);

  Eigen::Matrix4f finalTransform;

  double residual = sks::GeneralizedIterativeClosestPoint(sourceCloud,
                                                          targetCloud,
                                                          normalSearchRadius,
                                                          angleThresholdInDegreesForNormalCorrespondence,
                                                          maxNumberOfIterations,
                                                          maxCorrespondenceDistance,
                                                          transformationEpsilon,
                                                          fitnessEpsilon,
                                                          finalTransform,
                                                          transformedSourceCloud);

  ConvertEigenToNumpy(finalTransform, result);
  ConvertPointCloudToNumpy(transformedSourceCloud, transformedSource);

  return residual;
}

} // end namespace


#define BOOST_TEST_MODULE "test_parse_range"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif
#include <mjolnir/input/utility.hpp>
#include <cstdint>

BOOST_AUTO_TEST_CASE(test_parse_range_close_close)
{
    {
        const auto actual = mjolnir::parse_range("[1, 10]");
        const auto expect = std::vector<std::size_t>{1,2,3,4,5,6,7,8,9,10};
        BOOST_TEST(expect.size() == actual.size());
        if(expect.size() == actual.size())
        {
            BOOST_TEST(std::equal(expect.begin(), expect.end(), actual.begin()));
        }
    }
    {
        const auto actual = mjolnir::parse_range("[ 1, 10 ]");
        const auto expect = std::vector<std::size_t>{1,2,3,4,5,6,7,8,9,10};
        BOOST_TEST(expect.size() == actual.size());
        if(expect.size() == actual.size())
        {
            BOOST_TEST(std::equal(expect.begin(), expect.end(), actual.begin()));
        }
    }
    {
        const auto actual = mjolnir::parse_range("[1,10]");
        const auto expect = std::vector<std::size_t>{1,2,3,4,5,6,7,8,9,10};
        BOOST_TEST(expect.size() == actual.size());
        if(expect.size() == actual.size())
        {
            BOOST_TEST(std::equal(expect.begin(), expect.end(), actual.begin()));
        }
    }
}

BOOST_AUTO_TEST_CASE(test_parse_range_close_open)
{
    {
        const auto actual = mjolnir::parse_range("[1, 10)");
        const auto expect = std::vector<std::size_t>{1,2,3,4,5,6,7,8,9};
        BOOST_TEST(expect.size() == actual.size());
        if(expect.size() == actual.size())
        {
            BOOST_TEST(std::equal(expect.begin(), expect.end(), actual.begin()));
        }
    }
    {
        const auto actual = mjolnir::parse_range("[ 1, 10 )");
        const auto expect = std::vector<std::size_t>{1,2,3,4,5,6,7,8,9};
        BOOST_TEST(expect.size() == actual.size());
        if(expect.size() == actual.size())
        {
            BOOST_TEST(std::equal(expect.begin(), expect.end(), actual.begin()));
        }
    }
    {
        const auto actual = mjolnir::parse_range("[1,10)");
        const auto expect = std::vector<std::size_t>{1,2,3,4,5,6,7,8,9};
        BOOST_TEST(expect.size() == actual.size());
        if(expect.size() == actual.size())
        {
            BOOST_TEST(std::equal(expect.begin(), expect.end(), actual.begin()));
        }
    }
}

BOOST_AUTO_TEST_CASE(test_parse_range_open_close)
{
    {
        const auto actual = mjolnir::parse_range("(1, 10]");
        const auto expect = std::vector<std::size_t>{2,3,4,5,6,7,8,9,10};
        BOOST_TEST(expect.size() == actual.size());
        if(expect.size() == actual.size())
        {
            BOOST_TEST(std::equal(expect.begin(), expect.end(), actual.begin()));
        }
    }
    {
        const auto actual = mjolnir::parse_range("( 1, 10 ]");
        const auto expect = std::vector<std::size_t>{2,3,4,5,6,7,8,9,10};
        BOOST_TEST(expect.size() == actual.size());
        if(expect.size() == actual.size())
        {
            BOOST_TEST(std::equal(expect.begin(), expect.end(), actual.begin()));
        }
    }
    {
        const auto actual = mjolnir::parse_range("(1,10]");
        const auto expect = std::vector<std::size_t>{2,3,4,5,6,7,8,9,10};
        BOOST_TEST(expect.size() == actual.size());
        if(expect.size() == actual.size())
        {
            BOOST_TEST(std::equal(expect.begin(), expect.end(), actual.begin()));
        }
    }
}
BOOST_AUTO_TEST_CASE(test_parse_range_open_open)
{
    {
        const auto actual = mjolnir::parse_range("(1, 10)");
        const auto expect = std::vector<std::size_t>{2,3,4,5,6,7,8,9};
        BOOST_TEST(expect.size() == actual.size());
        if(expect.size() == actual.size())
        {
            BOOST_TEST(std::equal(expect.begin(), expect.end(), actual.begin()));
        }
    }
    {
        const auto actual = mjolnir::parse_range("( 1, 10 )");
        const auto expect = std::vector<std::size_t>{2,3,4,5,6,7,8,9};
        BOOST_TEST(expect.size() == actual.size());
        if(expect.size() == actual.size())
        {
            BOOST_TEST(std::equal(expect.begin(), expect.end(), actual.begin()));
        }
    }
    {
        const auto actual = mjolnir::parse_range("(1,10)");
        const auto expect = std::vector<std::size_t>{2,3,4,5,6,7,8,9};
        BOOST_TEST(expect.size() == actual.size());
        if(expect.size() == actual.size())
        {
            BOOST_TEST(std::equal(expect.begin(), expect.end(), actual.begin()));
        }
    }
}


/**
 * @file
 * @author lkoppel
 * Defines an Eigen expression for a square identity matrix, which can be trivially
 * multiplied.
 */

#ifndef WAVE_GEOMETRY_IDENTITYMATRIX_HPP
#define WAVE_GEOMETRY_IDENTITYMATRIX_HPP

#include <Eigen/Geometry>
#include "wave/geometry/src/util/meta/template_helpers.hpp"

namespace wave {

/**
 * An Eigen expression for a square identity matrix
 *
 * @f[ a \times b = \text{CrossMatrix}(a) b @f]
 *
 * In different works, `CrossMatrix(a)` might be written as @f$ a^{\times} @f$ or
 * @f$ \hat{a} @f$ .
 *
 * @tparam VecType the type of the vector expression
 */
// The implementation of an Eigen expression here was guided by
// https://eigen.tuxfamily.org/dox/TopicNewExpressionType.html
template <typename Scalar, int N>
class IdentityMatrix : public Eigen::Matrix<Scalar, N, N>::IdentityReturnType {
 public:
    using MatrixType = Eigen::Matrix<Scalar, N, N>;
    using Base = typename MatrixType::IdentityReturnType;
    IdentityMatrix() : Base{MatrixType::Identity()} {}
};

}  // namespace wave

namespace Eigen {

namespace internal {

// Static attributes of our Identity expression
// See https://eigen.tuxfamily.org/dox/TopicNewExpressionType.html
template <typename Scalar, int N>
struct traits<::wave::IdentityMatrix<Scalar, N>>
    : Eigen::Matrix<Scalar, N, N>::IdentityReturnType {};

}  // namespace internal

/**
 * Multiply an Identity expression by another matrix on the right
 */
//@todo Consider scalar mixing?
template <typename Scalar, int N, typename OtherDerived>
EIGEN_DEVICE_FUNC inline const OtherDerived &operator*(
  const wave::IdentityMatrix<Scalar, N> &, const Eigen::MatrixBase<OtherDerived> &rhs) {
    static_assert(OtherDerived::RowsAtCompileTime == N ||
                    OtherDerived::RowsAtCompileTime == Eigen::Dynamic,
                  "Invalid matrix product");
    return rhs.derived();
}

/**
 * Multiply an Identity expression by another matrix on the left
 */
//@todo Consider scalar mixing?
template <typename Scalar, int N, typename OtherDerived>
EIGEN_DEVICE_FUNC inline const OtherDerived &operator*(
  const Eigen::MatrixBase<OtherDerived> &lhs, const wave::IdentityMatrix<Scalar, N> &) {
    static_assert(OtherDerived::ColsAtCompileTime == N ||
                    OtherDerived::ColsAtCompileTime == Eigen::Dynamic,
                  "Invalid matrix product");
    return lhs.derived();
}

/**
 * Multiply an Identity expression by another Identity expression
 * (Provided to break tie between the other two specializations)
 */
template <typename Scalar, int N>
EIGEN_DEVICE_FUNC inline const wave::IdentityMatrix<Scalar, N> &operator*(
  const wave::IdentityMatrix<Scalar, N> &i, const wave::IdentityMatrix<Scalar, N> &) {
    return i;
}

}  // namespace Eigen

#endif  // WAVE_GEOMETRY_IDENTITYMATRIX_HPP


#define BOOST_TEST_MODULE "test_omp_random_number_generator"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <mjolnir/omp/RandomNumberGenerator.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <random>
#include <algorithm>

// It tests that ...
// - RandomNumberGenerator<OpenMPSimulatorTraits> can generate random numbers
//   in parallel
// - RandomNumberGenerator<OpenMPSimulatorTraits>'s can generate the same
//   sequence of random numbers if they are initialized with the same seed.
BOOST_AUTO_TEST_CASE(test_omp_random_number_generator)
{
    const int max_number_of_threads = omp_get_max_threads();
    BOOST_TEST_WARN(max_number_of_threads > 2);
    BOOST_TEST_MESSAGE("maximum number of threads = " << omp_get_max_threads());

    mjolnir::LoggerManager::set_default_logger("test_omp_random_number_generator.log");

    const std::uint32_t seed=123456789;

    for(int num_thread=1; num_thread<=max_number_of_threads; ++num_thread)
    {
        omp_set_num_threads(num_thread);
        BOOST_TEST_MESSAGE("maximum number of threads = " << omp_get_max_threads());

        mjolnir::RandomNumberGenerator<
            mjolnir::OpenMPSimulatorTraits<double, mjolnir::UnlimitedBoundary>
            > rng_1(seed);
        mjolnir::RandomNumberGenerator<
            mjolnir::OpenMPSimulatorTraits<double, mjolnir::UnlimitedBoundary>
            > rng_2(seed);

#pragma omp parallel for
        for(std::size_t i=0; i<10000; ++i)
        {
            // this part is called in parallel
            // {
            const auto real01_1 = rng_1.uniform_real01();
            const auto real01_2 = rng_2.uniform_real01();
            // }

#pragma omp critical
            {
                BOOST_TEST(0.0 <= real01_1);
                BOOST_TEST(real01_1 < 1.0);
                BOOST_TEST(0.0 <= real01_2);
                BOOST_TEST(real01_2 < 1.0);
                BOOST_TEST(real01_1 == real01_2);
            }
        }
    }
}


/*
 [auto_generated]
 boost/numeric/odeint/integrate/detail/integrate_const.hpp

 [begin_description]
 integrate const implementation
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_CONST_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_CONST_HPP_INCLUDED

#include <boost/range/algorithm/for_each.hpp>

#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/util/unit_helper.hpp>
#include <boost/numeric/odeint/iterator/const_step_time_iterator.hpp>
#include <boost/numeric/odeint/iterator/integrate/detail/integrate_adaptive.hpp>
#include <boost/numeric/odeint/iterator/integrate/detail/functors.hpp>
#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>

namespace boost {
namespace numeric {
namespace odeint {
namespace detail {

// forward declaration
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_adaptive(
        Stepper stepper , System system , State &start_state ,
        Time &start_time , Time end_time , Time &dt ,
        Observer observer , controlled_stepper_tag
);

template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_const(
        Stepper stepper , System system , State &start_state ,
        Time start_time , Time end_time , Time dt ,
        Observer observer , stepper_tag
)
{
    size_t obs_calls = 0;

    boost::for_each( make_const_step_time_range( stepper , system , start_state ,
                                                 start_time , end_time , dt ) ,
                     // should we use traits<Stepper>::state_type here instead of State? NO!
                     obs_caller< Observer >( obs_calls , observer ) );

    // step integration steps gives step+1 observer calls
    return obs_calls-1;
}

template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_const(
        Stepper stepper , System system , State &start_state ,
        Time start_time , Time end_time , Time dt ,
        Observer observer , controlled_stepper_tag
)
{
    typename odeint::unwrap_reference< Observer >::type &obs = observer;

    Time time = start_time;
    const Time time_step = dt;
    int step = 0;

    while( less_eq_with_sign( static_cast<Time>(time+time_step) , end_time , dt ) )
    {
        obs( start_state , time );
        detail::integrate_adaptive( stepper , system , start_state , time , time+time_step , dt ,
                                    null_observer() , controlled_stepper_tag() );
        // direct computation of the time avoids error propagation happening when using time += dt
        // we need clumsy type analysis to get boost units working here
        ++step;
        time = start_time + static_cast< typename unit_value_type<Time>::type >(step) * time_step;
    }
    obs( start_state , time );

    return step;
}

template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_const(
        Stepper stepper , System system , State &start_state ,
        Time start_time , Time end_time , Time dt ,
        Observer observer , dense_output_stepper_tag
)
{
    size_t obs_calls = 0;

    boost::for_each( make_const_step_time_range( stepper , system , start_state ,
                                                 start_time , end_time , dt ) ,
                     obs_caller< Observer >( obs_calls , observer ) );
    return obs_calls-1;
}

} } } }

#endif


// It is extremely important to use this header
// if you are using the numpy_eigen interface
#include <numpy_eigen/boost_python_headers.hpp>
#include <boost/shared_ptr.hpp>

#include <aslam/backend/ScalarDesignVariable.hpp>
#include <aslam/backend/ErrorTermObservation.hpp>
#include <aslam/backend/ErrorTermMotion.hpp>
#include <aslam/backend/ErrorTermPrior.hpp>

// The title of this library must match exactly the title of the produced library
BOOST_PYTHON_MODULE(libaslam_backend_tutorial_python)
{
  // Here we have to export the design variables and error terms used in this problem.
  // The aslam_python library exports the interface of the base classes so we
  // leverage this work by pointing the boost::python wrapper at these base classes.

  using namespace boost::python;
  using namespace aslam::backend;

  class_<
    // The first template argument is the class we are wrapping.
    ScalarDesignVariable,
    // This tells boost::python so wrap this class as a boost shared
    // pointer. While this is not strictly required, it does help as
    // it will allow us to pass objects from python to the OptimizationProblem
    // as boost shared pointers.
    boost::shared_ptr<ScalarDesignVariable>,
    // This template argument tells boost::python to inherit the interface of
    // DesignVariable.
    bases<DesignVariable>
    >("ScalarDesignVariable", // <-- this sets the name of the class in Python
      init<double>("ScalarDesignVariable( initialValue )")) // <-- Here we tell python that the default constructor takes a double. The string is for documentation.
    .def("value", &ScalarDesignVariable::value) // <-- And here we export methods specific to this class.
    ;

class_<ErrorTermObservation, boost::shared_ptr<ErrorTermObservation>, bases<ErrorTerm> >
("ErrorTermObservation",
 init<ScalarDesignVariable *,ScalarDesignVariable *,double,double>("ErrorTermObservation(ScalarDesignVariable x_k, ScalarDesignVariable w, double y, double sigma2_n)"))
;

class_<ErrorTermMotion, boost::shared_ptr<ErrorTermMotion>, bases<ErrorTerm> >
("ErrorTermMotion",
 init<ScalarDesignVariable *,ScalarDesignVariable *,double,double>("ErrorTermMotion(ScalarDesignVariable x_k, ScalarDesignVariable x_kp1, double u, double sigma2_u)"))
;

class_<ErrorTermPrior, boost::shared_ptr<ErrorTermPrior>, bases<ErrorTerm> >
("ErrorTermPrior",
 init<ScalarDesignVariable *,double,double>("ErrorTermPrior(ScalarDesignVariable * x, double hat_x, double sigma2_x)"))
;

}


  // Point Cloud Library
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/io.h>
#include <boost/thread/thread.hpp>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/passthrough.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/common.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/registration/icp.h>
#include <pcl/common/time.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/console/print.h>
#include <pcl/features/vfh.h>
#include <pcl/features/organized_edge_detection.h>
// #include <pcl/features/rsd.h>
#include <Eigen/Core>
// Standatrd libraries
#include <iostream>
#include <vector>

int main (int argc, char** argv)
{
  std::string filename = argv[1];
  std::cout << "Reading " << filename << std::endl;
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
  if(pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename, *cloud) == -1) // load the file
  {
    PCL_ERROR ("Couldn't read file");
    return -1;
  }
  std::cout << "points: " << cloud->points.size () <<std::endl;

  pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
  ne.setInputCloud (cloud);
  // Create an empty kdtree representation, and pass it to the normal estimation object.
  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
  ne.setSearchMethod (tree);
  // Output datasets
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
  // Use all neighbors in a sphere of radius 3cm
  ne.setRadiusSearch (0.3);
  // Compute the features
  ne.compute (*cloud_normals);

  // cloud_normals->points.size () should have the same size as the input cloud->points.size ()*
  pcl::PCDWriter writer;
  writer.write<pcl::Normal> ("normals_scene.pcd", *cloud_normals, false);

  pcl::OrganizedEdgeFromRGBNormals<pcl::PointXYZRGB, pcl::Normal, pcl::Label> oed;
  oed.setInputNormals (cloud_normals);
  oed.setInputCloud (cloud);
  oed.setDepthDisconThreshold (0.03); // 3cm
  oed.setMaxSearchNeighbors (100);
  pcl::PointCloud<pcl::Label> labels;
  std::vector<pcl::PointIndices> label_indices;
  oed.compute (labels, label_indices);

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr occluding_edges (new pcl::PointCloud<pcl::PointXYZRGB>),
        occluded_edges (new pcl::PointCloud<pcl::PointXYZRGB>),
        boundary_edges (new pcl::PointCloud<pcl::PointXYZRGB>),
        high_curvature_edges (new pcl::PointCloud<pcl::PointXYZRGB>),
        rgb_edges (new pcl::PointCloud<pcl::PointXYZRGB>);

  pcl::copyPointCloud (*cloud, label_indices[0].indices, *boundary_edges);
  pcl::copyPointCloud (*cloud, label_indices[1].indices, *occluding_edges);
  pcl::copyPointCloud (*cloud, label_indices[2].indices, *occluded_edges);
  pcl::copyPointCloud (*cloud, label_indices[3].indices, *high_curvature_edges);
  pcl::copyPointCloud (*cloud, label_indices[4].indices, *rgb_edges);

  writer.write<pcl::PointXYZRGB> ("occluding_edges_scene.pcd", *occluding_edges, false);
  writer.write<pcl::PointXYZRGB> ("boundary_edges_scene.pcd", *boundary_edges, false);
  writer.write<pcl::PointXYZRGB> ("occluded_edges_scene.pcd", *occluded_edges, false);
  writer.write<pcl::PointXYZRGB> ("high_curvature_edges_scene.pcd", *high_curvature_edges, false);
  writer.write<pcl::PointXYZRGB> ("rgb_edges_scene.pcd", *rgb_edges, false);
}


#include <boost/python.hpp>

#include "../../../def_submodule.hpp"

#include "IceRay/geometry/simple/usphere.hpp"

#include "./simple.hpp"

void expose_IceRay_geometry_simple_usphere()
 {
  //MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( core );
  MAKE_SUBMODULE( geometry );

  typedef GS_DDMRM::S_IceRay::S_geometry::GC_usphere        GTs_usphere;

  boost::python::class_< GTs_usphere, boost::python::bases<GTs__base> >( "GeometrySimpleUSphere" )
    .def( boost::python::init<>() )
  ;

 }


#include <boost/test/unit_test.hpp>

#include "bit_stream.h"

using namespace memb;

std::string prettyBitString(uint16_t value, size_t maxBits)
{
    std::string result;
    for (size_t i = 0; i < maxBits; ++i) {
        result.push_back('0' + value % 2);
        value /= 2;
    }
    std::reverse(result.begin(), result.end());

    return result;
}

std::string repr(const std::vector<uint8_t>& data)
{
    std::ostringstream stream;
    for (auto value : data) {
        stream << prettyBitString(value, 8);
    }

    return stream.str();
}

BOOST_AUTO_TEST_SUITE(bitStream)

BOOST_AUTO_TEST_CASE(bitStreamWorks)
{
    BitStream bitStream;

    std::vector<PrefixCode> prefixCodes = {
        {1023, 14},
        {33, 6},
        {0, 4},
        {1234, 11},
        {7, 2}
    };

    for (const auto& code : prefixCodes) {
        bitStream.push(code);
    }

    std::string bitStreamRepr = repr(bitStream.data());

    std::ostringstream stream;
    size_t totalLength = 0;
    for (const auto& code: prefixCodes) {
        stream << prettyBitString(code.code, code.bitsCount);
        totalLength += code.bitsCount;
    }
    stream << std::string(8 - totalLength % 8, '0');
    std::string simpleRepr = stream.str();

    BOOST_CHECK_EQUAL(bitStreamRepr, simpleRepr);
}

BOOST_AUTO_TEST_SUITE_END()


//  Copyright Neil Groves 2009. 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)
//
//
// For more information, see http://www.boost.org/libs/range/
//
#include <boost/range/algorithm/heap_algorithm.hpp>

#include <boost/test/test_tools.hpp>
#include <boost/test/unit_test.hpp>

#include <boost/assign.hpp>
#include <algorithm>
#include <functional>
#include <list>
#include <numeric>
#include <deque>
#include <vector>

namespace boost
{
    namespace
    {
        template<class Container1, class Container2>
        void check_equal(const Container1& cont1, const Container2& cont2)
        {
            BOOST_CHECK_EQUAL_COLLECTIONS(
                cont1.begin(), cont1.end(),
                cont2.begin(), cont2.end()
                );
        }

        void test()
        {
            using namespace boost::assign;

            std::vector<int> reference;
            reference += 1,2,3,4,5,6,7,8,9;

            std::vector<int> test_cont(reference);
            std::vector<int> test_cont2(reference);

            std::make_heap(reference.begin(), reference.end());
            boost::make_heap(test_cont);
            check_equal(reference, test_cont);
            boost::make_heap(boost::make_iterator_range(test_cont2));
            check_equal(reference, test_cont2);

            std::push_heap(reference.begin(), reference.end());
            boost::push_heap(test_cont);
            check_equal(reference, test_cont);
            boost::push_heap(boost::make_iterator_range(test_cont2));
            check_equal(reference, test_cont2);

            std::make_heap(reference.begin(), reference.end());
            boost::make_heap(test_cont);
            boost::make_heap(boost::make_iterator_range(test_cont2));

            std::sort_heap(reference.begin(), reference.end());
            boost::sort_heap(test_cont);
            check_equal(reference, test_cont);
            boost::sort_heap(boost::make_iterator_range(test_cont2));
            check_equal(reference, test_cont2);

            std::make_heap(reference.begin(), reference.end());
            boost::make_heap(test_cont);
            boost::make_heap(boost::make_iterator_range(test_cont2));

            std::pop_heap(reference.begin(), reference.end());
            boost::pop_heap(test_cont);
            check_equal(reference, test_cont);
            boost::pop_heap(boost::make_iterator_range(test_cont2));
            check_equal(reference, test_cont2);
        }

        template<class BinaryPredicate>
        void test_pred(BinaryPredicate pred)
        {
            using namespace boost::assign;

            std::vector<int> reference;
            reference += 1,2,3,4,5,6,7,8,9;
            std::sort(reference.begin(), reference.end(), pred);

            std::vector<int> test_cont(reference);
            std::vector<int> test_cont2(reference);

            std::make_heap(reference.begin(), reference.end(), pred);
            boost::make_heap(test_cont, pred);
            check_equal(reference, test_cont);
            boost::make_heap(boost::make_iterator_range(test_cont2), pred);
            check_equal(reference, test_cont2);

            reference.push_back(5);
            test_cont.push_back(5);
            test_cont2.push_back(5);
            std::push_heap(reference.begin(), reference.end(), pred);
            boost::push_heap(test_cont, pred);
            check_equal(reference, test_cont);
            boost::push_heap(boost::make_iterator_range(test_cont2), pred);
            check_equal(reference, test_cont2);

            std::make_heap(reference.begin(), reference.end(), pred);
            boost::make_heap(test_cont, pred);
            boost::make_heap(boost::make_iterator_range(test_cont2), pred);

            std::sort_heap(reference.begin(), reference.end(), pred);
            boost::sort_heap(test_cont, pred);
            check_equal(reference, test_cont);
            boost::sort_heap(boost::make_iterator_range(test_cont2), pred);
            check_equal(reference, test_cont2);

            std::make_heap(reference.begin(), reference.end(), pred);
            boost::make_heap(test_cont, pred);
            boost::make_heap(boost::make_iterator_range(test_cont2), pred);

            std::pop_heap(reference.begin(), reference.end(), pred);
            boost::pop_heap(test_cont, pred);
            check_equal(reference, test_cont);
            boost::pop_heap(boost::make_iterator_range(test_cont2), pred);
            check_equal(reference, test_cont2);
        }

        void test_heap()
        {
            test();
            test_pred(std::less<int>());
            test_pred(std::greater<int>());
        }
    }
}

boost::unit_test::test_suite*
init_unit_test_suite(int argc, char* argv[])
{
    boost::unit_test::test_suite* test
        = BOOST_TEST_SUITE( "RangeTestSuite.algorithm.heap" );

    test->add( BOOST_TEST_CASE( &boost::test_heap ) );

    return test;
}


#ifndef _RYSQ_FOCK_IMPL_HPP_
#define _RYSQ_FOCK_IMPL_HPP_

#include <boost/mpl/int.hpp>
#include <boost/mpl/if.hpp>
#include <boost/utility/enable_if.hpp>

#include "externals/cxx/namespace.hpp"

#include <rysq/fock.hpp>

#include "fock-matrix.hpp"
#include "quartet.hpp"
#include "eri-impl.hpp"
#include "meta.hpp"
#include "transpose.hpp"
#include "fock-eval.hpp"

struct rysq::Fock::Impl {
    virtual ~Impl() {};
    virtual void operator()(const Quartet<Center> &centers,
			    const density_set &D,
			    const fock_set &F,
			    const Parameters &parameters) = 0;

    template<type A, type B, type C, type D>
    static Impl* instance(const Quartet<Shell> &shells);

};

BEGIN_NAMESPACE(rysq, fock)

namespace mpl = boost::mpl;

template<class braket, class Enable = void>
struct Kernel;

template<type A_, type B_, type C_, type D_>
struct Kernel<meta::braket<A_,B_,C_,D_>, /*void*/
	      typename boost::enable_if<
		  eri::Kernel<typename meta::sorted<A_,B_,C_,D_>::braket>
		  >::type> : Fock::Impl {

    typedef meta::braket<A_,B_,C_,D_> braket;
    typedef eri::Kernel<typename meta::sorted<A_,B_,C_,D_>::braket> eri;
    static const int transpose = meta::sorted<A_,B_,C_,D_>::value;

    const Quartet<Shell> quartet;

    Kernel(const Quartet<Shell> &quartet)
	: quartet(quartet.transposed(transpose)) { }

    static Fock::Impl* instance(const Quartet<Shell> &quartet) {
	return new Kernel(quartet);
    }

    void operator()(const Quartet<Center> &centers,
		    const density_set &D6,
		    const fock_set &F6,
		    const Parameters &parameters) {
	eval(quartet, centers, D6, F6, parameters);
    }

    static void eval(const Quartet<Shell> &quartet,
		     const Quartet<Center> &centers,
		     const density_set &D6,
		     const fock_set &F6,
		     const Parameters &parameters) {
	double I[braket::size] __attribute__ ((aligned(16))) = { 0.0 };
	double scale = 1.0;
	double cutoff = parameters.cutoff/(5e1*SQRT_4PI5);
	eri::eval(quartet, centers, I, scale, cutoff);
	Parameters p = parameters;
	p.scale *= SQRT_4PI5;
	fock::eval<braket>(D6, F6, I, p);
    }
};

template<class braket, class Enable>
struct Kernel {
    static Fock::Impl* instance(const Quartet<Shell> &quartet) {
	typedef typename braket::A A;
	typedef typename braket::B B;
	typedef typename braket::C C;
	typedef typename braket::D D;

	typedef typename braket::bra bra;
	typedef typename braket::ket ket;
	typedef mpl::int_<(braket::L)/2+1> N;

	// 		typedef mpl::bool_<A::L == 0 && B::L != 0> t0;
	// 		typedef mpl::bool_<C::L == 0 && D::L != 0> t1;
	// 		typedef mpl::bool_<bra::L < ket::L && bra::L == 0> t01;

	typedef mpl::bool_<((A::value == C::value && B::value < D::value) ||
			    (A::value < C::value))> t01;

	typedef meta::transpose<A, B, (A::value < B::value)> bra_;
	typedef meta::transpose<C, D, (C::value < D::value)> ket_;

	typedef typename mpl::if_<t01,
	    typename ket_::type, typename bra_::type>::type kBra;

	int transpose = Transpose::mask(bra_::value, ket_::value, t01::value);
	return NULL;
    }
};

template<type A, type B, int N>
struct Kernel<meta::state<A,B>, mpl::int_<N> >
    : Fock::Impl, Quadrature::Transform {

    typedef meta::state<A,B> bra;

    const Quartet<Shell> quartet;
    Quadrature::Primitives primitives;
    Parameters parameters;
    fock::Matrix matrix;

    Kernel(const Quartet<Shell> &quartet)
	: quartet(quartet),
	  primitives(this->quartet),
	  matrix(this->quartet) {
    }

    static Fock::Impl* instance(const Quartet<Shell> &quartet) {
	return new Kernel(quartet);
    }

    void operator()(const Quartet<Center> &centers,
		    const density_set &D,
		    const fock_set &F,
		    const Parameters &parameters) {
	matrix.set(D);

	double scale = 1.0;
	double cutoff = parameters.cutoff/(5e1*SQRT_4PI5);
	Quadrature::eval<bra,N>(quartet, centers, scale, cutoff, primitives, *this);

	scale = parameters.scale*SQRT_4PI5;
	double scale2 = parameters.scale2*scale;
	matrix.get(scale, scale2, F);
    }

    struct Transform {
	static const int ni = bra::A::size;
	static const int nj = bra::B::size;
	density_set D;
	fock_set F;
	Transform(const density_set &D, const fock_set &F)
	    : D(D), F(F) {}
	void operator()(int k, int l, int kl, const double *I) {
	    fock::eval<ni,nj>(k, l, kl, D, F, I);
	}
    };

    void operator()(const Quadrature::Primitives &primitives) {
	int K = primitives.K;
	if (K == 0) return;
	const Shell &c = quartet[2];
	const Shell &d = quartet[3];
	Transform transform(matrix.D(), matrix.F());
	Quadrature::apply<bra,N>(c, d, primitives, transform);
    }

};

END_NAMESPACE(rysq,fock)

namespace rysq {

    typedef Fock::density_set density_set;
    typedef Fock::fock_set fock_set;

    template<class bra, class ket = void>
    struct Transform {
	static const size_t size = bra::size*ket::size;
	density_set D;
	fock_set F;
	Parameters parameters;
	Transform(const density_set &D, const fock_set &F,
		  const Parameters &parameters)
	    : D(D), F(F), parameters(parameters) {}
	void operator()(double scale, const double (&Q)[size], size_t num = size) {
	    Parameters p = parameters;
	    p.scale *= SQRT_4PI5;
	    fock::eval<bra,ket>(D, F, Q, p);
	}
    };

    template<class bra>
    struct Transform<bra,void> {
	static const int ni = bra::A::size;
	static const int nj = bra::B::size;
	density_set D;
	fock_set F;
	Parameters parameters;
	Transform(const density_set &D, const fock_set &F,
		  const Parameters &parameters)
	    : D(D), F(F), parameters(parameters) {}
	void operator()(int k, int l, int kl, const double *I) {
	    fock::eval<ni,nj>(k, l, kl, D, F, I);
	}
	void operator()(const double Q[bra::size]) {
	    double scale = 1.0;
	    Parameters p = parameters;
	    p.scale *= SQRT_4PI5;
	    fock::eval<bra>(D, F, Q, p);
	}
    };

    template<type A, type B, type C, type D>
    inline Fock::Impl* Fock::Impl::instance(const Quartet<Shell> &quartet) {
	typedef meta::braket<A,B,C,D> braket;
	return fock::Kernel<braket>::instance(quartet);
    }

}

#endif /* _RYSQ_FOCK_HPP_ */


// Boost.Maral library (Molecular Archiving, Retrieval & Algorithm Library)
//
// Copyright (C) 2014-2015 Armin Madadkar Sobhani
//
// 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)
//
// $Id$

#ifndef MARAL_ALGORITHMS_CONNECT_HPP
#define MARAL_ALGORITHMS_CONNECT_HPP

#include <vector>
//#include <omp.h>

#include <boost/range/distance.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm/sort.hpp>

namespace maral {

template<class BondType, class AtomRange, typename T = float>
inline void connect(
    const AtomRange& rng
,   T tolerance = T(0.45))
{
    std::vector<typename AtomRange::value_type> atoms;
    atoms.reserve(boost::distance(rng));
    boost::copy(rng, back_inserter(atoms));
    connect<BondType>(atoms, tolerance);
}

template<class BondType, class AtomType, typename T = float>
inline void connect(
    std::vector<AtomType>& atoms
,   T tolerance = T(0.45))
{
    boost::sort(atoms,
        [](const AtomType i, const AtomType j)
        { return ((*i)[2] < (*j)[2]); });

    //omp_lock_t writelock;
    //omp_init_lock(&writelock);

#pragma omp parallel for
    for (int i = 0 ; i < atoms.size() ; ++i)
    {
        for (int j = i+1 ; j < atoms.size() ; ++j)
        {
            T cutoff = atoms[i]->covalent_radius().value()
                     + atoms[j]->covalent_radius().value()
                     + tolerance;
            T z_diff = (*atoms[j])[2] - (*atoms[i])[2];
            if (z_diff > cutoff)
                break;
            if (distance(atoms[i]->center(), atoms[j]->center()) < cutoff)
            #pragma omp critical
            {
                //omp_set_lock(&writelock);
                make<BondType>(atoms[i], atoms[j]);
                //omp_unset_lock(&writelock);
            }
        }
    }
}

}   // maral

#endif    // MARAL_ALGORITHMS_CONNECT_HPP


#include <Data/DATA.h>
#include <Driver/SIMULATION.h>
#include <Equation/MATRIX_BUNDLE.h>
#include <Equation/NONLINEAR_EQUATION.h>
#include <Evolution/EVOLUTION.h>
#include <Evolution/QUALITY.h>
#include <Force/FORCE.h>
#include <Force/FORCE_TYPE.h>
#include <Utilities/EIGEN_HELPERS.h>
#include <Utilities/LOG.h>
#include <iostream>
#include <vector>
#include <Eigen/IterativeSolvers>
using namespace Mechanics;
///////////////////////////////////////////////////////////////////////
template<class TV> Matrix<typename TV::Scalar,Dynamic,1> NONLINEAR_EQUATION<TV>::
Get_Unknowns(const DATA<TV>& data,const FORCE<TV>& force) const
{
    Matrix<T,Dynamic,1> velocities,forces,unknowns;
    data.Pack_Velocities(velocities);
    STORED_FORCE<T> stored_force;
    force.Pack_Forces(stored_force);
    forces=stored_force.Vector();
    unknowns.resize(velocities.size()+forces.size());
    for(int i=0;i<velocities.size();i++){
        unknowns(i)=velocities(i);}
    for(int i=0;i<forces.size();i++){
        unknowns(i+velocities.size())=forces(i);}
    return unknowns;
}
///////////////////////////////////////////////////////////////////////
template<class TV> void NONLINEAR_EQUATION<TV>::
Increment_Unknowns(const Matrix<T,Dynamic,1>& unknowns,DATA<TV>& data,FORCE<TV>& force)
{
    int velocity_dof=Velocity_DOF();
    Matrix<T,Dynamic,1> solve_velocities=unknowns.block(0,0,velocity_dof,1);
    STORED_FORCE<T> stored_force;
    force.Pack_Forces(stored_force);
    stored_force.Set(unknowns.block(velocity_dof,0,unknowns.size()-velocity_dof,1));
    force.Increment_Forces(stored_force,1);

    Matrix<T,Dynamic,1> current_velocities;
    data.Pack_Velocities(current_velocities);
    Matrix<T,Dynamic,1> candidate_velocities=current_velocities+solve_velocities;
    Unpack_Velocities(data,candidate_velocities);
    data.Step();
}
///////////////////////////////////////////////////////////////////////
template<class TV> void NONLINEAR_EQUATION<TV>::
Identify_DOF(const DATA<TV>& data,const FORCE<TV>& force,int index)
{
    int current_index=0;
    for(int i=0;i<data.size();i++){
        int data_size=kinematic_projection_matrices[i].rows();//=data[i]->Velocity_DOF();
        if(index<current_index+data_size){
            data[i]->Identify_DOF(index-current_index);
            return;}
        current_index+=data_size;
    }
    for(int i=0;i<force.size();i++){
        int force_size=force[i]->DOF();
        if(index<current_index+force_size){
            force[i]->Identify_DOF(index-current_index);
            return;}
        current_index+=force_size;}
    LOG::cout<<"Unidentified DOF"<<std::endl;
}
///////////////////////////////////////////////////////////////////////
template<class TV> void NONLINEAR_EQUATION<TV>::
Unpack_Velocities(DATA<TV>& data,const Matrix<T,Dynamic,1>& velocities)
{
    assert(data.size()==1);
    Matrix<Matrix<T,Dynamic,1>,Dynamic,1> full_velocities(data.size());
    int current_index=0;
    for(int i=0;i<data.size();i++){
        int compressed_size=kinematic_projection_matrices[i].rows();
        full_velocities[i]=kinematic_projection_matrices[i].transpose()*velocities.block(current_index,0,current_index+compressed_size,1);
        current_index+=compressed_size;
    }
    Matrix<T,Dynamic,1> full_velocity;
    Merge_Block_Vectors(full_velocities,full_velocity);
    data.Unpack_Velocities(full_velocity);
}
///////////////////////////////////////////////////////////////////////
template<class TV> void NONLINEAR_EQUATION<TV>::
Store_Errors(DATA<TV>& data,const Matrix<T,Dynamic,1>& errors)
{
    assert(data.size()==1);
    Matrix<Matrix<T,Dynamic,1>,Dynamic,1> full_errors(data.size());
    int current_index=0;
    for(int i=0;i<data.size();i++){
        int compressed_size=kinematic_projection_matrices[i].rows();
        full_errors[i]=kinematic_projection_matrices[i].transpose()*errors.block(current_index,0,current_index+compressed_size,1);
        current_index+=compressed_size;
    }
    Matrix<T,Dynamic,1> full_error;
    Merge_Block_Vectors(full_errors,full_error);
    data.Store_Errors(full_error);
}
///////////////////////////////////////////////////////////////////////
template<class TV> void NONLINEAR_EQUATION<TV>::
Initialize(DATA<TV>& data,FORCE<TV>& force)
{
    kinematic_projection_matrices.resize(data.size());
    for(int i=0;i<data.size();i++){
        data[i]->Kinematic_Projection(kinematic_projection_matrices[i]);}
}
///////////////////////////////////////////////////////////////////////
template<class TV> void NONLINEAR_EQUATION<TV>::
Linearize(DATA<TV>& data,FORCE<TV>& force,const T dt,const T time,const bool stochastic)
{
    system.Initialize(data,force);
    for(int i=0;i<data.size();i++){
        data[i]->Inertia(dt,system.jacobian_block_terms[i],system.inverse_inertia_matrices[i],system.error_blocks[i]);}
    for(int i=0;i<force.size();i++){
        force[i]->Identify_Interactions_And_Compute_Errors(data,force,dt,time,system,stochastic);}
    system.Scale_Errors(data,force,kinematic_projection_matrices);
    for(int i=0;i<force.size();i++){
        force[i]->Compute_Derivatives(data,force,system);}
    system.Scale_Derivatives(data,force,kinematic_projection_matrices);
    jacobian.resize(data.Velocity_DOF(),data.Velocity_DOF());
    Merge_Block_Matrices(system.jacobian_blocks,jacobian);
    Merge_Block_Vectors(system.error_blocks,error);

    // build the Hessian
    //SparseMatrix<T> hessian_addition;
    //Merge_Block_Matrices(system.hessian_blocks,hessian_addition);
    //accurate_hessian=jacobian.adjoint()*jacobian+hessian_addition;
    hessian=jacobian.adjoint()*jacobian;
}
///////////////////////////////////////////////////////////////////////
GENERIC_TYPE_DEFINITION(NONLINEAR_EQUATION)


#ifndef __SIMPLE3DOBJECT_INCLUDE__
#define __SIMPLE3DOBJECT_INCLUDE__

#include <Eigen/Core>
#include <Eigen/Eigen>
#include <GL/glew.h>

#include <vector>

class Simple3DObject {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Simple3DObject(Eigen::Vector3f position, bool isStatic);
    ~Simple3DObject();

    void addPoint(float x, float y, float z, float r, float g, float b);
    void pushToGPU();
    void clear();

    void setDrawingType(GLenum type);

    void draw();

    void translate(const Eigen::Vector3f& t);
    void setPosition(const Eigen::Vector3f& p);

    void setRT(const Eigen::Matrix4f& mRT);

    void rotate(const Eigen::Quaternionf& rot);
    void rotate(const Eigen::Matrix3f& m);
    void setRotation(const Eigen::Quaternionf& rot);
    void setRotation(const Eigen::Matrix3f& m);

    const Eigen::Vector3f& getPosition() const;

    Eigen::Matrix4f getModelMatrix() const;
private:
    std::vector<float> vertices_;
    std::vector<float> colors_;
    std::vector<unsigned int> indices_;

    bool isStatic_;

    GLenum drawingType_;

    GLuint vaoID_;
    /*
            Vertex buffer IDs:
            - [0]: vertices coordinates;
            - [1]: RGB color values;
            - [2]: indices;
     */
    GLuint vboID_[3];

    Eigen::Vector3f position_;
    Eigen::Quaternionf rotation_;

};

#endif


// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// 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 <boost/thread/locks.hpp>

#include "SurgSim/Devices/DeviceFilters/PoseTransform.h"

#include "SurgSim/Math/MathConvert.h"
#include "SurgSim/Math/Matrix.h"

using SurgSim::DataStructures::DataGroup;
using SurgSim::Math::RigidTransform3d;
using SurgSim::Math::Vector3d;

namespace SurgSim
{
namespace Devices
{

SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::PoseTransform, PoseTransform);

PoseTransform::PoseTransform(const std::string& name) :
	DeviceFilter(name),
	m_transform(RigidTransform3d::Identity()),
	m_transformInverse(RigidTransform3d::Identity()),
	m_translationScale(1.0)
{
	SURGSIM_ADD_SERIALIZABLE_PROPERTY(PoseTransform, double, TranslationScale,
		getTranslationScale, setTranslationScale);
	SURGSIM_ADD_SERIALIZABLE_PROPERTY(PoseTransform, RigidTransform3d, Transform, getTransform, setTransform);
}

void PoseTransform::filterInput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result)
{
	boost::lock_guard<boost::mutex> lock(m_mutex); // Prevent the transform or scaling from being set simultaneously.

	*result = dataToFilter;  // Pass on all the data entries.

	RigidTransform3d pose; // If there is a pose, scale the translation, then transform the result.
	if (dataToFilter.poses().get(DataStructures::Names::POSE, &pose))
	{
		pose.translation() *= m_translationScale;
		pose = m_transform * pose;
		result->poses().set(DataStructures::Names::POSE, pose);
	}

	// If there is a linear velocity, scale then rotate it.  The linear velocity is scaled because it is the change
	// in translation over time, and the translation is being scaled.
	Vector3d linearVelocity;
	if (dataToFilter.vectors().get(DataStructures::Names::LINEAR_VELOCITY, &linearVelocity))
	{
		linearVelocity *= m_translationScale;
		linearVelocity = m_transform.linear() * linearVelocity;
		result->vectors().set(DataStructures::Names::LINEAR_VELOCITY, linearVelocity);
	}

	Vector3d angularVelocity; // If there is an angular velocity, rotate it.
	if (dataToFilter.vectors().get(DataStructures::Names::ANGULAR_VELOCITY, &angularVelocity))
	{
		angularVelocity = m_transform.linear() * angularVelocity;
		result->vectors().set(DataStructures::Names::ANGULAR_VELOCITY, angularVelocity);
	}
}

void PoseTransform::filterOutput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result)
{
	boost::lock_guard<boost::mutex> lock(m_mutex); // Prevent the transform or scaling from being set simultaneously.

	*result = dataToFilter;  // Pass on all the data entries.

	// Since the haptic devices will compare the data in the output DataGroup to a raw input pose, the filter must
	// perform the reverse transform and scaling to data used by the haptic devices.

	// The force and torque must be transformed into device space.  In order to reliably display the desired
	// forces as calculated by the simulation, the nominal forces and torques are not scaled by the translation scaling.
	// The benefit is that increasing the translation scaling does not result in larger penetrations for the
	// same force, but the downside is that as the translation scaling increases it becomes more likely that the haptic
	// feedback loop at a surface will become "active" (smaller motions penetrating another object are sufficient to
	// create forces ejecting the device's collision representation).  Therefore, a device that is having its
	// translation scaled may required a force scaling filter to reduce the forces.
	Vector3d force;
	if (dataToFilter.vectors().get(DataStructures::Names::FORCE, &force))
	{
		force = m_transformInverse.linear() * force;
		result->vectors().set(DataStructures::Names::FORCE, force);
	}

	Vector3d torque;
	if (dataToFilter.vectors().get(DataStructures::Names::TORQUE, &torque))
	{
		torque = m_transformInverse.linear() * torque;
		result->vectors().set(DataStructures::Names::TORQUE, torque);
	}

	// The Jacobians must be transformed into device space.  The Jacobians are scaled based on the translation scaling,
	// so that the forces and torques displayed by the device will be correct for the scene-space motions, not for the
	// device-space motions.  Let R be the linear rotation portion of our transform, s be the translation scaling
	// factor, and J be the 3x3 upper-left corner of the spring Jacobian.  Then:
	// delta-translation-in-scene = s * R * delta-translation-in-device
	// delta-force-in-scene = J * delta-translation-in-scene
	// So to transform J into the device space, we left-multiply by R^-1, and right-multiply by R.
	// Then, because we want the forces to be scaled as in scene-space, we scale J by s.  The bottom-left 3x3 block
	// in springJacobian (or damperJacobian) also transforms from delta-translation and is treated the same, while the
	// two 3x3 blocks on the right half (of springJacobian or damperJacobian) will be multiplied by the delta-rotation
	// (not delta-translation) and so should not be scaled.
	DataGroup::DynamicMatrixType springJacobian;
	if (dataToFilter.matrices().get(DataStructures::Names::SPRING_JACOBIAN, &springJacobian))
	{
		springJacobian.block<3,3>(0, 0).applyOnTheLeft(m_transformInverse.linear());
		springJacobian.block<3,3>(0, 0).applyOnTheRight(m_transform.linear());
		springJacobian.block<3,3>(3, 0).applyOnTheLeft(m_transformInverse.linear());
		springJacobian.block<3,3>(3, 0).applyOnTheRight(m_transform.linear());
		springJacobian.block<3,3>(0, 3).applyOnTheLeft(m_transformInverse.linear());
		springJacobian.block<3,3>(0, 3).applyOnTheRight(m_transform.linear());
		springJacobian.block<3,3>(3, 3).applyOnTheLeft(m_transformInverse.linear());
		springJacobian.block<3,3>(3, 3).applyOnTheRight(m_transform.linear());
		springJacobian.block<6,3>(0, 0) *= m_translationScale;
		result->matrices().set(DataStructures::Names::SPRING_JACOBIAN, springJacobian);
	}

	RigidTransform3d inputPose;
	if (dataToFilter.poses().get(DataStructures::Names::INPUT_POSE, &inputPose))
	{
		inputPose = m_transformInverse * inputPose;
		inputPose.translation() /= m_translationScale;
		result->poses().set(DataStructures::Names::INPUT_POSE, inputPose);
	}

	DataGroup::DynamicMatrixType damperJacobian;
	if (dataToFilter.matrices().get(DataStructures::Names::DAMPER_JACOBIAN, &damperJacobian))
	{
		damperJacobian.block<3,3>(0, 0).applyOnTheLeft(m_transformInverse.linear());
		damperJacobian.block<3,3>(0, 0).applyOnTheRight(m_transform.linear());
		damperJacobian.block<3,3>(3, 0).applyOnTheLeft(m_transformInverse.linear());
		damperJacobian.block<3,3>(3, 0).applyOnTheRight(m_transform.linear());
		damperJacobian.block<3,3>(0, 3).applyOnTheLeft(m_transformInverse.linear());
		damperJacobian.block<3,3>(0, 3).applyOnTheRight(m_transform.linear());
		damperJacobian.block<3,3>(3, 3).applyOnTheLeft(m_transformInverse.linear());
		damperJacobian.block<3,3>(3, 3).applyOnTheRight(m_transform.linear());
		damperJacobian.block<6,3>(0, 0) *= m_translationScale;
		result->matrices().set(DataStructures::Names::DAMPER_JACOBIAN, damperJacobian);
	}

	Vector3d inputLinearVelocity;
	if (dataToFilter.vectors().get(DataStructures::Names::INPUT_LINEAR_VELOCITY, &inputLinearVelocity))
	{
		inputLinearVelocity = m_transformInverse.linear() * inputLinearVelocity;
		inputLinearVelocity /= m_translationScale;
		result->vectors().set(DataStructures::Names::INPUT_LINEAR_VELOCITY, inputLinearVelocity);
	}

	Vector3d inputAngularVelocity;
	if (dataToFilter.vectors().get(DataStructures::Names::INPUT_ANGULAR_VELOCITY, &inputAngularVelocity))
	{
		inputAngularVelocity = m_transformInverse.linear() * inputAngularVelocity;
		result->vectors().set(DataStructures::Names::INPUT_ANGULAR_VELOCITY, inputAngularVelocity);
	}
}

double PoseTransform::getTranslationScale() const
{
	return m_translationScale;
}

void PoseTransform::setTranslationScale(double translationScale)
{
	boost::lock_guard<boost::mutex> lock(m_mutex);
	m_translationScale = translationScale;
}

const RigidTransform3d& PoseTransform::getTransform() const
{
	return m_transform;
}

void PoseTransform::setTransform(const RigidTransform3d& transform)
{
	boost::lock_guard<boost::mutex> lock(m_mutex);
	m_transform = transform;
	m_transformInverse = m_transform.inverse();
}

};  // namespace Devices
};  // namespace SurgSim


/* test_thread_safe_arrays.cpp - Tests that Adept arrays are thread-safe

  Copyright (C) 2017 ECMWF

  Copying and distribution of this file, with or without modification,
  are permitted in any medium without royalty provided the copyright
  notice and this notice are preserved.  This file is offered as-is,
  without any warranty.

*/

#ifdef _OPENMP
#include <omp.h>
#endif

//#define ADEPT_STORAGE_THREAD_SAFE 1

#include <adept_arrays.h>

int main(int argc, const char** argv)
{
  using namespace adept;

  int N = 2;
  Matrix A(N,N);
  SymmMatrix S(N);

  Matrix B;
  SymmMatrix T;
#ifdef ADEPT_STORAGE_THREAD_SAFE
  std::cout << "Storage should be thread safe\n";
  // B shares the data and increases the reference counter of the
  // shared Storage object. If A goes out of scope, B will "steal" the
  // data.
  B >>= A;
  T >>= S;
#else
  std::cout << "Storage is not thread safe: using soft_link()\n";
  // B points to the data but does not have access to the Storage
  // object. If A goes out of scope, B will most likely point to an
  // inaccessible memory location.
  B >>= A.soft_link();
  T >>= S.soft_link();
#endif

  A = 1.0; // Also seen by B
  S = 2.0; // Also seen by S

  int nthreads = 1;

#ifdef _OPENMP
  nthreads = omp_get_max_threads();
  std::cout << omp_get_num_procs() << " processors available running maximum of "
	    << nthreads << " threads\n";
#else
  std::cout << "Compiled without OpenMP support: 1 thread\n";
#endif

  // The following almost always causes a crash if the code is not
  // properly thread safe
#pragma omp parallel for
  for (int i = 0; i < N*1000; ++i) {

    for (int j = 0; j < N*1000; ++j) {
      B[j % N] = noalias(B(__,j)) + T.diag_vector();
    }

  }

  if (nthreads > 1) {
    std::cout << "Parallel subsetting of array zillions of times was successful\n";
  }
  else {
    std::cout << "Serial subsetting of array zillions of times was successful (unsurprisingly)\n";
  }

  return 0;

}


#include <iostream>

#include <boost/program_options.hpp>

#include "functionality.hpp"
#include "functionality_eigen.hpp"

namespace po = boost::program_options;

int main() {

    std::cout << "Checkpoint 3" << std::endl;
    std::cout << "Hypergeometric probability: " << hypergeometricPmf(10'000, 4'270, 300, 128) << std::endl;
    std::cout << "Random matrix: " << std::endl;
    printRandomMatrix();

    // Use boost program options
    {
        po::options_description desc("Allowed options");
        desc.add_options()
                ("help", "produce help message")
                ("compression", po::value<int>(), "set compression level");

        po::variables_map vm;

        std::cout << desc << std::endl;
    }

    // int unused_variable = 0;

    return 0;
}


#pragma once
#ifndef CANNON_RAY_AABB_H
#define CANNON_RAY_AABB_H

/*!
 * \file cannon/ray/aabb.hpp
 * \brief File containing Aabb class definition.
 */

#include <cmath>

#include <Eigen/Dense>

#include <cannon/utils/class_forward.hpp>

using namespace Eigen;

namespace cannon {
  namespace ray {

    CANNON_CLASS_FORWARD(Ray);

    /*!
     * \brief Class representing an axis-aligned bounding box.
     */
    class Aabb {
      public:

        /*!
         * Default constructor.
         */
        Aabb() {}

        /*!
         * Constructor taking two points defining this Aabb component-wise.
         */
        Aabb(const Vector3d& a, const Vector3d& b) : minimum_(std::fmin(a.x(),
              b.x()), std::fmin(a.y(), b.y()), std::fmin(a.z(), b.z())),
        maximum_(std::fmax(a.x(), b.x()), std::fmax(a.y(), b.y()),
            std::fmax(a.z(), b.z())) {}

        /*!
         * Test whether a Ray hits this AABB.
         *
         * \param r The ray to test.
         * \param t_min Minimal distance along ray.
         * \param t_max Maximal distance along ray.
         *
         * \returns Whether the ray intersects this AABB between t_min and t_max.
         */
        bool hit(const Ray& r, double t_min, double t_max) const;

      public:
        Vector3d minimum_; //!< Minimal corner of this AABB.
        Vector3d maximum_; //!< Maximal corner of this AABB.

    };

    // Free functions

    /*!
     * Function to compute bounding box surrounding both input boxes.
     */
    Aabb surrounding_box(const Aabb& box_0, const Aabb& box_1);

    /*!
     * Function to compute bounding box surrounding input box and an additional point.
     */
    Aabb surrounding_box(const Aabb& box_0, const Vector3d& p);

  } // namespace ray
} // namespace cannon

#endif /* ifndef CANNON_RAY_AABB_H */


/*-----------------------------------------------------------------------------+
Copyright (c) 2010-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_CONTINUOUS_INTERVAL_HPP_JOFA_100327
#define BOOST_ICL_CONTINUOUS_INTERVAL_HPP_JOFA_100327

#include <functional>
#include <boost/static_assert.hpp>
#include <boost/concept/assert.hpp>
#include <boost/icl/detail/concept_check.hpp>
#include <boost/icl/concept/interval.hpp>
#include <boost/icl/concept/container.hpp>
#include <boost/icl/type_traits/value_size.hpp>
#include <boost/icl/type_traits/type_to_string.hpp>
#include <boost/icl/type_traits/is_continuous.hpp>
#include <boost/icl/type_traits/is_continuous_interval.hpp>
#include <boost/icl/interval_bounds.hpp>

namespace boost{namespace icl
{

template <class DomainT,
          ICL_COMPARE Compare = ICL_COMPARE_INSTANCE(ICL_COMPARE_DEFAULT, DomainT)>
class continuous_interval
{
public:
    typedef continuous_interval<DomainT,Compare> type;
    typedef DomainT domain_type;
    typedef ICL_COMPARE_DOMAIN(Compare,DomainT) domain_compare;
    typedef typename bounded_value<DomainT>::type bounded_domain_type;

public:
    //==========================================================================
    //= Construct, copy, destruct
    //==========================================================================
    /** Default constructor; yields an empty interval <tt>[0,0)</tt>. */
    continuous_interval()
        : _lwb(identity_element<DomainT>::value()), _upb(identity_element<DomainT>::value())
        , _bounds(interval_bounds::right_open())
    {
        BOOST_CONCEPT_ASSERT((DefaultConstructibleConcept<DomainT>));
        BOOST_CONCEPT_ASSERT((LessThanComparableConcept<DomainT>));
        BOOST_STATIC_ASSERT((icl::is_continuous<DomainT>::value));
    }

    //NOTE: Compiler generated copy constructor is used

    /** Constructor for a closed singleton interval <tt>[val,val]</tt> */
    explicit continuous_interval(const DomainT& val)
        : _lwb(val), _upb(val), _bounds(interval_bounds::closed())
    {
        BOOST_CONCEPT_ASSERT((DefaultConstructibleConcept<DomainT>));
        BOOST_CONCEPT_ASSERT((LessThanComparableConcept<DomainT>));
        BOOST_STATIC_ASSERT((icl::is_continuous<DomainT>::value));
    }

    /** Interval from <tt>low</tt> to <tt>up</tt> with bounds <tt>bounds</tt> */
    continuous_interval(const DomainT& low, const DomainT& up,
                      interval_bounds bounds = interval_bounds::right_open(),
                      continuous_interval* = 0)
        : _lwb(low), _upb(up), _bounds(bounds)
    {
        BOOST_CONCEPT_ASSERT((DefaultConstructibleConcept<DomainT>));
        BOOST_CONCEPT_ASSERT((LessThanComparableConcept<DomainT>));
        BOOST_STATIC_ASSERT((icl::is_continuous<DomainT>::value));
    }

    domain_type     lower()const { return _lwb; }
    domain_type     upper()const { return _upb; }
    interval_bounds bounds()const{ return _bounds; }

    static continuous_interval open     (const DomainT& lo, const DomainT& up){ return continuous_interval(lo, up, interval_bounds::open());      }
    static continuous_interval right_open(const DomainT& lo, const DomainT& up){ return continuous_interval(lo, up, interval_bounds::right_open());}
    static continuous_interval left_open (const DomainT& lo, const DomainT& up){ return continuous_interval(lo, up, interval_bounds::left_open()); }
    static continuous_interval closed   (const DomainT& lo, const DomainT& up){ return continuous_interval(lo, up, interval_bounds::closed());    }

private:
    domain_type     _lwb;
    domain_type     _upb;
    interval_bounds _bounds;
};

//==============================================================================
//=T continuous_interval -> concept interval
//==============================================================================
template<class DomainT, ICL_COMPARE Compare>
struct interval_traits< icl::continuous_interval<DomainT, Compare> >
{
    typedef interval_traits type;
    typedef DomainT domain_type;
    typedef ICL_COMPARE_DOMAIN(Compare,DomainT) domain_compare;
    typedef icl::continuous_interval<DomainT, Compare> interval_type;

    static interval_type construct(const domain_type& lo, const domain_type& up)
    {
        return interval_type(lo, up);
    }

    static domain_type lower(const interval_type& inter_val){ return inter_val.lower(); };
    static domain_type upper(const interval_type& inter_val){ return inter_val.upper(); };
};

//==============================================================================
//=T continuous_interval -> concept dynamic_interval
//==============================================================================
template<class DomainT, ICL_COMPARE Compare>
struct dynamic_interval_traits<boost::icl::continuous_interval<DomainT,Compare> >
{
    typedef dynamic_interval_traits type;
    typedef boost::icl::continuous_interval<DomainT,Compare> interval_type;
    typedef DomainT domain_type;
    typedef ICL_COMPARE_DOMAIN(Compare,DomainT) domain_compare;

    static interval_type construct(const domain_type lo, const domain_type up, interval_bounds bounds)
    {
        return icl::continuous_interval<DomainT,Compare>(lo, up, bounds,
            static_cast<icl::continuous_interval<DomainT,Compare>* >(0) );
    }

    static interval_type construct_bounded(const bounded_value<DomainT>& lo,
                                           const bounded_value<DomainT>& up)
    {
        return  icl::continuous_interval<DomainT,Compare>
                (
                    lo.value(), up.value(),
                    lo.bound().left() | up.bound().right(),
                    static_cast<icl::continuous_interval<DomainT,Compare>* >(0)
                );
    }
};

//==============================================================================
//= Type traits
//==============================================================================
template <class DomainT, ICL_COMPARE Compare>
struct interval_bound_type< continuous_interval<DomainT,Compare> >
{
    typedef interval_bound_type type;
    BOOST_STATIC_CONSTANT(bound_type, value = interval_bounds::dynamic);
};

template <class DomainT, ICL_COMPARE Compare>
struct is_continuous_interval<continuous_interval<DomainT,Compare> >
{
    typedef is_continuous_interval<continuous_interval<DomainT,Compare> > type;
    BOOST_STATIC_CONSTANT(bool, value = true);
};

template <class DomainT, ICL_COMPARE Compare>
struct type_to_string<icl::continuous_interval<DomainT,Compare> >
{
    static std::string apply()
    { return "cI<"+ type_to_string<DomainT>::apply() +">"; }
};

template<class DomainT>
struct value_size<icl::continuous_interval<DomainT> >
{
    static std::size_t apply(const icl::continuous_interval<DomainT>&)
    { return 2; }
};

}} // namespace icl boost

#endif


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/lexical_cast.hpp>
using namespace std;
int main() {
    long long int n; int k; cin >> n >> k;
    for (int i = 1; i <= k; i++) {
        string x = boost::lexical_cast<string>(n);
        sort(x.begin(), x.end());
        long long int a = boost::lexical_cast<long long int>(x);
        reverse(x.begin(), x.end());
        long long int b = boost::lexical_cast<long long int>(x);
        n = b - a;
    }
    cout << n << endl;
}


#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>

#include "cxxpznn1/core.hpp"

BOOST_AUTO_TEST_CASE(test_grid_001)
{
    unsigned int size = 4;

    int** a1 = new int*[size];
    for (int i = 0; i < size; i++)
    {
        a1[i] = new int[size];
    }

    int** a2 = new int*[size];
    for (int i = 0; i < size; i++)
    {
        a2[i] = new int[size];
    }

    for (int i = 0; i < size; i++)
    {
        for (int j = 0; j < size; j++)
        {
            a1[i][j] = size * i + j;
            a2[i][j] = size * i + j;
        }
    }

    cxxpznn1::Grid g1(size, a1);
    cxxpznn1::Grid g2(size, a2);

    BOOST_CHECK(g1 == g2);
}

BOOST_AUTO_TEST_CASE(test_grid_002)
{
    unsigned int s1 = 3;
    unsigned int s2 = 4;

    int** a1 = new int*[s1];
    for (int i = 0; i < s1; i++)
    {
        a1[i] = new int[s1];
    }

    int** a2 = new int*[s2];
    for (int i = 0; i < s2; i++)
    {
        a2[i] = new int[s2];
    }

    for (int i = 0; i < s1; i++)
    {
        for (int j = 0; j < s1; j++)
        {
            a1[i][j] = s1 * i + j;
        }
    }

    for (int i = 0; i < s2; i++)
    {
        for (int j = 0; j < s2; j++)
        {
            a2[i][j] = s2 * i + j;
        }
    }

    cxxpznn1::Grid g1(s1, a1);
    cxxpznn1::Grid g2(s2, a2);

    BOOST_CHECK(g1 != g2);
}

BOOST_AUTO_TEST_CASE(test_grid_003)
{
    unsigned int size = 4;

    int** a1 = new int*[size];
    for (int i = 0; i < size; i++)
    {
        a1[i] = new int[size];
    }

    int** a2 = new int*[size];
    for (int i = 0; i < size; i++)
    {
        a2[i] = new int[size];
    }

    for (int i = 0; i < size; i++)
    {
        for (int j = 0; j < size; j++)
        {
            a1[i][j] = size * 2 * i + j;
            a2[i][j] = size * 3 * i + j;
        }
    }

    cxxpznn1::Grid g1(size, a1);
    cxxpznn1::Grid g2(size, a2);

    BOOST_CHECK(g1 != g2);
}


#ifndef INTER_ANALYSIS_DATASTRUCTURES_HPP
#define INTER_ANALYSIS_DATASTRUCTURES_HPP

#include <boost/optional.hpp>
#include <boost/unordered_map.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/noncopyable.hpp>

#include <crab/common/types.hpp>
#include <crab/cfg/cfg.hpp>

/* Datastructures needed during inter-procedural analysis */

namespace crab {

  namespace analyzer {

    /* Store the calling contexts of each function */
    template <typename CFG, typename AbsDomain>
    class call_ctx_table: boost::noncopyable {
     public:

      typedef typename CFG::basic_block_t::callsite_t callsite_t;
      typedef typename CFG::fdecl_t fdecl_t;
      typedef typename CFG::varname_t varname_t;
      typedef AbsDomain abs_domain_t;

     private:
      typedef boost::unordered_map <std::size_t, AbsDomain> call_table_t;

      call_table_t m_call_table;

      // Assume context-insensitive analysis so it will merge all
      // calling contexts using abstract domain's join keeping a
      // single calling context per function.
      void insert_helper (std::size_t func_key, AbsDomain inv) {
        auto it = m_call_table.find (func_key);
        if (it != m_call_table.end ()) {
          //crab::CrabStats::count (AbsDomain::getDomainName() + ".count.join");
          //crab::ScopedCrabStats __st__(AbsDomain::getDomainName() + ".join");
          it->second = it->second | inv;
        }
        else
          m_call_table.insert (make_pair (func_key, inv));
      }

     public:

      call_ctx_table() { }

      void insert (callsite_t cs, AbsDomain inv) {
        insert_helper (cfg_hasher<CFG>::hash (cs), inv);
      }

      void insert (fdecl_t d, AbsDomain inv) {
        insert_helper (cfg_hasher<CFG>::hash (d), inv);
      }

      AbsDomain get_call_ctx (fdecl_t d) const {
        auto it = m_call_table.find (cfg_hasher<CFG>::hash (d));
        if (it != m_call_table.end ())
          return it->second;
        else
          return AbsDomain::top ();
      }

    };

    /* Store the summaries for each function*/
    template<typename CFG, typename AbsDomain>
    class summary_table: boost::noncopyable {

     public:

      typedef typename CFG::basic_block_t::callsite_t callsite_t;
      typedef typename CFG::fdecl_t fdecl_t;
      typedef AbsDomain abs_domain_t;
      typedef typename CFG::varname_t varname_t;

      // A summary is an input-output relationship between function
      // parameters. The relationship can be as expressive as
      // AbsDomain.
      class Summary {

        // --- function info
        fdecl_t m_fdecl;
        // --- Summary involving only m_params + m_out variables
        abs_domain_t m_sum;
        // --- Keep the returned value of the function if any
        boost::optional <varname_t> m_ret;
        // --- Keep all the formal parameters of the function. They can
        //     be INT_TYPE or PTR_TYPE inputs as well as input/output
        //     ARR_TYPE parameters.
        std::vector <varname_t> m_params;

       public:

        Summary (fdecl_t fdecl,
                 abs_domain_t sum,
                 boost::optional<varname_t> ret,
                 const vector<varname_t> &params):
            m_fdecl (fdecl),
            m_sum (sum),
            m_ret (ret),
            m_params(params)  { }

        fdecl_t get_fdecl () const { return m_fdecl; }

        abs_domain_t get_sum () const { return m_sum;}

        const vector<varname_t>& get_params () const { return m_params;}

        boost::optional<varname_t> get_ret_val () const { return m_ret;}

        void write(crab_os &o)  {
          o << m_fdecl << " --> " << " variables = {";
          for (auto const &p: m_params) {
            o << p << ";";
          }
          if (m_ret) o << *m_ret;
          o << "}" << " summary = " << m_sum;
        }
      };

     private:

      typedef boost::shared_ptr <Summary> summary_ptr;
      typedef boost::unordered_map <std::size_t, summary_ptr> summary_table_t;

      summary_table_t m_sum_table;

     public:

      summary_table () { }

      // insert summary information
      void insert (fdecl_t d,
                   AbsDomain sum,
                   boost::optional<varname_t> ret,
                   const std::vector<varname_t>& params) {

        std::vector<varname_t> ps (params.begin(), params.end ());
        summary_ptr sum_tuple (new Summary (d, sum, ret, ps));
        m_sum_table.insert (std::make_pair (cfg_hasher<CFG>::hash (d), sum_tuple));
      }

      // return true if there is a summary
      bool hasSummary (callsite_t cs) const {
        auto it = m_sum_table.find (cfg_hasher<CFG>::hash (cs));
        return (it != m_sum_table.end ());
      }

      bool hasSummary (fdecl_t d) const {
        auto it = m_sum_table.find (cfg_hasher<CFG>::hash (d));
        return (it != m_sum_table.end ());
      }

      // get the summary
      Summary& get (callsite_t cs) const {
        auto it = m_sum_table.find (cfg_hasher<CFG>::hash (cs));
        assert (it != m_sum_table.end ());

        return *(it->second);
      }

      Summary& get (fdecl_t d) const {
        auto it = m_sum_table.find (cfg_hasher<CFG>::hash (d));
        assert (it != m_sum_table.end ());

        return *(it->second);
      }

      void write (crab_os &o) const {
        o << "--- Begin summary table: \n";
        for (auto const &p: m_sum_table) {
          p.second->write (o);
          o << "\n";
        }
        o << "--- End summary table\n";
      }

      friend crab_os& operator<<(crab_os& o, const summary_table<CFG, AbsDomain>&t) {
        t.write (o);
        return o;
      }

    };

  } // end namespace
} // end namespace

#endif


/*!
@file
Defines `boost::hana::IntegralDomain`.

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

#ifndef BOOST_HANA_INTEGRAL_DOMAIN_HPP
#define BOOST_HANA_INTEGRAL_DOMAIN_HPP

#include <boost/hana/fwd/integral_domain.hpp>

#include <boost/hana/bool.hpp>
#include <boost/hana/constant.hpp>
#include <boost/hana/core/common.hpp>
#include <boost/hana/core/convert.hpp>
#include <boost/hana/core/datatype.hpp>
#include <boost/hana/core/default.hpp>
#include <boost/hana/core/models.hpp>
#include <boost/hana/core/operators.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/core/wrong.hpp>
#include <boost/hana/detail/has_common_embedding.hpp>
#include <boost/hana/detail/std/enable_if.hpp>
#include <boost/hana/detail/std/forward.hpp>
#include <boost/hana/detail/std/is_integral.hpp>

namespace boost { namespace hana {
    //////////////////////////////////////////////////////////////////////////
    // Operators
    //////////////////////////////////////////////////////////////////////////
    namespace operators {
        template <typename X, typename Y, typename = typename detail::std::enable_if<
            has_operator<datatype_t<X>, decltype(rem)>::value ||
            has_operator<datatype_t<Y>, decltype(rem)>::value
        >::type>
        constexpr decltype(auto) operator%(X&& x, Y&& y) {
            return hana::rem(detail::std::forward<X>(x),
                             detail::std::forward<Y>(y));
        }

        template <typename X, typename Y, typename = typename detail::std::enable_if<
            has_operator<datatype_t<X>, decltype(quot)>::value ||
            has_operator<datatype_t<Y>, decltype(quot)>::value
        >::type>
        constexpr decltype(auto) operator/(X&& x, Y&& y) {
            return hana::quot(detail::std::forward<X>(x),
                              detail::std::forward<Y>(y));
        }
    }

    //////////////////////////////////////////////////////////////////////////
    // quot
    //////////////////////////////////////////////////////////////////////////
    template <typename T, typename U, typename>
    struct quot_impl : quot_impl<T, U, when<true>> { };

    template <typename T, typename U, bool condition>
    struct quot_impl<T, U, when<condition>> : default_ {
        template <typename X, typename Y>
        static constexpr void apply(X&&, Y&&) {
            static_assert(wrong<quot_impl<T, U>, X, Y>{},
            "no definition of boost::hana::quot for the given data types");
        }
    };

    // Cross-type overload
    template <typename T, typename U>
    struct quot_impl<T, U, when<
        detail::has_nontrivial_common_embedding<IntegralDomain, T, U>{}
    >> {
        using C = typename common<T, U>::type;
        template <typename X, typename Y>
        static constexpr decltype(auto) apply(X&& x, Y&& y) {
            return hana::quot(to<C>(detail::std::forward<X>(x)),
                              to<C>(detail::std::forward<Y>(y)));
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // mod
    //////////////////////////////////////////////////////////////////////////
    template <typename T, typename U, typename>
    struct rem_impl : rem_impl<T, U, when<true>> { };

    template <typename T, typename U, bool condition>
    struct rem_impl<T, U, when<condition>> : default_ {
        template <typename X, typename Y>
        static constexpr void apply(X&&, Y&&) {
            static_assert(wrong<rem_impl<T, U>, X, Y>{},
            "no definition of boost::hana::rem for the given data types");
        }
    };

    // Cross-type overload
    template <typename T, typename U>
    struct rem_impl<T, U, when<
        detail::has_nontrivial_common_embedding<IntegralDomain, T, U>{}
    >> {
        using C = typename common<T, U>::type;
        template <typename X, typename Y>
        static constexpr decltype(auto) apply(X&& x, Y&& y) {
            return hana::rem(to<C>(detail::std::forward<X>(x)),
                             to<C>(detail::std::forward<Y>(y)));
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // models
    //////////////////////////////////////////////////////////////////////////
    template <typename D>
    struct models_impl<IntegralDomain, D>
        : _integral_constant<bool,
            !is_default<rem_impl<D, D>>{} &&
            !is_default<quot_impl<D, D>>{}
        >
    { };

    //////////////////////////////////////////////////////////////////////////
    // Model for integral data types
    //////////////////////////////////////////////////////////////////////////
    template <typename T>
    struct quot_impl<T, T, when<detail::std::is_non_boolean_integral<T>{}>> {
        template <typename X, typename Y>
        static constexpr decltype(auto) apply(X&& x, Y&& y)
        { return detail::std::forward<X>(x) / detail::std::forward<Y>(y); }
    };

    template <typename T>
    struct rem_impl<T, T, when<detail::std::is_non_boolean_integral<T>{}>> {
        template <typename X, typename Y>
        static constexpr decltype(auto) apply(X&& x, Y&& y)
        { return detail::std::forward<X>(x) % detail::std::forward<Y>(y); }
    };

    //////////////////////////////////////////////////////////////////////////
    // Model for Constants over an IntegralDomain
    //////////////////////////////////////////////////////////////////////////
    template <typename C>
    struct quot_impl<C, C, when<
        _models<Constant, C>{} && _models<IntegralDomain, typename C::value_type>{}
    >> {
        using T = typename C::value_type;
        template <typename X, typename Y>
        struct _constant {
            static constexpr decltype(auto) get() {
                return boost::hana::quot(boost::hana::value<X>(),
                                         boost::hana::value<Y>());
            }
            struct hana { using datatype = detail::CanonicalConstant<T>; };
        };
        template <typename X, typename Y>
        static constexpr decltype(auto) apply(X const&, Y const&)
        { return to<C>(_constant<X, Y>{}); }
    };

    template <typename C>
    struct rem_impl<C, C, when<
        _models<Constant, C>{} && _models<IntegralDomain, typename C::value_type>{}
    >> {
        using T = typename C::value_type;
        template <typename X, typename Y>
        struct _constant {
            static constexpr decltype(auto) get() {
                return boost::hana::rem(boost::hana::value<X>(),
                                        boost::hana::value<Y>());
            }
            struct hana { using datatype = detail::CanonicalConstant<T>; };
        };
        template <typename X, typename Y>
        static constexpr decltype(auto) apply(X const&, Y const&)
        { return to<C>(_constant<X, Y>{}); }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_INTEGRAL_DOMAIN_HPP


#include "gmp_factorial.h"

#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
using namespace boost::python;

BOOST_PYTHON_MODULE(my_booster) {
    def("factorial", fact);
}