#include <iostream>
#include "GUI/GlutGUI/GLApp.h"
#include "Framework/Framework/SceneGraph.h"
#include "Dynamics/EmbeddedMethod/EmbeddedFiniteElement.h"
#include "Dynamics/ParticleSystem/StaticBoundary.h"
#include "Dynamics/ParticleSystem/ElasticityModule.h"
#include "Dynamics/ParticleSystem/ParticleElasticBody.h"
#include "Rendering/SurfaceMeshRender.h"
#include "Rendering/PointRenderModule.h"
#include <boost/property_tree/json_parser.hpp>

#include "Framework/Topology/TriangleSet.h"

using namespace PhysIKA;
using namespace std;

int main()
{
    Log::sendMessage(Log::Info, "Simulation start");

    SceneGraph& scene = SceneGraph::getInstance();

    std::shared_ptr<StaticBoundary<DataType3f>> root = scene.createNewScene<StaticBoundary<DataType3f>>();
    root->loadCube(Vector3f(0), Vector3f(1), 0.005f, true);
    std::shared_ptr<EmbeddedFiniteElement<DataType3f>> bunny = std::make_shared<EmbeddedFiniteElement<DataType3f>>();

    root->addParticleSystem(bunny);

    //auto m_pointsRender = std::make_shared<PointRenderModule>();
    //m_pointsRender->setColor(Vector3f(0, 1, 1));
    //bunny->addVisualModule(m_pointsRender);

    bunny->setMass(1.0);

    //bunny->loadParticles("../../Media/bunny/bunny_points.obj");
    //bunny->loadSurface("../../Media/bunny/bunny_mesh.obj");

    /*const string particles_file = "../../Media/dragon/dragon_points_1190.obj";
    bunny->loadParticles(particles_file);
    bunny->loadSurface("../../Media/dragon/dragon.obj");*/

    const string particles_file = "../../Media/zju/armadillo/armadillo_points.obj";
    bunny->loadParticles(particles_file);
    bunny->loadSurface("../../Media/zju/armadillo/armadillo.obj");

    // bunny->scale(1.0 / 6);
    bunny->translate(Vector3f(0.5, 0.3, 0.5));
    bunny->setVisible(true);

    // Output all particles to .txt file.
    {
        auto                pSet   = TypeInfo::CastPointerDown<PointSet<DataType3f>>(bunny->getTopologyModule());
        auto&               points = pSet->getPoints();
        HostArray<Vector3f> hpoints(points.size());
        Function1Pt::copy(hpoints, points);

        std::ofstream outf("Particles.obj");
        if (outf.is_open())
        {
            for (int i = 0; i < hpoints.size(); ++i)
            {
                Vector3f curp = hpoints[i];
                outf << "v " << curp[0] << " " << curp[1] << " " << curp[2] << std::endl;
            }
            outf.close();

            std::cout << " Particle output:  FINISHED." << std::endl;
        }
    }

    auto sRender = std::make_shared<SurfaceMeshRender>();
    bunny->getSurfaceNode()->addVisualModule(sRender);
    sRender->setColor(Vector3f(1, 1, 0));

    // bunny->getElasticitySolver()->setIterationNumber(10);
    boost::property_tree::ptree pt;
    /* const std::string jsonfile_path = "../../Media/bunny/embedded_finite_element.json";*/
    /* const std::string jsonfile_path = "../../Media/dragon/embedded_finite_element.json";*/
    const std::string jsonfile_path = "../../Media/zju/armadillo/fem_hybrid.json";

    read_json(jsonfile_path, pt);

    bunny->init_problem_and_solver(pt);
    GLApp window;
    window.createWindow(1024, 768);

    window.mainLoop();

    return 0;
}


#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>
#include <cmath>
#include <cstdio>
#include <fstream>
#include <iostream>
#include "bte_config.h"
#include "aux/rdtsc_timer.hpp"
#include "collision_tensor/collision_tensor_galerkin.hpp"
#include "collision_tensor/dense/collision_tensor_zfirst.hpp"
#include "collision_tensor/dense/collision_tensor_zlast.hpp"
#include "collision_tensor/dense/collision_tensor_zlast_aligned.hpp"
#include "collision_tensor/dense/multi_slices_factory.hpp"
#include "collision_tensor/dense/storage/vbcrs_sparsity.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"

namespace po = boost::program_options;
namespace bf = boost::filesystem;

using namespace std;
using namespace boltzmann;

typedef ct_dense::CollisionTensorZLastAligned ct_dense_t;
typedef SpectralBasisFactoryKS basis_factory_t;
typedef SpectralBasisFactoryKS::basis_type basis_type;

int
main(int argc, char* argv[])
{
  // dummy call, internal storage of ct_dense is using MPI shmem, therefore:
  MPI_Init(&argc, &argv);

  std::string version_id = GIT_SHA1;
  cout << "VersionID: " << version_id << "@" << GIT_BNAME << std::endl;

  int nrep;
  if (argc < 2) {
    cerr << "usage: " << argv[0] << " nrep"
         << "\n";
    return 1;
  } else {
    nrep = atoi(argv[1]);
    cerr << "nrep: " << nrep << "\n";
  }

  // Load config
  if (!boost::filesystem::is_regular_file("config.yaml")) {
    cout << "config file not found\n";
    return 1;
  }
  YAML::Node config = YAML::LoadFile("config.yaml");
  const size_t K = config["SpectralBasis"]["deg"].as<size_t>();

  // create basis
  basis_type basis;
  SpectralBasisFactoryKS::create(basis, K);
  unsigned int n_velo_dofs = basis.n_dofs();

  ct_dense_t ct_dense(basis);
  ct_dense.import_entries_mpishmem(config["Scattering"]["file"].as<std::string>());

  int npadded = ct_dense.padded_vector_length();
  Eigen::VectorXd x(npadded);
  Eigen::VectorXd y(n_velo_dofs);
  x.setRandom();

  RDTSCTimer timer;

  for (int i = 0; i < nrep; ++i) {
    timer.start();
    ct_dense.apply(y, x);
    auto tlap = timer.stop();
    timer.print(cout, tlap, "dense");
  }

  MPI_Finalize();
  return 0;
}


#ifndef plane_calibration_SRC_PLANE_CALIBRATION_NODELET_HPP_
#define plane_calibration_SRC_PLANE_CALIBRATION_NODELET_HPP_

#include <atomic>
#include <mutex>
#include <memory>
#include <Eigen/Dense>

#include <nodelet/nodelet.h>
#include <dynamic_reconfigure/server.h>
#include <plane_calibration/PlaneCalibrationConfig.h>
#include <ros/time.h>
#include <ros/subscriber.h>
#include <ros/publisher.h>
#include <sensor_msgs/Image.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>

#include "camera_model.hpp"
#include "calibration_parameters.hpp"
#include "input_filter.hpp"
#include "plane_calibration.hpp"
#include "calibration_validation.hpp"
#include "plane_to_depth_image.hpp"
#include "depth_visualizer.hpp"

namespace plane_calibration
{

class PlaneCalibrationNodelet : public nodelet::Nodelet
{
public:
  PlaneCalibrationNodelet();

  virtual void onInit();

protected:
  virtual void reconfigureCB(PlaneCalibrationConfig &config, uint32_t level);
  virtual void cameraInfoCB(const sensor_msgs::CameraInfoConstPtr& camera_info_msg);
  virtual void depthImageCB(const sensor_msgs::ImageConstPtr& depth_image_msg);

  virtual void getTransform();
  virtual std::pair<Eigen::Vector3d, Eigen::AngleAxisd> getTransformManual();
  virtual std::pair<Eigen::Vector3d, Eigen::AngleAxisd> getTransformTF();

  virtual void runCalibration(Eigen::MatrixXf depth_matrix);
  virtual void publishTransform();

  std::atomic<bool> enable_;
  ros::Time last_call_time_;
  double calibration_rate_;

  bool precompute_planes_;
  int precomputed_plane_pairs_count_;
  CameraModelPtr camera_model_;
  CalibrationParametersPtr calibration_parameters_;

  tf2_ros::Buffer transform_listener_buffer_;
  tf2_ros::TransformListener transform_listener_;
  std::shared_ptr<std::pair<Eigen::Vector3d, Eigen::AngleAxisd>> transform_;

  InputFilterPtr input_filter_;
  PlaneCalibrationPtr plane_calibration_;
  CalibrationValidationPtr calibration_validation_;
  PlaneToDepthImagePtr plane_to_depth_converter_;

  std::atomic<double> max_deviation_;
  Eigen::Vector3d ground_plane_offset_;
  Eigen::AngleAxisd ground_plane_rotation_;
  std::atomic<int> iterations_;

  std::pair<double, double> last_valid_calibration_result_;
  Eigen::MatrixXf last_valid_calibration_result_plane_;
  Eigen::Affine3d last_valid_calibration_transformation_;

  InputFilter::Config input_filter_config_;
  CalibrationValidation::Config calibration_validation_config_;

  std::shared_ptr<DepthVisualizer> depth_visualizer_;
  std::shared_ptr<dynamic_reconfigure::Server<PlaneCalibrationConfig>> reconfigure_server_;

  std::atomic<bool> debug_;
  std::atomic<bool> use_manual_ground_transform_;
  std::atomic<bool> always_update_;
  std::atomic<double> x_offset_;
  std::atomic<double> y_offset_;
  std::atomic<double> z_offset_;

  std::atomic<double> px_offset_;
  std::atomic<double> py_offset_;
  std::atomic<double> pz_offset_;

  std::string ground_frame_;
  std::string camera_depth_frame_;
  std::string result_frame_;

  ros::Publisher pub_update_;
  ros::Subscriber sub_camera_info_;
  ros::Subscriber sub_depth_image_;
};

} /* end namespace */

#endif


/*
 boost/numeric/odeint/stepper/detail/adaptive_adams_coefficients.hpp

 [begin_description]
 Calculation of the coefficients for the adaptive adams stepper.
 [end_description]

 Copyright 2017 Valentin Noah Hartmann

 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_STEPPER_DETAIL_ADAPTIVE_ADAMS_COEFFICIENTS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAPTIVE_ADAMS_COEFFICIENTS_HPP_INCLUDED

#include <boost/numeric/odeint/stepper/detail/rotating_buffer.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>

#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/util/bind.hpp>

#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

#include <boost/array.hpp>

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

template<
size_t Steps,
class Deriv,
class Value = double,
class Time = double,
class Algebra = typename algebra_dispatcher< Deriv >::algebra_type,
class Operations = typename operations_dispatcher< Deriv >::operations_type,
class Resizer = initially_resizer
>
class adaptive_adams_coefficients
{
public:
    static const size_t steps = Steps;

    typedef unsigned short order_type;
    static const order_type order_value = steps;

    typedef Value value_type;
    typedef Deriv deriv_type;
    typedef Time time_type;

    typedef state_wrapper< deriv_type > wrapped_deriv_type;
    typedef rotating_buffer< time_type , steps+1 > time_storage_type;

    typedef Algebra algebra_type;
    typedef Operations operations_type;
    typedef Resizer resizer_type;

    typedef adaptive_adams_coefficients< Steps , Deriv , Value , Time , Algebra , Operations , Resizer > aac_type;

    adaptive_adams_coefficients( const algebra_type &algebra = algebra_type())
    :m_eo(1), m_steps_init(1), beta(), phi(), m_ns(0), m_time_storage(),
    m_algebra(algebra),
    m_phi_resizer()
    {
        for (size_t i=0; i<order_value+2; ++i)
        {
            c[i] = 1.0/(i+1);
            c[c_size+i] = 1.0/((i+1)*(i+2));
        }

        g[0] = c[0];
        g[1] = c[c_size];

        beta[0][0] = 1;
        beta[1][0] = 1;

        gs[0] = 1.0;
        gs[1] = -1.0/2;
        gs[2] = -1.0/12;
        gs[3] = -1.0/24;
        gs[4] = -19.0/720;
        gs[5] = -3.0/160;
        gs[6] = -863.0/60480;
        gs[7] = -275.0/24192;
        gs[8] = -33953.0/3628800;
        gs[9] = 35.0/4436;
        gs[10] =  40.0/5891;
        gs[11] = 37.0/6250;
        gs[12] = 25.0/4771;
        gs[13] = 40.0/8547;
    };

    void predict(time_type t, time_type dt)
    {
        using std::abs;

        m_time_storage[0] = t;

        if (abs(m_time_storage[0] - m_time_storage[1] - dt) > 1e-16 || m_eo >= m_ns)
        {
            m_ns = 0;
        }
        else if (m_ns < order_value + 2)
        {
            m_ns++;
        }

        for(size_t i=1+m_ns; i<m_eo+1 && i<m_steps_init; ++i)
        {
            time_type diff = m_time_storage[0] - m_time_storage[i];
            beta[0][i] = beta[0][i-1]*(m_time_storage[0] + dt - m_time_storage[i-1])/diff;
        }

        for(size_t i=2+m_ns; i<m_eo+2 && i<m_steps_init+1; ++i)
        {
            time_type diff = m_time_storage[0] + dt - m_time_storage[i-1];
            for(size_t j=0; j<m_eo+1-i+1; ++j)
            {
                c[c_size*i+j] = c[c_size*(i-1)+j] - c[c_size*(i-1)+j+1]*dt/diff;
            }

            g[i] = c[c_size*i];
        }
    };

    void do_step(const deriv_type &dxdt, const int o = 0)
    {
        m_phi_resizer.adjust_size( dxdt , detail::bind( &aac_type::template resize_phi_impl< deriv_type > , detail::ref( *this ) , detail::_1 ) );

        phi[o][0].m_v = dxdt;

        for(size_t i=1; i<m_eo+3 && i<m_steps_init+2 && i<order_value+2; ++i)
        {
            if (o == 0)
            {
                this->m_algebra.for_each3(phi[o][i].m_v, phi[o][i-1].m_v, phi[o+1][i-1].m_v,
                    typename Operations::template scale_sum2<value_type, value_type>(1.0, -beta[o][i-1]));
            }
            else
            {
                this->m_algebra.for_each2(phi[o][i].m_v, phi[o][i-1].m_v,
                    typename Operations::template scale_sum1<value_type>(1.0));
            }
        }
    };

    void confirm()
    {
        beta.rotate();
        phi.rotate();
        m_time_storage.rotate();

        if(m_steps_init < order_value+1)
        {
            ++m_steps_init;
        }
    };

    void reset() { m_eo = 1; m_steps_init = 1; };

    size_t m_eo;
    size_t m_steps_init;

    rotating_buffer<boost::array<value_type, order_value+1>, 2> beta; // beta[0] = beta(n)
    rotating_buffer<boost::array<wrapped_deriv_type, order_value+2>, 3> phi; // phi[0] = phi(n+1)
    boost::array<value_type, order_value + 2> g;
    boost::array<value_type, 14> gs;

private:
    template< class StateType >
    bool resize_phi_impl( const StateType &x )
    {
        bool resized( false );

        for(size_t i=0; i<(order_value + 2); ++i)
        {
            resized |= adjust_size_by_resizeability( phi[0][i], x, typename is_resizeable<deriv_type>::type() );
            resized |= adjust_size_by_resizeability( phi[1][i], x, typename is_resizeable<deriv_type>::type() );
            resized |= adjust_size_by_resizeability( phi[2][i], x, typename is_resizeable<deriv_type>::type() );
        }
        return resized;
    };

    size_t m_ns;

    time_storage_type m_time_storage;
    static const size_t c_size = order_value + 2;
    boost::array<value_type, c_size*c_size> c;

    algebra_type m_algebra;

    resizer_type m_phi_resizer;
};

} // detail
} // odeint
} // numeric
} // boost

#endif


////////////////////////////////////////////////////////////////////////////////
/// Copyright 2018-present Xinyan DAI<xinyan.dai@outlook.com>
///
/// permission is hereby granted, free of charge, to any person obtaining a copy
/// of this software and associated documentation files (the "Software"), to
/// deal in the Software without restriction, including without limitation the
/// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
/// sell copies of the Software, and to permit persons to whom the Software is
/// furnished to do so, subject to the following conditions:
///
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions ofthe Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
/// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
/// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
/// IN THE SOFTWARE.

/// @version 0.1
/// @author  Xinyan DAI
/// @contact xinyan.dai@outlook.com
//////////////////////////////////////////////////////////////////////////////

#pragma once

#include <utility>
#include <vector>
#include <random>
#include <unordered_map>
#include <boost/progress.hpp>

#include "kmeans.hpp"

#include "../utils/hashers.hpp"
#include "../index.hpp"

namespace ss {

    using std::vector;
    using std::unordered_map;
    using HashTableType = unordered_map< vector<int >, vector<int>, ss::SSHasher< vector<int > > >;

    template<class DataType>
    class PQIndex : public Index<DataType> {

    protected:
        vector<KMeansIndex<DataType > >  _K_means;
        vector<int >                     _dimensions;
        vector<int >                     _offsets;
        HashTableType                    _hash_map;

    public:
        explicit PQIndex(const parameter& para) :
                Index<DataType >(para),
                _K_means(para.num_codebook, KMeansIndex<DataType>(para)), _offsets(para.num_codebook) {

            int sub_dimension = this->_para.dim / this->_para.num_codebook;
            int reminder = this->_para.dim % this->_para.num_codebook;
            _dimensions.insert(_dimensions.end(), reminder, sub_dimension+1);
            _dimensions.insert(_dimensions.end(), this->_para.num_codebook - reminder, sub_dimension);

            _offsets[0] = 0;
            for (int i = 1; i < this->_para.num_codebook; ++i) {
                _offsets[i] = _offsets[i-1] + _dimensions[i-1];
            }
        }

        void Train(const Matrix<DataType > & data) override {

            for (int i=0; i<_K_means.size(); ++i) {
                /// train each code-book,
                /// use Visitor to represent data in each code-book
                _K_means[i].Iterate(Visitor<DataType >(data, _offsets[i], _dimensions[i]));
            }
        }

        void Add(const Matrix<DataType > & data) override {

            for (int i = 0; i < data.getSize(); ++i) {
                /// find each point's nearest center
                vector<int > cluster_id(this->_para.num_codebook, 0);
                /// find each point's nearest center in each code-book
                for (int code_book = 0; code_book < this->_para.num_codebook; ++code_book) {
                    cluster_id[code_book] =
                            _K_means[code_book].NearestCenter(data[i] + _offsets[code_book], _dimensions[code_book]);
                }
                /// add into centers
                _hash_map[cluster_id].push_back(i);
            }
        }

        HashTableType & hash_map() {
            return _hash_map;
        }

        /**
         * dist_to_centers[i][j] means the distance between query and j'th center in i'th code-book.
         * @return dist_to_centers
         */
        vector<vector<std::pair<DataType, int> > > DistToCenters(const DataType* query) {
            vector<vector<std::pair<DataType, int>   > > dist_to_centers(this->_para.num_codebook);
            for (int code_book = 0; code_book < this->_para.num_codebook ; ++code_book) {
                dist_to_centers[code_book] =
                        _K_means[code_book].ClusterDistance(query + _offsets[code_book], _dimensions[code_book]);
            }

            return dist_to_centers;
        }

    };

} // namespace ss


//==================================================================================================
/*!
  @file

  @copyright 2012-2015 NumScale SAS

  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_SIMD_CONSTANT_LIMITEXPONENT_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_LIMITEXPONENT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the limit exposant of floating point numbers,  that is Maxexponant+1

    @par Semantic:

    @code
    as_integer<T> r = Limitexponent<T>();
    @endcode

    @code
    if T is double
      r =  1024;
    else if T is float
      r =  128;
    @endcode

    @return The Limitexponent constant for the proper type
  **/
  template<typename T> T Limitexponent();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant limitexponent.

      @return The Limitexponent constant for the proper type
    **/
    const boost::dispatch::functor<tag::limitexponent_> limitexponent = {};
  }
} }
#endif

#include <boost/simd/constant/definition/limitexponent.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//
// 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)
//=======================================================================
/**
 * @file get_bound.hpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-10-04
 */
#ifndef PAAL_GET_BOUND_HPP
#define PAAL_GET_BOUND_HPP

#include "paal/utils/accumulate_functors.hpp"
#include "paal/utils/knapsack_utils.hpp"
#include "paal/greedy/knapsack/knapsack_greedy.hpp"

#include <boost/function_output_iterator.hpp>

namespace paal {
namespace detail {

template <typename SizeType, typename ValueType>
using GetIntegralTag = typename std::conditional<
    std::is_integral<SizeType>::value && std::is_integral<ValueType>::value,
    integral_value_and_size_tag,
    typename std::conditional<
        std::is_integral<SizeType>::value, integral_size_tag,
        typename std::conditional<
            std::is_integral<ValueType>::value, integral_value_tag,
            non_integral_value_and_size_tag>::type>::type>::type;

template <typename SizeType>
using Getarithmetic_size_tag = typename std::conditional<
    std::is_arithmetic<SizeType>::value, arithmetic_size_tag,
    Nonarithmetic_size_tag>::type;

// this overloads checks if SizeType and ValueType are integral

struct upper_tag{};
struct lower_tag{};

/**
 * @brief upper bound is computed as biggest density times capacity +
 *        values for all elements with size 0. It is correct upper bound for 0/1.
 *        For unbounded case there will be no elements with size 0.
 */
template <typename KnapsackData >
typename KnapsackData::value get_density_based_value_upper_bound(KnapsackData knap_data) {
    using Size      = typename KnapsackData::size;
    using ObjectRef = typename KnapsackData::object_ref;
    auto density = knap_data.get_density();

    // this filters are really needed only in 0/1 case
    // in unbounded case, there is a guarantee that sizes are not 0
    auto not_zero_sizel = [=](ObjectRef obj) {return knap_data.get_size(obj) > Size{};};
    auto not_zero_size = utils::make_assignable_functor(not_zero_sizel);
    auto zeroSize = utils::make_not_functor(not_zero_size);

    auto not_zeros = knap_data.get_objects() | boost::adaptors::filtered(not_zero_size);
    auto zeros     = knap_data.get_objects() | boost::adaptors::filtered(zeroSize     );

    auto maxElement = *max_element_functor(not_zeros, density);
    return knap_data.get_capacity() * maxElement + sum_functor(zeros, knap_data.get_value());
}

//non-arithmetic size, upper bound
template <typename KnapsackData,
          typename Is_0_1_Tag>
typename KnapsackData::value get_value_bound(
                      KnapsackData knap_data,
                      Nonarithmetic_size_tag, Is_0_1_Tag, upper_tag) {
    return get_density_based_value_upper_bound(std::move(knap_data));
}

//arithmetic size, upper bound
template <typename KnapsackData, typename Is_0_1_Tag>
typename KnapsackData::value get_value_bound(
                      KnapsackData knap_data,
                      arithmetic_size_tag, Is_0_1_Tag is_0_1_Tag, upper_tag) {
    return std::min(2 * get_value_bound(knap_data, is_0_1_Tag, lower_tag{}),
                    get_density_based_value_upper_bound(knap_data));
}

//non-arithmetic size, lower bound
template <typename KnapsackData, typename Is_0_1_Tag>
typename KnapsackData::value get_value_bound(KnapsackData knap_data,
                      Nonarithmetic_size_tag, Is_0_1_Tag, lower_tag) {
    //computes lower bound as value of the most valuable element
    return *max_element_functor(knap_data.get_objects(), knap_data.get_value()).base();
}

//arithmetic size, lower bound
template <typename KnapsackData, typename Is_0_1_Tag>
typename KnapsackData::value get_value_bound(KnapsackData knap_data,
                      arithmetic_size_tag, Is_0_1_Tag is_0_1_Tag, lower_tag) {
    auto out = boost::make_function_output_iterator(utils::skip_functor{});
    return knapsack_general_two_app(detail::make_knapsack_data(knap_data.get_objects(),
            knap_data.get_capacity(), knap_data.get_size(), knap_data.get_value(), out), is_0_1_Tag).first;
}

//decide whether size is arithmetic or not
template <typename KnapsackData, typename Is_0_1_Tag, typename BoundType>
typename KnapsackData::value get_value_bound(KnapsackData knap_data,
                      Is_0_1_Tag is_0_1_tag, BoundType bound_type_tag) {
    return get_value_bound(std::move(knap_data),
                                 Getarithmetic_size_tag<typename KnapsackData::size>{},
                                 is_0_1_tag, bound_type_tag);
}

} //! detail
} //! paal
#endif // PAAL_GET_BOUND_HPP


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2014 Erik Erlandson
//
// 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_UNITS_SYSTEMS_INFORMATION_PREFIXES_HPP_INCLUDED
#define BOOST_UNITS_SYSTEMS_INFORMATION_PREFIXES_HPP_INCLUDED

#include <boost/units/make_scaled_unit.hpp>
#include <boost/units/static_rational.hpp>
#include <boost/units/scale.hpp>

#include <boost/units/systems/information/byte.hpp>

#define BOOST_UNITS_INFOSYS_PREFIX(exponent, name) \
    typedef make_scaled_unit<dimensionless, scale<2, static_rational<exponent> > >::type name ## _pf_type; \
    BOOST_UNITS_STATIC_CONSTANT(name, name ## _pf_type)

namespace boost {
namespace units {
namespace information {

// Note, these are defined (somewhat arbitrarily) against the 'byte' system.
// They work smoothly with bit_information, nat_information, etc, so it is
// transparent to the user.
BOOST_UNITS_INFOSYS_PREFIX(10, kibi);
BOOST_UNITS_INFOSYS_PREFIX(20, mebi);
BOOST_UNITS_INFOSYS_PREFIX(30, gibi);
BOOST_UNITS_INFOSYS_PREFIX(40, tebi);
BOOST_UNITS_INFOSYS_PREFIX(50, pebi);
BOOST_UNITS_INFOSYS_PREFIX(60, exbi);
BOOST_UNITS_INFOSYS_PREFIX(70, zebi);
BOOST_UNITS_INFOSYS_PREFIX(80, yobi);

} // namespace information
} // namespace units
} // namespace boost

#undef BOOST_UNITS_INFOSYS_PREFIX

#endif // BOOST_UNITS_SYSTEMS_INFORMATION_PREFIXES_HPP_INCLUDED


#include <algorithm>
#include <cmath>
#include <climits>
#include <map>
#include <vector>
#include <iostream>

#include <Eigen/Dense>

#include "AprilTags/Edge.h"
#include "AprilTags/FloatImage.h"
#include "AprilTags/Gaussian.h"
#include "AprilTags/GrayModel.h"
#include "AprilTags/GLine2D.h"
#include "AprilTags/GLineSegment2D.h"
#include "AprilTags/Gridder.h"
#include "AprilTags/Homography33.h"
#include "AprilTags/MathUtil.h"
#include "AprilTags/Quad.h"
#include "AprilTags/Segment.h"
#include "AprilTags/TagFamily.h"
#include "AprilTags/UnionFindSimple.h"
#include "AprilTags/XYWeight.h"

#include "AprilTags/TagRectDetector.h"

//#define DEBUG_APRIL

#ifdef DEBUG_APRIL
#include <opencv/cv.h>
#include <opencv/highgui.h>
#endif

using namespace std;

namespace AprilTags {

  std::vector<TagDetection> TagRectDetector::extractTags(const cv::Mat& image, std::vector<cv::Rect>& quad_proposals) {

    // convert to internal AprilTags image (todo: slow, change internally to OpenCV)
    int width = image.cols;
    int height = image.rows;
    AprilTags::FloatImage fimOrig(width, height);
    int i = 0;
    for (int y=0; y<height; y++) {
      for (int x=0; x<width; x++) {
        fimOrig.set(x, y, image.data[i]/255.);
        i++;
      }
    }
    std::pair<int,int> opticalCenter(width/2, height/2);

#ifdef DEBUG_APRIL
#if 0
  { // debug - write
    int height_ = fimOrig.getHeight();
    int width_  = fimOrig.getWidth();
    cv::Mat image(height_, width_, CV_8UC3);
    {
      for (int y=0; y<height_; y++) {
        for (int x=0; x<width_; x++) {
          cv::Vec3b v;
          //        float vf = fimMag.get(x,y);
          float vf = fimOrig.get(x,y);
          int val = (int)(vf * 255.);
          if ((val & 0xffff00) != 0) {printf("problem... %i\n", val);}
          for (int k=0; k<3; k++) {
            v(k) = val;
          }
          image.at<cv::Vec3b>(y, x) = v;
        }
      }
    }
    imwrite("out.bmp", image);
  }
#endif
#if 0
  FloatImage fimOrig = fimOrig_;
  { // debug - read

    cv::Mat image = cv::imread("test.bmp");
    int height_ = fimOrig.getHeight();
    int width_  = fimOrig.getWidth();
    {
      for (int y=0; y<height_; y++) {
        for (int x=0; x<width_; x++) {
          cv::Vec3b v = image.at<cv::Vec3b>(y,x);
          float val = (float)v(0)/255.;
          fimOrig.set(x,y,val);
        }
      }
    }
  }
#endif
#endif

  //================================================================
  // Step one: preprocess image (convert to grayscale) and low pass if necessary

  FloatImage fim = fimOrig;

  //! Gaussian smoothing kernel applied to image (0 == no filter).
  /*! Used when sampling bits. Filtering is a good idea in cases
   * where A) a cheap camera is introducing artifical sharpening, B)
   * the bayer pattern is creating artifcats, C) the sensor is very
   * noisy and/or has hot/cold pixels. However, filtering makes it
   * harder to decode very small tags. Reasonable values are 0, or
   * [0.8, 1.5].
   */
  float sigma = 0;

  //! Gaussian smoothing kernel applied to image (0 == no filter).
  /*! Used when detecting the outline of the box. It is almost always
   * useful to have some filtering, since the loss of small details
   * won't hurt. Recommended value = 0.8. The case where sigma ==
   * segsigma has been optimized to avoid a redundant filter
   * operation.
   */
  float segSigma = 0.8f;

  if (sigma > 0) {
    int filtsz = ((int) max(3.0f, 3*sigma)) | 1;
    std::vector<float> filt = Gaussian::makeGaussianFilter(sigma, filtsz);
    fim.filterFactoredCentered(filt, filt);
  }

  //================================================================
  // Step two: Compute the local gradient. We store the direction and magnitude.
  // This step is quite sensitve to noise, since a few bad theta estimates will
  // break up segments, causing us to miss Quads. It is useful to do a Gaussian
  // low pass on this step even if we don't want it for encoding.

  FloatImage fimSeg;
  if (segSigma > 0) {
    if (segSigma == sigma) {
      fimSeg = fim;
    } else {
      // blur anew
      int filtsz = ((int) max(3.0f, 3*segSigma)) | 1;
      std::vector<float> filt = Gaussian::makeGaussianFilter(segSigma, filtsz);
      fimSeg = fimOrig;
      fimSeg.filterFactoredCentered(filt, filt);
    }
  } else {
    fimSeg = fimOrig;
  }

  FloatImage fimTheta(fimSeg.getWidth(), fimSeg.getHeight());
  FloatImage fimMag(fimSeg.getWidth(), fimSeg.getHeight());

  #pragma omp parallel for
  for (int y = 1; y < fimSeg.getHeight()-1; y++) {
    for (int x = 1; x < fimSeg.getWidth()-1; x++) {
      float Ix = fimSeg.get(x+1, y) - fimSeg.get(x-1, y);
      float Iy = fimSeg.get(x, y+1) - fimSeg.get(x, y-1);

      float mag = Ix*Ix + Iy*Iy;
#if 0 // kaess: fast version, but maybe less accurate?
      float theta = MathUtil::fast_atan2(Iy, Ix);
#else
      float theta = atan2(Iy, Ix);
#endif

      fimTheta.set(x, y, theta);
      fimMag.set(x, y, mag);
    }
  }

#ifdef DEBUG_APRIL
  int height_ = fimSeg.getHeight();
  int width_  = fimSeg.getWidth();
  cv::Mat image(height_, width_, CV_8UC3);
  {
    for (int y=0; y<height_; y++) {
      for (int x=0; x<width_; x++) {
        cv::Vec3b v;
        //        float vf = fimMag.get(x,y);
        float vf = fimOrig.get(x,y);
        int val = (int)(vf * 255.);
        if ((val & 0xffff00) != 0) {printf("problem... %i\n", val);}
        for (int k=0; k<3; k++) {
          v(k) = val;
        }
        image.at<cv::Vec3b>(y, x) = v;
      }
    }
  }
#endif

  //================================================================
  // Step three. Extract edges by grouping pixels with similar
  // thetas together. This is a greedy algorithm: we start with
  // the most similar pixels.  We use 4-connectivity.
  UnionFindSimple uf(fimSeg.getWidth()*fimSeg.getHeight());

  vector<Edge> edges(width*height*4);
  size_t nEdges = 0;

  // Bounds on the thetas assigned to this group. Note that because
  // theta is periodic, these are defined such that the average
  // value is contained *within* the interval.
  { // limit scope of storage
    /* Previously all this was on the stack, but this is 1.2MB for 320x240 images
     * That's already a problem for OS X (default 512KB thread stack size),
     * could be a problem elsewhere for bigger images... so store on heap */
    vector<float> storage(width*height*4);  // do all the memory in one big block, exception safe
    float * tmin = &storage[width*height*0];
    float * tmax = &storage[width*height*1];
    float * mmin = &storage[width*height*2];
    float * mmax = &storage[width*height*3];

    for (int y = 0; y+1 < height; y++) {
      for (int x = 0; x+1 < width; x++) {

        float mag0 = fimMag.get(x,y);
        if (mag0 < Edge::minMag)
          continue;
        mmax[y*width+x] = mag0;
        mmin[y*width+x] = mag0;

        float theta0 = fimTheta.get(x,y);
        tmin[y*width+x] = theta0;
        tmax[y*width+x] = theta0;

        // Calculates then adds edges to 'vector<Edge> edges'
        Edge::calcEdges(theta0, x, y, fimTheta, fimMag, edges, nEdges);

        // XXX Would 8 connectivity help for rotated tags?
        // Probably not much, so long as input filtering hasn't been disabled.
      }
    }

    edges.resize(nEdges);
    std::stable_sort(edges.begin(), edges.end());
    Edge::mergeEdges(edges,uf,tmin,tmax,mmin,mmax);
  }

  //================================================================
  // Step four: Loop over the pixels again, collecting statistics for each cluster.
  // We will soon fit lines (segments) to these points.

  map<int, vector<XYWeight> > clusters;
  for (int y = 0; y+1 < fimSeg.getHeight(); y++) {
    for (int x = 0; x+1 < fimSeg.getWidth(); x++) {
      if (uf.getSetSize(y*fimSeg.getWidth()+x) < Segment::minimumSegmentSize)
	continue;

      int rep = (int) uf.getRepresentative(y*fimSeg.getWidth()+x);

      map<int, vector<XYWeight> >::iterator it = clusters.find(rep);
      if ( it == clusters.end() ) {
	clusters[rep] = vector<XYWeight>();
	it = clusters.find(rep);
      }
      vector<XYWeight> &points = it->second;
      points.push_back(XYWeight(x,y,fimMag.get(x,y)));
    }
  }

  //================================================================
  // Step five: Loop over the clusters, fitting lines (which we call Segments).
  std::vector<Segment> segments; //used in Step six
  std::map<int, std::vector<XYWeight> >::const_iterator clustersItr;
  for (clustersItr = clusters.begin(); clustersItr != clusters.end(); clustersItr++) {
    std::vector<XYWeight> points = clustersItr->second;
    GLineSegment2D gseg = GLineSegment2D::lsqFitXYW(points);

    // filter short lines
    float length = MathUtil::distance2D(gseg.getP0(), gseg.getP1());
    if (length < Segment::minimumLineLength)
      continue;

    Segment seg;
    float dy = gseg.getP1().second - gseg.getP0().second;
    float dx = gseg.getP1().first - gseg.getP0().first;

    float tmpTheta = std::atan2(dy,dx);

    seg.setTheta(tmpTheta);
    seg.setLength(length);

    // We add an extra semantic to segments: the vector
    // p1->p2 will have dark on the left, white on the right.
    // To do this, we'll look at every gradient and each one
    // will vote for which way they think the gradient should
    // go. This is way more retentive than necessary: we
    // could probably sample just one point!

    float flip = 0, noflip = 0;
    for (unsigned int i = 0; i < points.size(); i++) {
      XYWeight xyw = points[i];

      float theta = fimTheta.get((int) xyw.x, (int) xyw.y);
      float mag = fimMag.get((int) xyw.x, (int) xyw.y);

      // err *should* be +M_PI/2 for the correct winding, but if we
      // got the wrong winding, it'll be around -M_PI/2.
      float err = MathUtil::mod2pi(theta - seg.getTheta());

      if (err < 0)
	noflip += mag;
      else
	flip += mag;
    }

    if (flip > noflip) {
      float temp = seg.getTheta() + (float)M_PI;
      seg.setTheta(temp);
    }

    float dot = dx*std::cos(seg.getTheta()) + dy*std::sin(seg.getTheta());
    if (dot > 0) {
      seg.setX0(gseg.getP1().first); seg.setY0(gseg.getP1().second);
      seg.setX1(gseg.getP0().first); seg.setY1(gseg.getP0().second);
    }
    else {
      seg.setX0(gseg.getP0().first); seg.setY0(gseg.getP0().second);
      seg.setX1(gseg.getP1().first); seg.setY1(gseg.getP1().second);
    }

    segments.push_back(seg);
  }

#ifdef DEBUG_APRIL
#if 0
  {
    for (vector<Segment>::iterator it = segments.begin(); it!=segments.end(); it++) {
      long int r = random();
      cv::line(image,
               cv::Point2f(it->getX0(), it->getY0()),
               cv::Point2f(it->getX1(), it->getY1()),
               cv::Scalar(r%0xff,(r%0xff00)>>8,(r%0xff0000)>>16,0) );
    }
  }
#endif
#endif

  // Step six: For each segment, find segments that begin where this segment ends.
  // (We will chain segments together next...) The gridder accelerates the search by
  // building (essentially) a 2D hash table.
  Gridder<Segment> gridder(0,0,width,height,10);

  // add every segment to the hash table according to the position of the segment's
  // first point. Remember that the first point has a specific meaning due to our
  // left-hand rule above.
  for (unsigned int i = 0; i < segments.size(); i++) {
    gridder.add(segments[i].getX0(), segments[i].getY0(), &segments[i]);
  }

  // Now, find child segments that begin where each parent segment ends.
  for (unsigned i = 0; i < segments.size(); i++) {
    Segment &parentseg = segments[i];

    //compute length of the line segment
    GLine2D parentLine(std::pair<float,float>(parentseg.getX0(), parentseg.getY0()),
		       std::pair<float,float>(parentseg.getX1(), parentseg.getY1()));

    Gridder<Segment>::iterator iter = gridder.find(parentseg.getX1(), parentseg.getY1(), 0.5f*parentseg.getLength());
    while(iter.hasNext()) {
      Segment &child = iter.next();
      if (MathUtil::mod2pi(child.getTheta() - parentseg.getTheta()) > 0) {
	continue;
      }

      // compute intersection of points
      GLine2D childLine(std::pair<float,float>(child.getX0(), child.getY0()),
			std::pair<float,float>(child.getX1(), child.getY1()));

      std::pair<float,float> p = parentLine.intersectionWith(childLine);
      if (p.first == -1) {
	continue;
      }

      float parentDist = MathUtil::distance2D(p, std::pair<float,float>(parentseg.getX1(),parentseg.getY1()));
      float childDist = MathUtil::distance2D(p, std::pair<float,float>(child.getX0(),child.getY0()));

      if (max(parentDist,childDist) > parentseg.getLength()) {
	// cout << "intersection too far" << endl;
	continue;
      }

      // everything's OK, this child is a reasonable successor.
      parentseg.children.push_back(&child);
    }
  }

  //================================================================
  // Step seven: Search all connected segments to see if any form a loop of length 4.
  // Add those to the quads list.
  vector<Quad> quads;

  vector<Segment*> tmp(5);
  for (unsigned int i = 0; i < segments.size(); i++) {
    tmp[0] = &segments[i];
    Quad::search(fimOrig, tmp, segments[i], 0, quads, opticalCenter);
  }

  // add to quad_proposals
  std::vector<cv::Point> vertices;

  for (unsigned int qi = 0; qi < quads.size(); qi++ ) {
      Quad &quad = quads[qi];
      std::pair<float, float> p1 = quad.quadPoints[0];
      std::pair<float, float> p2 = quad.quadPoints[1];
      std::pair<float, float> p3 = quad.quadPoints[2];
      std::pair<float, float> p4 = quad.quadPoints[3];
	  //std::cout << p1 << std::endl;

	  vertices.push_back(cv::Point(p1.first, p1.second));
      vertices.push_back(cv::Point(p2.first, p2.second));
      vertices.push_back(cv::Point(p3.first, p3.second));
      vertices.push_back(cv::Point(p4.first, p4.second));
      cv::Rect r = boundingRect(cv::Mat(vertices));
	  //std::cout << r << std::endl;

	  quad_proposals.push_back(r);
  }

#ifdef DEBUG_APRIL
  {
    for (unsigned int qi = 0; qi < quads.size(); qi++ ) {
      Quad &quad = quads[qi];
      std::pair<float, float> p1 = quad.quadPoints[0];
      std::pair<float, float> p2 = quad.quadPoints[1];
      std::pair<float, float> p3 = quad.quadPoints[2];
      std::pair<float, float> p4 = quad.quadPoints[3];
      cv::line(image, cv::Point2f(p1.first, p1.second), cv::Point2f(p2.first, p2.second), cv::Scalar(0,0,255,0) );
      cv::line(image, cv::Point2f(p2.first, p2.second), cv::Point2f(p3.first, p3.second), cv::Scalar(0,0,255,0) );
      cv::line(image, cv::Point2f(p3.first, p3.second), cv::Point2f(p4.first, p4.second), cv::Scalar(0,0,255,0) );
      cv::line(image, cv::Point2f(p4.first, p4.second), cv::Point2f(p1.first, p1.second), cv::Scalar(0,0,255,0) );

      p1 = quad.interpolate(-1,-1);
      p2 = quad.interpolate(-1,1);
      p3 = quad.interpolate(1,1);
      p4 = quad.interpolate(1,-1);
      cv::circle(image, cv::Point2f(p1.first, p1.second), 3, cv::Scalar(0,255,0,0), 1);
      cv::circle(image, cv::Point2f(p2.first, p2.second), 3, cv::Scalar(0,255,0,0), 1);
      cv::circle(image, cv::Point2f(p3.first, p3.second), 3, cv::Scalar(0,255,0,0), 1);
      cv::circle(image, cv::Point2f(p4.first, p4.second), 3, cv::Scalar(0,255,0,0), 1);
    }
    cv::imshow("debug_april", image);
  }
#endif

  //================================================================
  // Step eight. Decode the quads. For each quad, we first estimate a
  // threshold color to decide between 0 and 1. Then, we read off the
  // bits and see if they make sense.

  std::vector<TagDetection> detections;

  for (unsigned int qi = 0; qi < quads.size(); qi++ ) {
    Quad &quad = quads[qi];

    // Find a threshold
    GrayModel blackModel, whiteModel;
    const int dd = 2 * thisTagFamily.blackBorder + thisTagFamily.dimension;

    for (int iy = -1; iy <= dd; iy++) {
      float y = (iy + 0.5f) / dd;
      for (int ix = -1; ix <= dd; ix++) {
	float x = (ix + 0.5f) / dd;
	std::pair<float,float> pxy = quad.interpolate01(x, y);
	int irx = (int) (pxy.first + 0.5);
	int iry = (int) (pxy.second + 0.5);
	if (irx < 0 || irx >= width || iry < 0 || iry >= height)
	  continue;
	float v = fim.get(irx, iry);
	if (iy == -1 || iy == dd || ix == -1 || ix == dd)
	  whiteModel.addObservation(x, y, v);
	else if (iy == 0 || iy == (dd-1) || ix == 0 || ix == (dd-1))
	  blackModel.addObservation(x, y, v);
      }
    }

    bool bad = false;
    unsigned long long tagCode = 0;
    for ( int iy = thisTagFamily.dimension-1; iy >= 0; iy-- ) {
      float y = (thisTagFamily.blackBorder + iy + 0.5f) / dd;
      for (int ix = 0; ix < thisTagFamily.dimension; ix++ ) {
	float x = (thisTagFamily.blackBorder + ix + 0.5f) / dd;
	std::pair<float,float> pxy = quad.interpolate01(x, y);
	int irx = (int) (pxy.first + 0.5);
	int iry = (int) (pxy.second + 0.5);
	if (irx < 0 || irx >= width || iry < 0 || iry >= height) {
	  // cout << "*** bad:  irx=" << irx << "  iry=" << iry << endl;
	  bad = true;
	  continue;
	}
	float threshold = (blackModel.interpolate(x,y) + whiteModel.interpolate(x,y)) * 0.5f;
	float v = fim.get(irx, iry);
	tagCode = tagCode << 1;
	if ( v > threshold)
	  tagCode |= 1;
#ifdef DEBUG_APRIL
        {
          if (v>threshold)
            cv::circle(image, cv::Point2f(irx, iry), 1, cv::Scalar(0,0,255,0), 2);
          else
            cv::circle(image, cv::Point2f(irx, iry), 1, cv::Scalar(0,255,0,0), 2);
        }
#endif
      }
    }

    if ( !bad ) {
      TagDetection thisTagDetection;
      thisTagFamily.decode(thisTagDetection, tagCode);

      // compute the homography (and rotate it appropriately)
      thisTagDetection.homography = quad.homography.getH();
      thisTagDetection.hxy = quad.homography.getCXY();

      float c = std::cos(thisTagDetection.rotation*(float)M_PI/2);
      float s = std::sin(thisTagDetection.rotation*(float)M_PI/2);
      Eigen::Matrix3d R;
      R.setZero();
      R(0,0) = R(1,1) = c;
      R(0,1) = -s;
      R(1,0) = s;
      R(2,2) = 1;
      Eigen::Matrix3d tmp;
      tmp = thisTagDetection.homography * R;
      thisTagDetection.homography = tmp;

      // Rotate points in detection according to decoded
      // orientation.  Thus the order of the points in the
      // detection object can be used to determine the
      // orientation of the target.
      std::pair<float,float> bottomLeft = thisTagDetection.interpolate(-1,-1);
      int bestRot = -1;
      float bestDist = FLT_MAX;
      for ( int i=0; i<4; i++ ) {
	float const dist = AprilTags::MathUtil::distance2D(bottomLeft, quad.quadPoints[i]);
	if ( dist < bestDist ) {
	  bestDist = dist;
	  bestRot = i;
	}
      }

      for (int i=0; i< 4; i++)
	thisTagDetection.p[i] = quad.quadPoints[(i+bestRot) % 4];

      if (thisTagDetection.good) {
	thisTagDetection.cxy = quad.interpolate01(0.5f, 0.5f);
	thisTagDetection.observedPerimeter = quad.observedPerimeter;
	detections.push_back(thisTagDetection);
      }
    }
  }

#ifdef DEBUG_APRIL
  {
    cv::imshow("debug_april", image);
  }
#endif

  //================================================================
  //Step nine: Some quads may be detected more than once, due to
  //partial occlusion and our aggressive attempts to recover from
  //broken lines. When two quads (with the same id) overlap, we will
  //keep the one with the lowest error, and if the error is the same,
  //the one with the greatest observed perimeter.

  std::vector<TagDetection> goodDetections;

  // NOTE: allow multiple non-overlapping detections of the same target.

  for ( vector<TagDetection>::const_iterator it = detections.begin();
	it != detections.end(); it++ ) {
    const TagDetection &thisTagDetection = *it;

    bool newFeature = true;

    for ( unsigned int odidx = 0; odidx < goodDetections.size(); odidx++) {
      TagDetection &otherTagDetection = goodDetections[odidx];

      if ( thisTagDetection.id != otherTagDetection.id ||
	   ! thisTagDetection.overlapsTooMuch(otherTagDetection) )
	continue;

      // There's a conflict.  We must pick one to keep.
      newFeature = false;

      // This detection is worse than the previous one... just don't use it.
      if ( thisTagDetection.hammingDistance > otherTagDetection.hammingDistance )
	continue;

      // Otherwise, keep the new one if it either has strictly *lower* error, or greater perimeter.
      if ( thisTagDetection.hammingDistance < otherTagDetection.hammingDistance ||
	   thisTagDetection.observedPerimeter > otherTagDetection.observedPerimeter )
	goodDetections[odidx] = thisTagDetection;
    }

     if ( newFeature )
       goodDetections.push_back(thisTagDetection);

  }

  //cout << "AprilTags: edges=" << nEdges << " clusters=" << clusters.size() << " segments=" << segments.size()
  //     << " quads=" << quads.size() << " detections=" << detections.size() << " unique tags=" << goodDetections.size() << endl;

  return goodDetections;
  }

  std::vector<TagDetection> TagRectDetector::extractTags(const cv::Mat& image) {

  std::vector<cv::Rect> quad_proposals;
  return this->extractTags(image, quad_proposals);

}

} // namespace


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <limits>
#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>
#include <boost/lambda/lambda.hpp>

#include "tudat/interface/spice/spiceInterface.h"
#include "tudat/simulation/estimation_setup/createCartesianStatePartials.h"
#include "tudat/astro/orbit_determination/observation_partials/rotationMatrixPartial.h"
#include "tudat/astro/reference_frames/referenceFrameTransformations.h"
#include "tudat/io/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat::ephemerides;
using namespace tudat::estimatable_parameters;
using namespace tudat::observation_partials;

BOOST_AUTO_TEST_SUITE( test_rotation_matrix_partaisl )

//! Test whether partial derivatives of rotation matrix computed by SimpleRotationalEphemeris works correctly
BOOST_AUTO_TEST_CASE( testSimpleRotationalEphemerisPartials )
{
    // Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Create rotation model
    double nominalRotationRate = 2.0 * mathematical_constants::PI / 86400.0;
    std::shared_ptr< SimpleRotationalEphemeris > rotationalEphemeris =
            std::make_shared< SimpleRotationalEphemeris >(
                spice_interface::computeRotationQuaternionBetweenFrames( "ECLIPJ2000", "IAU_Earth", 1.0E7 ),
                nominalRotationRate, 1.0E7, "ECLIPJ2000", "IAU_Earth" );

    {
        // Create partial object.
        std::shared_ptr< RotationMatrixPartialWrtConstantRotationRate > rotationMatrixPartialObject =
                std::make_shared< RotationMatrixPartialWrtConstantRotationRate >( rotationalEphemeris );

        // Compute partial analytically
        double testTime = 1.0E6;
        Eigen::Matrix3d rotationMatrixPartial =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixToBaseFrameWrParameter(
                    testTime ).at( 0 );

        Eigen::Matrix3d rotationMatrixDerivativePartial =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixDerivativeToBaseFrameWrParameter(
                    testTime ).at( 0 );

        // Compute partial numerically.
        double perturbation = 1.0E-12;
        rotationalEphemeris->resetRotationRate( nominalRotationRate + perturbation );
        Eigen::Matrix3d upperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame(
                    testTime).toRotationMatrix( );
        Eigen::Matrix3d upperturbedRotationMatrixDerivative = rotationalEphemeris->getDerivativeOfRotationToBaseFrame(
                    testTime );

        rotationalEphemeris->resetRotationRate( nominalRotationRate - perturbation );
        Eigen::Matrix3d downperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame(
                    testTime).toRotationMatrix( );
        Eigen::Matrix3d downperturbedRotationMatrixDerivative = rotationalEphemeris->getDerivativeOfRotationToBaseFrame(
                    testTime );

        Eigen::Matrix3d numericalRotationMatrixPartial =
                ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );
        Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                ( upperturbedRotationMatrixDerivative - downperturbedRotationMatrixDerivative ) / ( 2.0 * perturbation );

        Eigen::Matrix3d matrixDifference = rotationMatrixPartial - numericalRotationMatrixPartial;

        // Compare analytical and numerical result.
        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 0.1 );
            }
        }

        matrixDifference = rotationMatrixDerivativePartial - numericalRotationMatrixDerivativePartial;

        // Compare analytical and numerical result.
        for( unsigned int i = 0; i < 3; i++ )
        {
            for( unsigned int j = 0; j < 3; j++ )
            {
                BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-5 );
            }
        }
    }

    {
        // Create partial object.
        std::shared_ptr< RotationMatrixPartialWrtPoleOrientation > rotationMatrixPartialObject =
                std::make_shared< RotationMatrixPartialWrtPoleOrientation >( rotationalEphemeris );

        // Compute partial analytically
        double testTime = 1.0E6;
        std::vector< Eigen::Matrix3d > rotationMatrixPartials =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixToBaseFrameWrParameter(
                    testTime );

        std::vector< Eigen::Matrix3d > rotationMatrixDerivativePartials =
                rotationMatrixPartialObject->calculatePartialOfRotationMatrixDerivativeToBaseFrameWrParameter(
                    testTime );

        Eigen::Vector3d nominalEulerAngles = rotationalEphemeris->getInitialEulerAngles( );
        double perturbedAngle;

        // Compute partial numerically.
        double perturbation = 1.0E-6;
        {

            // Compute partial for right ascension numerically.
            {
                perturbedAngle = nominalEulerAngles( 0 ) + perturbation;
                rotationalEphemeris->resetInitialPoleRightAscensionAndDeclination( perturbedAngle, nominalEulerAngles( 1 ) );
                Eigen::Matrix3d upperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame(
                            testTime).toRotationMatrix( );
                Eigen::Matrix3d upperturbedRotationMatrixDerivative = rotationalEphemeris->getDerivativeOfRotationToBaseFrame(
                            testTime );

                perturbedAngle = nominalEulerAngles( 0 ) - perturbation;
                rotationalEphemeris->resetInitialPoleRightAscensionAndDeclination( perturbedAngle, nominalEulerAngles( 1 ) );
                Eigen::Matrix3d downperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame(
                            testTime).toRotationMatrix( );
                Eigen::Matrix3d downperturbedRotationMatrixDerivative =
                        rotationalEphemeris->getDerivativeOfRotationToBaseFrame(
                            testTime );

                Eigen::Matrix3d numericalRotationMatrixPartial =
                        ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );
                Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                        ( upperturbedRotationMatrixDerivative - downperturbedRotationMatrixDerivative ) /
                        ( 2.0 * perturbation );

                Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 0 ) - numericalRotationMatrixPartial;

                // Compare analytical and numerical result.
                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-8 );
                    }
                }

                matrixDifference = rotationMatrixDerivativePartials.at( 0 ) - numericalRotationMatrixDerivativePartial;

                // Compare analytical and numerical result.
                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-13 );
                    }
                }
            }

            // Compute partial for declination numerically.
            {
                perturbedAngle = nominalEulerAngles( 1 ) + perturbation;
                rotationalEphemeris->resetInitialPoleRightAscensionAndDeclination( nominalEulerAngles( 0 ), perturbedAngle );
                Eigen::Matrix3d upperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame(
                            testTime).toRotationMatrix( );
                Eigen::Matrix3d upperturbedRotationMatrixDerivative = rotationalEphemeris->getDerivativeOfRotationToBaseFrame(
                            testTime );

                perturbedAngle = nominalEulerAngles( 1 ) - perturbation;
                rotationalEphemeris->resetInitialPoleRightAscensionAndDeclination( nominalEulerAngles( 0 ), perturbedAngle );
                Eigen::Matrix3d downperturbedRotationMatrix = rotationalEphemeris->getRotationToBaseFrame(
                            testTime).toRotationMatrix( );
                Eigen::Matrix3d downperturbedRotationMatrixDerivative =
                        rotationalEphemeris->getDerivativeOfRotationToBaseFrame( testTime );

                Eigen::Matrix3d numericalRotationMatrixPartial =
                        ( upperturbedRotationMatrix - downperturbedRotationMatrix ) / ( 2.0 * perturbation );
                Eigen::Matrix3d numericalRotationMatrixDerivativePartial =
                        ( upperturbedRotationMatrixDerivative -
                          downperturbedRotationMatrixDerivative ) / ( 2.0 * perturbation );

                Eigen::Matrix3d matrixDifference = rotationMatrixPartials.at( 1 ) - numericalRotationMatrixPartial;

                // Compare analytical and numerical result.
                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-8 );
                    }
                }

                matrixDifference = rotationMatrixDerivativePartials.at( 1 ) - numericalRotationMatrixDerivativePartial;

                // Compare analytical and numerical result.
                for( unsigned int i = 0; i < 3; i++ )
                {
                    for( unsigned int j = 0; j < 3; j++ )
                    {
                        BOOST_CHECK_SMALL( std::fabs( matrixDifference( i, j ) ), 1.0E-13 );
                    }
                }
            }
        }
    }
}

//! Test whether partial derivatives of rotation matrix computed by SynchronousRotationalEphemeris works correctly
BOOST_AUTO_TEST_CASE( testSynchronousRotationPartials )
{
    // Define nominal state
    Eigen::Vector6d nominalState =
            tudat::spice_interface::getBodyCartesianStateAtEpoch(
                                       "Mercury", "SSB", "ECLIPJ2000", "None", 1.0E7 );

    // Define nominal state function
    Eigen::Vector6d currentState = nominalState;
    std::function< Eigen::Vector6d( const double, bool ) > relativeStateFunction =
            [ & ]( const double, bool ){ return currentState; };

    // Create rotation model
    std::shared_ptr< tudat::ephemerides::SynchronousRotationalEphemeris > synchronousRotationModel =
            std::make_shared< ephemerides::SynchronousRotationalEphemeris >(
                relativeStateFunction, "SSB", "Mercury_Fixed", "ECLIPJ2000" );

    // Create rotation partial model
    std::shared_ptr< RotationMatrixPartial > rotationMatrixPartialObject =
            std::make_shared< SynchronousRotationMatrixPartialWrtTranslationalState >( synchronousRotationModel );

    // Define test settings
    double testTime = 1.0E7;
    double positionPerturbation = 10000.0;
    double velocityPerturbation = 0.1;

    // Test partials w.r.t. position and velocity components
    std::vector< Eigen::Matrix3d > rotationMatrixPartials =
            rotationMatrixPartialObject->calculatePartialOfRotationMatrixToBaseFrameWrParameter( testTime );
    for( int i = 0; i < 3; i++ )
    {
        currentState = nominalState;
        currentState( i ) += positionPerturbation;
        Eigen::Matrix3d upPerturbedRotationMatrix = synchronousRotationModel->getRotationToBaseFrame(
                    1.0E7 ).toRotationMatrix( );

        currentState = nominalState;
        currentState( i ) -= positionPerturbation;
        Eigen::Matrix3d downPerturbedRotationMatrix = synchronousRotationModel->getRotationToBaseFrame(
                    1.0E7 ).toRotationMatrix( );

        Eigen::Matrix3d relativePartialError =
                ( ( upPerturbedRotationMatrix - downPerturbedRotationMatrix ) / ( 2.0 * positionPerturbation ) -
                rotationMatrixPartials.at( i ) ) / rotationMatrixPartials.at( i ).norm( );

        for( int j = 0; j < 3; j++ )
        {
            for( int k = 0; k < 3; k++ )
            {
                BOOST_CHECK_SMALL( std::fabs( relativePartialError( j, k ) ), 1.0E-8 );
            }
        }

        currentState = nominalState;
        currentState( i + 3 ) += velocityPerturbation;
        upPerturbedRotationMatrix = synchronousRotationModel->getRotationToBaseFrame(
                    1.0E7 ).toRotationMatrix( );

        currentState = nominalState;
        currentState( i + 3 ) -= velocityPerturbation;
        downPerturbedRotationMatrix = synchronousRotationModel->getRotationToBaseFrame(
                    1.0E7 ).toRotationMatrix( );

        relativePartialError =
                        ( ( upPerturbedRotationMatrix - downPerturbedRotationMatrix ) / ( 2.0 * velocityPerturbation ) -
                        rotationMatrixPartials.at( i + 3 ) ) / rotationMatrixPartials.at( i + 3 ).norm( );

        for( int j = 0; j < 3; j++ )
        {
            for( int k = 0; k < 3; k++ )
            {
                BOOST_CHECK_SMALL( std::fabs( relativePartialError( j, k ) ), 1.0E-8 );
            }
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


#pragma once

// deal.II includes --------------------------------------------------------
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/fe/fe_q.h>

// own includes ------------------------------------------------------------
#include "abstract_method.hpp"
#include "enum/enum.hpp"
#include "spectral/basis/spectral_basis.hpp"
#include "spectral/basis/spectral_elem.hpp"
#include "var_form/least_squares/least_squares.hpp"
#include "var_form/least_squares/least_squares_rhs.hpp"

namespace boltzmann {

// short cut for least squares method
class MLSMethod : public AbstractMethod<LeastSquaresVarForm,
                                        RhsVarForm,
                                        dealii::FE_Q<2>,
                                        SpectralBasis<SpectralElem<double, XiR, LaguerreKS> > >
{
  const static std::string info;
};
const std::string MLSMethod::info = "MLS";

template <enum METHOD>
class Method
{
};

template <>
class Method<METHOD::MODLEASTSQUARES> : public MLSMethod
{
 public:
  const static enum METHOD lsq_type;
};

const METHOD Method<METHOD::MODLEASTSQUARES>::lsq_type = METHOD::MODLEASTSQUARES;

}  // end namespace boltzmann


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>
#include <algorithm>

#include <GL/gl.h>

#include <vtkActor.h>
#include <vtkCamera.h>
#include <vtkgl.h>
#include <vtkInteractorStyleSwitch.h>
#include <vtkMapper.h>
#include <vtkObjectFactory.h>
#include <vtkOpenGLExtensionManager.h>
#include <vtkOpenGLRenderWindow.h>
#include <vtkProperty.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkSmartPointer.h>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/iota.hpp>
#include <boost/compute/interop/opengl.hpp>
#include <boost/compute/interop/vtk.hpp>
#include <boost/compute/utility/dim.hpp>
#include <boost/compute/utility/source.hpp>

namespace compute = boost::compute;

// tesselates a sphere with radius, phi_slices, and theta_slices. returns
// a shared opencl/opengl buffer containing the vertex data.
compute::opengl_buffer tesselate_sphere(float radius,
                                        size_t phi_slices,
                                        size_t theta_slices,
                                        compute::command_queue &queue)
{
    using compute::dim;

    const compute::context &context = queue.get_context();

    const size_t vertex_count = phi_slices * theta_slices;

    // create opengl buffer
    GLuint vbo;
    vtkgl::GenBuffersARB(1, &vbo);
    vtkgl::BindBufferARB(vtkgl::ARRAY_BUFFER, vbo);
    vtkgl::BufferDataARB(vtkgl::ARRAY_BUFFER,
                         sizeof(float) * 4 * vertex_count,
                         NULL,
                         vtkgl::STREAM_DRAW);
    vtkgl::BindBufferARB(vtkgl::ARRAY_BUFFER, 0);

    // create shared opengl/opencl buffer
    compute::opengl_buffer vertex_buffer(context, vbo);

    // tesselate_sphere kernel source
    const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE(
        __kernel void tesselate_sphere(float radius,
                                       uint phi_slices,
                                       uint theta_slices,
                                       __global float4 *vertex_buffer)
        {
            const uint phi_i = get_global_id(0);
            const uint theta_i = get_global_id(1);

            const float phi = phi_i * 2.f * M_PI_F / phi_slices;
            const float theta = theta_i * 2.f * M_PI_F / theta_slices;

            float4 v;
            v.x = radius * cos(theta) * cos(phi);
            v.y = radius * cos(theta) * sin(phi);
            v.z = radius * sin(theta);
            v.w = 1.f;

            vertex_buffer[phi_i*phi_slices+theta_i] = v;
        }
    );

    // build tesselate_sphere program
    compute::program program =
        compute::program::create_with_source(source, context);
    program.build();

    // setup tesselate_sphere kernel
    compute::kernel kernel(program, "tesselate_sphere");
    kernel.set_arg<compute::float_>(0, radius);
    kernel.set_arg<compute::uint_>(1, phi_slices);
    kernel.set_arg<compute::uint_>(2, theta_slices);
    kernel.set_arg(3, vertex_buffer);

    // acqurire buffer so that it is accessible to OpenCL
    compute::opengl_enqueue_acquire_buffer(vertex_buffer, queue);

    // execute tesselate_sphere kernel
    queue.enqueue_nd_range_kernel(
        kernel, dim(0, 0), dim(phi_slices, theta_slices), dim(1, 1)
    );

    // release buffer so that it is accessible to OpenGL
    compute::opengl_enqueue_release_buffer(vertex_buffer, queue);

    return vertex_buffer;
}

// simple vtkMapper subclass to render the tesselated sphere on the gpu.
class gpu_sphere_mapper : public vtkMapper
{
public:
    vtkTypeMacro(gpu_sphere_mapper, vtkMapper)

    static gpu_sphere_mapper* New()
    {
        return new gpu_sphere_mapper;
    }

    void Render(vtkRenderer *renderer, vtkActor *actor)
    {
        if(!m_initialized){
            Initialize(renderer, actor);
            m_initialized = true;
        }

        if(!m_tesselated){
            m_vertex_count = m_phi_slices * m_theta_slices;

            // tesselate sphere
            m_vertex_buffer = tesselate_sphere(
                m_radius, m_phi_slices, m_theta_slices, m_command_queue
            );

            // ensure tesselation is finished (seems to be required on AMD)
            m_command_queue.finish();

            // set tesselated flag to true
            m_tesselated = true;
        }

        // draw sphere
        glEnableClientState(GL_VERTEX_ARRAY);
        vtkgl::BindBufferARB(vtkgl::ARRAY_BUFFER, m_vertex_buffer.get_opengl_object());
        glVertexPointer(4, GL_FLOAT, sizeof(float)*4, 0);
        glDrawArrays(GL_POINTS, 0, m_vertex_count);
    }

    void Initialize(vtkRenderer *renderer, vtkActor *actor)
    {
        // initialize opengl extensions
        vtkOpenGLExtensionManager *extensions =
            static_cast<vtkOpenGLRenderWindow *>(renderer->GetRenderWindow())
                ->GetExtensionManager();
        extensions->LoadExtension("GL_ARB_vertex_buffer_object");

        // initialize opencl/opengl shared context
        m_context = compute::opengl_create_shared_context();
        compute::device device = m_context.get_device();
        std::cout << "device: " << device.name() << std::endl;

        // create command queue for the gpu device
        m_command_queue = compute::command_queue(m_context, device);
    }

    double* GetBounds()
    {
        static double bounds[6];
        bounds[0] = -m_radius; bounds[1] = m_radius;
        bounds[2] = -m_radius; bounds[3] = m_radius;
        bounds[4] = -m_radius; bounds[5] = m_radius;
        return bounds;
    }

protected:
    gpu_sphere_mapper()
    {
        m_radius = 5.0f;
        m_phi_slices = 100;
        m_theta_slices = 100;
        m_initialized = false;
        m_tesselated = false;
    }

private:
    float m_radius;
    int m_phi_slices;
    int m_theta_slices;
    int m_vertex_count;
    bool m_initialized;
    bool m_tesselated;
    compute::context m_context;
    compute::command_queue m_command_queue;
    compute::opengl_buffer m_vertex_buffer;
};

int main(int argc, char *argv[])
{
    // create gpu sphere mapper
    vtkSmartPointer<gpu_sphere_mapper> mapper =
        vtkSmartPointer<gpu_sphere_mapper>::New();

    // create actor for gpu sphere mapper
    vtkSmartPointer<vtkActor> actor =
        vtkSmartPointer<vtkActor>::New();
    actor->GetProperty()->LightingOff();
    actor->GetProperty()->SetInterpolationToFlat();
    actor->SetMapper(mapper);

    // create render window
    vtkSmartPointer<vtkRenderer> renderer =
        vtkSmartPointer<vtkRenderer>::New();
    renderer->SetBackground(.1, .2, .31);
    vtkSmartPointer<vtkRenderWindow> renderWindow =
        vtkSmartPointer<vtkRenderWindow>::New();
    renderWindow->SetSize(800, 600);
    renderWindow->AddRenderer(renderer);
    vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =
        vtkSmartPointer<vtkRenderWindowInteractor>::New();
    vtkInteractorStyleSwitch *interactorStyle =
        vtkInteractorStyleSwitch::SafeDownCast(
            renderWindowInteractor->GetInteractorStyle()
        );
    interactorStyle->SetCurrentStyleToTrackballCamera();
    renderWindowInteractor->SetRenderWindow(renderWindow);
    renderer->AddActor(actor);

    // render
    renderer->ResetCamera();
    vtkCamera *camera = renderer->GetActiveCamera();
    camera->Elevation(-90.0);
    renderWindowInteractor->Initialize();
    renderWindow->Render();
    renderWindowInteractor->Start();

    return 0;
}


/* Copyright (C) 2012,2013 IBM Corp.
 * This program 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 2 of the License, or
 * (at your option) any later version.
 *
 * This program 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.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */
// g++ -o cgauss -I/usr/local/include cgauss.cpp -L/usr/local/lib -lntl -lm

// So this confirms that E(s^k)/H^k = k!

#include <NTL/ZZ.h>

#include <complex>

NTL_CLIENT

//evaluate f at e^{2 pi i/m}, returning a complex number

complex<double> evalPoly(double *f, long i, long m)
{
  complex<double> t(0.0, 2*M_PI*i/((double) m));
  complex<double> x = exp(t);

  complex<double> res = 0.0;
  for (long j = m-1; j >= 0; j--)
    res = res*x + f[j];

  return res;
}

int main()
{
   long m = 1001;
   long h = 100;

   long maxpow = 7;
   long iter = 100000;

   double *s = new double[m];
   complex<double> *pow = new complex<double> [m];
   double *sum = new double[maxpow+1];

   for (long k = 1; k <= maxpow; k++) sum[k] = 0;

   complex<double> t(0.0, 2*M_PI*1/((double) m));
   complex<double> x = exp(t);
   complex<double> xi = 1.0;
   for (long i = 0; i < m; i++) {
      pow[i] = xi;
      xi *= x;
   }

   for (long u = 0; u < iter; u++) {
      complex<double> v = 0.0;
      for (long i = 0; i < m; i++) {
         if (RandomBnd(m) < h) { // true w/ prob. h/m
            if (RandomBnd(2) == 0)
               v += pow[i];
            else
               v -= pow[i];
         }
      }

      double nv = norm(v);
      double nvk = nv;

      for (long k = 1; k <= maxpow; k++) {
         sum[k] += nvk;
         nvk *= nv;
      }
   }

   for (long k = 1; k <= maxpow; k++) {
      double ave = sum[k]/iter;
      cout << ave/exp(k*log(h)) << "\n";
   }
}


/**
 * @file main.cpp
 * @brief Main file in this project.
 *
 * @author Maxim <john.jasper.doe@gmail.com>
 * @date 2020
 */

/* See the license in the file "LICENSE.txt" in the root directory. */

#include "core/clusterization.hpp"
#include <boost/program_options.hpp>
#include <iostream>

/** @brief Anonymous namespace. */
namespace {

/** @brief Input data struct. */
struct param {
  std::size_t k_means{0};
};

using param_t = param;

/** @brief Get option on cmd line. */
void get_param(const int argc, const char* const argv[], param_t& param) {
  namespace po = boost::program_options;

  // clang-format off
  po::options_description desc("Options: ");
  desc.add_options()
      ("help,h", "this help")
      ("kmeans,k", po::value<std::size_t>(), "number of clusters for k-means algorithm");
  // clang-format on

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
  }
  catch (boost::program_options::error& err) {
    throw std::invalid_argument(err.what());
  }

  if (vm.count("help") || vm.count("h")) {
    std::cout << desc << "\n";
    exit(0);
  }

  if (vm.count("kmeans"))
    param.k_means = vm["kmeans"].as<std::size_t>();
  else if (vm.count("k"))
    param.k_means = vm["k"].as<std::size_t>();
  else
    throw std::invalid_argument("Number of clusters was not set");
}

/**
 * @brief Reading data from an input stream.
 *
 * @details
 * Data is received from the input stream, then read line by line, parsed and
 * formed into a data vector.
 *
 * @param [in] istrm - the input stream where the data comes from.
 * @return Data vector ready.
 */
kkmeans::core::datas_t read_from(std::istream& istrm) {
  std::vector<kkmeans::core::sample_t> res;

  while (istrm) {
    std::string line;
    istrm >> line;

    if (!line.empty()) {
      kkmeans::core::sample_t dot;
      std::size_t sep_pos = line.find(';');
      dot(0) = std::stod(line.substr(0, sep_pos));
      dot(1) = std::stod(line.substr(sep_pos + 1, line.size() - sep_pos - 1));
      res.push_back(std::move(dot));
    }
  }

  return res;
}

} /* :: */

/** @brief Main entry point */
int main(int argc, const char* argv[]) {
  param_t prm;

  try {
    get_param(argc, argv, prm);
  }
  catch (const std::invalid_argument& e) {
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
  }

  kkmeans::core::datas_t input_data = read_from(std::cin);
  kkmeans::core::clusterize clust(std::move(input_data));
  kkmeans::core::clust_datas_t output_data = clust.exec(prm.k_means);

  for (const auto& data : output_data) {
    std::cout << data.x << ';' << data.y << ';' << data.clust << '\n';
  }

  return EXIT_SUCCESS;
}


/* Copyright (c) 2014-2016, Stefan.Eilemann@epfl.ch
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * - Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 * - Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 * - Neither the name of Eyescale Software GmbH nor the names of its
 *   contributors may be used to endorse or promote products derived from this
 *   software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <vmmlib/frustum.hpp>
#include <vmmlib/frustumCuller.hpp>
#include <vmmlib/types.hpp>

#define BOOST_TEST_MODULE frustum
#include <boost/test/unit_test.hpp>

static void _testCull(const vmml::FrustumCullerf& fc)
{
    const vmml::vector<4, float> sphereIn(0.f, 0.f, -10.f, 1.f);
    const vmml::vector<4, float> sphereOut(0.f, 0.f, 0.f, .5f);
    const vmml::vector<4, float> sphereBorder(0.f, 0.f, -1.f, 1.f);

    BOOST_CHECK_EQUAL(fc.test(sphereIn), vmml::VISIBILITY_FULL);
    BOOST_CHECK_EQUAL(fc.test(sphereOut), vmml::VISIBILITY_NONE);
    BOOST_CHECK_EQUAL(fc.test(sphereBorder), vmml::VISIBILITY_PARTIAL);

    std::stringstream str;
    str << vmml::VISIBILITY_PARTIAL;
    BOOST_CHECK_EQUAL(str.str(), "partially visible");
    str.str("");
    str << vmml::VISIBILITY_FULL;
    BOOST_CHECK_EQUAL(str.str(), "fully visible    ");

    vmml::AABBf aabbIn(vmml::Vector3f(-1.f, -1.f, -4.f),
                       vmml::Vector3f(1.f, 1.f, -2.f));
    vmml::AABBf aabbOut(vmml::Vector3f(-1.f, -1.f, -.5f),
                        vmml::Vector3f(1.f, 1.f, 0.f));
    vmml::AABBf aabbBorder(vmml::Vector3f(-1.f, -1.f, -1.5f),
                           vmml::Vector3f(1.f, 1.f, -.5f));

    BOOST_CHECK_EQUAL(fc.test(aabbIn), vmml::VISIBILITY_FULL);
    BOOST_CHECK_EQUAL(fc.test(aabbOut), vmml::VISIBILITY_NONE);
    BOOST_CHECK_EQUAL(fc.test(aabbBorder), vmml::VISIBILITY_PARTIAL);
}

BOOST_AUTO_TEST_CASE(convert)
{
    const vmml::Frustumf f1;
    const vmml::Frustumf f2(f1.computePerspectiveMatrix());
    BOOST_CHECK_MESSAGE(f1.equals(f2, 0.0001f), f2);
}

BOOST_AUTO_TEST_CASE(base)
{
    const vmml::Frustumf frustum(-1.f, 1., -1.f, 1., 1.f, 100.f);
    const vmml::Matrix<4, 4, float> mvp = frustum.computePerspectiveMatrix();

    const vmml::FrustumCullerf fc(mvp);
    _testCull(fc);

    //   e_____f
    //  /     /|
    // | a b | |
    // | c d |/h
    //  -----
    const vmml::Vector3f a(-1.f, 1.f, -1.f);
    const vmml::Vector3f b(1.f, 1.f, -1.f);
    const vmml::Vector3f c(-1.f, -1.f, -1.f);
    const vmml::Vector3f d(1.f, -1.f, -1.f);
    const vmml::Vector3f e(-100.f, 100.f, -100.f);
    const vmml::Vector3f f(100.f, 100.f, -100.f);
    const vmml::Vector3f g(-100.f, -100.f, -100.f);
    const vmml::Vector3f h(100.f, -100.f, -100.f);

    const vmml::FrustumCullerf fc2(a, b, c, d, e, f, g, h);
    _testCull(fc2);
}

BOOST_AUTO_TEST_CASE(fovy_to_projection)
{
    const vmml::Frustumf frustum(45.f, 4.f / 3.f, 0.01f, 10.f);
    const auto projection = frustum.computePerspectiveMatrix();

    // same as glm::perspective(M_PI_4, 4.f/3., 0.01, 10.)
    BOOST_CHECK_CLOSE(projection(0, 0), -1.81066f, 0.0001f);
    BOOST_CHECK_CLOSE(projection(1, 1), -2.414214f, 0.0001f);
    BOOST_CHECK_CLOSE(projection(2, 2), -1.002002f, 0.0001f);
    BOOST_CHECK_CLOSE(projection(2, 3), -0.02002002f, 0.0001f);
    BOOST_CHECK_CLOSE(projection(3, 2), -1.f, 0.0001f);
}


#include <mesh_array.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(MESH_ARRAY);

BOOST_AUTO_TEST_CASE(triangle_adjacency)
{
  typedef mesh_array::T3Mesh                           mesh_type;
  typedef mesh_array::VertexRing< mesh_type >          vertex_ring_type;
  typedef mesh_array::AdjacencyInfo<mesh_type>         adjacency_type;

  // Empty mesh case
  {
    mesh_type M;

    vertex_ring_type R( M );

    adjacency_type A(R);

    // BOOST_CHECK_EQUAL( A.i(0) , mesh_array::UNASSIGNED());   // Hmmm really not defined!
  }

  // Small mesh case
  {
    mesh_type M;

    M.set_capacity(4,2);

    /**

     0
     *
     / \
     1 *---* 2
     \ /
     *
     3
     */

    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();

    M.push_triangle( 0, 1, 2);
    M.push_triangle( 1, 3, 2);

    vertex_ring_type R( M );

    adjacency_type  A(R);

    BOOST_CHECK_EQUAL( A.i(0), 1 );
    BOOST_CHECK_EQUAL( A.j(0), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.k(0), mesh_array::UNASSIGNED() );

    BOOST_CHECK_EQUAL( A.i(1), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.j(1), 0 );
    BOOST_CHECK_EQUAL( A.k(1), mesh_array::UNASSIGNED() );
  }

  // Non empty mesh case
  {
    mesh_type M;
    M.set_capacity(9,7);

    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();

    M.push_triangle( 0, 6, 3);
    M.push_triangle( 0, 3, 1);
    M.push_triangle( 1, 3, 4);
    M.push_triangle( 1, 4, 2);
    M.push_triangle( 3, 6, 7);
    M.push_triangle( 3, 7, 4);
    M.push_triangle( 4, 7, 8);

    vertex_ring_type R( M );
    adjacency_type  A(R);

    // 0 6 3
    BOOST_CHECK_EQUAL( A.i(0), 4 );
    BOOST_CHECK_EQUAL( A.j(0), 1 );
    BOOST_CHECK_EQUAL( A.k(0), mesh_array::UNASSIGNED() );

    // 0 3 1
    BOOST_CHECK_EQUAL( A.i(1), 2 );
    BOOST_CHECK_EQUAL( A.j(1), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.k(1), 0 );

    // 1 3 4
    BOOST_CHECK_EQUAL( A.i(2), 5 );
    BOOST_CHECK_EQUAL( A.j(2), 3 );
    BOOST_CHECK_EQUAL( A.k(2), 1 );

    // 1 4 2
    BOOST_CHECK_EQUAL( A.i(3), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.j(3), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.k(3), 2 );

    // 3 6 7
    BOOST_CHECK_EQUAL( A.i(4), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.j(4), 5 );
    BOOST_CHECK_EQUAL( A.k(4), 0 );

    // 3 7 4
    BOOST_CHECK_EQUAL( A.i(5), 6 );
    BOOST_CHECK_EQUAL( A.j(5), 2 );
    BOOST_CHECK_EQUAL( A.k(5), 4 );

    // 4 7 8
    BOOST_CHECK_EQUAL( A.i(6), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.j(6), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.k(6), 5 );

    /**

     0   1   2
     *---*---*
     |\1/2\3/
     |0*3--*4  *5
     |/4\5/6\
     *---*---*
     6   7   8
     */

  }
}

BOOST_AUTO_TEST_CASE(tetrahedron_adjacency)
{
  typedef mesh_array::T4Mesh                           mesh_type;
  typedef mesh_array::VertexRing< mesh_type >          vertex_ring_type;
  typedef mesh_array::AdjacencyInfo<mesh_type>          adjacency_type;

  // Empty mesh case
  {
    mesh_type M;

    vertex_ring_type R( M );

    adjacency_type A(R);

    // BOOST_CHECK_EQUAL( A.i(0) , mesh_array::UNASSIGNED());   // Hmmm really not defined!
  }

  // Small mesh case
  {
    mesh_type M;

    M.set_capacity(5,2);

    /**

         0
         *
        / \
     1 *---* 2
        \ /
         *
        3
     */

    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();

    M.push_tetrahedron( 0, 1, 2, 4);
    M.push_tetrahedron( 1, 3, 2, 4);

    vertex_ring_type R( M );

    adjacency_type  A(R);

    BOOST_CHECK_EQUAL( A.i(0), 1 );
    BOOST_CHECK_EQUAL( A.j(0), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.k(0), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.m(0), mesh_array::UNASSIGNED() );

    BOOST_CHECK_EQUAL( A.i(1), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.j(1), 0 );
    BOOST_CHECK_EQUAL( A.k(1), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.m(1), mesh_array::UNASSIGNED() );
  }

  // Non empty mesh case
  {
    mesh_type M;
    M.set_capacity(10,7);

    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();
    M.push_vertex();

    M.push_tetrahedron( 0, 6, 3, 9);
    M.push_tetrahedron( 0, 3, 1, 9);
    M.push_tetrahedron( 1, 3, 4, 9);
    M.push_tetrahedron( 1, 4, 2, 9);
    M.push_tetrahedron( 3, 6, 7, 9);
    M.push_tetrahedron( 3, 7, 4, 9);
    M.push_tetrahedron( 4, 7, 8, 9);

    vertex_ring_type R( M );
    adjacency_type  A(R);

    // 0 6 3
    BOOST_CHECK_EQUAL( A.i(0), 4 );
    BOOST_CHECK_EQUAL( A.j(0), 1 );
    BOOST_CHECK_EQUAL( A.k(0), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.m(0), mesh_array::UNASSIGNED() );

    // 0 3 1
    BOOST_CHECK_EQUAL( A.i(1), 2 );
    BOOST_CHECK_EQUAL( A.j(1), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.k(1), 0 );
    BOOST_CHECK_EQUAL( A.m(1), mesh_array::UNASSIGNED() );

    // 1 3 4
    BOOST_CHECK_EQUAL( A.i(2), 5 );
    BOOST_CHECK_EQUAL( A.j(2), 3 );
    BOOST_CHECK_EQUAL( A.k(2), 1 );
    BOOST_CHECK_EQUAL( A.m(2), mesh_array::UNASSIGNED() );

    // 1 4 2
    BOOST_CHECK_EQUAL( A.i(3), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.j(3), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.k(3), 2 );
    BOOST_CHECK_EQUAL( A.m(3), mesh_array::UNASSIGNED() );

    // 3 6 7
    BOOST_CHECK_EQUAL( A.i(4), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.j(4), 5 );
    BOOST_CHECK_EQUAL( A.k(4), 0 );
    BOOST_CHECK_EQUAL( A.m(4), mesh_array::UNASSIGNED() );

    // 3 7 4
    BOOST_CHECK_EQUAL( A.i(5), 6 );
    BOOST_CHECK_EQUAL( A.j(5), 2 );
    BOOST_CHECK_EQUAL( A.k(5), 4 );
    BOOST_CHECK_EQUAL( A.m(5), mesh_array::UNASSIGNED() );

    // 4 7 8
    BOOST_CHECK_EQUAL( A.i(6), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.j(6), mesh_array::UNASSIGNED() );
    BOOST_CHECK_EQUAL( A.k(6), 5 );
    BOOST_CHECK_EQUAL( A.m(6), mesh_array::UNASSIGNED() );

    /**

     0   1   2
     *---*---*
     |\1/2\3/
     |0*3--*4  *5
     |/4\5/6\
     *---*---*
     6   7   8
     */

  }
}

BOOST_AUTO_TEST_SUITE_END();


/*
 * Copyright 2015 Christoph Jud (christoph.jud@unibas.ch)
 *
 * 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 <string>
#include <iostream>
#include <fstream>
#include <memory>

#include <Eigen/Dense>

#include "MatrixIO.h"

namespace gpr{

typedef Eigen::Matrix<float, Eigen::Dynamic, 1> VectorType;
typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixType;
typedef Eigen::DiagonalMatrix<float, Eigen::Dynamic> DiagMatrixType;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorTypeDoublePrecision;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixTypeDoublePrecision;
typedef Eigen::DiagonalMatrix<double, Eigen::Dynamic> DiagMatrixTypeDoublePrecision;

typedef unsigned long long int StreamSizeType;

template<typename _Matrix_Type_>
_Matrix_Type_ ReadMatrix(std::string filename){
	typedef typename _Matrix_Type_::Scalar MatrixElementType;
	typedef std::shared_ptr<MatrixElementType> MatrixElementTypePointer;

	std::ifstream matrix_infile;
	matrix_infile.open( filename.c_str() ); // ASCII mode

	// reading header
	std::string line;
	StreamSizeType rows, cols;
	if(std::getline(matrix_infile, line)) {
		std::stringstream line_stream(line);
		if(!(line_stream >> rows && line_stream >> cols)){
			std::stringstream error_message;
			error_message << "ReadMatrix: header is corrupt (filename " << filename << ")." << std::endl;
			throw error_message.str();
		}
	}
	matrix_infile.close();

	// reopen for binary read
	matrix_infile.open( filename.c_str(), std::ifstream::in | std::ifstream::binary );

	// read data
	unsigned header_size = line.size()+1; // +1 because of endline
	char header_buf[header_size];
	matrix_infile.read((char *)&header_buf, header_size*sizeof(char));

	// for large matrices array has to be allocated on the heap
	// a shared pointer is needed because data_buf cannot be deleted before return
	MatrixElementTypePointer data_buf(new MatrixElementType[rows*cols]);
	matrix_infile.read( (char *)&(*data_buf), rows * cols * (StreamSizeType)sizeof(MatrixElementType) );

	typename Eigen::Map<_Matrix_Type_> matrix(&(*data_buf), rows, cols);
	return matrix;
}

template<typename _Matrix_Type_>
void WriteMatrix(const _Matrix_Type_& matrix, std::string filename){
	typedef typename _Matrix_Type_::Scalar MatrixElementType;

	std::ofstream matrix_outfile;
	matrix_outfile.open( filename.c_str(), std::ios::binary );

	StreamSizeType rows = matrix.rows();
	StreamSizeType cols = matrix.cols();

	// writing header
	std::stringstream header_stream;
	header_stream << rows << " " << cols << std::endl;
	matrix_outfile.write( (char *)(header_stream.str().c_str()), header_stream.str().size() * sizeof(char) );

	// writing body
	// has to go onto the heap for large matrices
	MatrixElementType *data = new MatrixElementType[rows*cols];
	typename Eigen::Map<_Matrix_Type_>(data, rows, cols) = matrix;
	matrix_outfile.write( (char *)data, rows * cols * (StreamSizeType)sizeof(MatrixElementType) );
	matrix_outfile.close();

	delete[] data;
}

// returns true if test has been performed successfully
bool MatrixIOTest(){
	MatrixTypeDoublePrecision M = MatrixTypeDoublePrecision::Random(10,3);
	WriteMatrix<MatrixTypeDoublePrecision>(M, "/tmp/matrix.txt");

	MatrixTypeDoublePrecision N = ReadMatrix<MatrixTypeDoublePrecision>("/tmp/matrix.txt");

	if(M.size() != N.size()) return false;

	for(unsigned i=0; i<M.cols(); i++){
		for(unsigned j=0; j<M.rows(); j++){
			if(M(i,j) != N(i,j)) return false;
		}
	}
	return true;
}

template MatrixType ReadMatrix<MatrixType> (std::string);
template MatrixTypeDoublePrecision ReadMatrix<MatrixTypeDoublePrecision> (std::string);
template void WriteMatrix<MatrixType> (const MatrixType&, std::string);
template void WriteMatrix<MatrixTypeDoublePrecision> (const MatrixTypeDoublePrecision&, std::string);

}


#include <dlib/pixel.h>
#include <dlib/array2d.h>
#include <dlib/image_loader/image_loader.h>
#include <dlib/image_keypoint.h>
using namespace std;
using namespace dlib;

// [[Rcpp::export]]
List dlib_surf_points(const std::string file_name,
                      long max_points = 10000,
                      double detection_threshold = 30.0) {
  array2d<rgb_pixel> img;
  load_bmp(img, file_name.c_str());
  std::vector<surf_point> sp = get_surf_points(img, max_points, detection_threshold);
  NumericVector ip_center_x(sp.size());
  NumericVector ip_center_y(sp.size());
  NumericVector ip_angle(sp.size());
  NumericVector ip_scale(sp.size());
  NumericVector ip_score(sp.size());
  NumericVector ip_laplacian(sp.size());
  NumericMatrix ip_surf(sp.size(), 64);

  int points_nr;
  points_nr = sp.size();
  for(int i=0; i<points_nr; i++){
    ip_center_x[i] = sp[i].p.center(0);
    ip_center_y[i] = sp[i].p.center(1);
    ip_angle[i] = sp[i].angle;
    ip_scale[i] = sp[i].p.scale;
    ip_score[i] = sp[i].p.score;
    ip_laplacian[i] = sp[i].p.laplacian;
    for(int j=0; j<64; j++){
      ip_surf(i, j) = sp[i].des(j, 1);
    }

  }
  return List::create(_["points"] = sp.size(),
                      _["x"] = ip_center_x,
                      _["y"] = ip_center_y,
                      _["angle"] = ip_angle,
                      _["pyramid_scale"] = ip_scale,
                      _["score"] = ip_score,
                      _["laplacian"] = ip_laplacian,
                      _["surf"] = ip_surf);
}


/* Boost test/cmp_exn.cpp
 * test policies with respect to exception throwing
 *
 * Copyright 2004 Guillaume Melquiond
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <boost/numeric/interval/interval.hpp>
#include <boost/numeric/interval/checking.hpp>
#include <boost/numeric/interval/compare.hpp>
#include <boost/numeric/interval/policies.hpp>
#include <boost/numeric/interval/compare/tribool.hpp>
#include <boost/core/lightweight_test.hpp>

struct my_checking
{
  static int nan()          { return -1; }
  static bool is_nan(int x) { return x < 0; }
  static int empty_lower()  { return -1; }
  static int empty_upper()  { return -1; }
  static bool is_empty(int l, int u) { return l == -1 && u == -1; }
};

struct empty_class {};

typedef boost::numeric::interval_lib::policies< empty_class, my_checking >
  my_policies;

typedef boost::numeric::interval<int, my_policies> I;

#define BOOST_C_EXN(e) \
  BOOST_TEST_THROWS(e, boost::numeric::interval_lib::comparison_error)

static void test_cer()
{
  I const a(I::empty()), b(1,2);
  int const c = 0, d = my_checking::nan();
  using namespace boost::numeric::interval_lib::compare::certain;

  BOOST_C_EXN(a < b);
  BOOST_C_EXN(a <= b);
  BOOST_C_EXN(a > b);
  BOOST_C_EXN(a >= b);
  BOOST_C_EXN(a == b);
  BOOST_C_EXN(a != b);
  BOOST_C_EXN(b < a);
  BOOST_C_EXN(b <= a);
  BOOST_C_EXN(b > a);
  BOOST_C_EXN(b >= a);
  BOOST_C_EXN(b == a);
  BOOST_C_EXN(b != a);

  BOOST_C_EXN(a < c);
  BOOST_C_EXN(a <= c);
  BOOST_C_EXN(a > c);
  BOOST_C_EXN(a >= c);
  BOOST_C_EXN(a == c);
  BOOST_C_EXN(a != c);
  BOOST_C_EXN(b < d);
  BOOST_C_EXN(b <= d);
  BOOST_C_EXN(b > d);
  BOOST_C_EXN(b >= d);
  BOOST_C_EXN(b == d);
  BOOST_C_EXN(b != d);
}

static void test_def()
{
  I const a(I::empty()), b(1,2);
  int const c = 0, d = my_checking::nan();

  BOOST_C_EXN(a < b);
  BOOST_C_EXN(a <= b);
  BOOST_C_EXN(a > b);
  BOOST_C_EXN(a >= b);
  BOOST_C_EXN(a == b);
  BOOST_C_EXN(a != b);
  BOOST_C_EXN(b < a);
  BOOST_C_EXN(b <= a);
  BOOST_C_EXN(b > a);
  BOOST_C_EXN(b >= a);
  BOOST_C_EXN(b == a);
  BOOST_C_EXN(b != a);

  BOOST_C_EXN(a < c);
  BOOST_C_EXN(a <= c);
  BOOST_C_EXN(a > c);
  BOOST_C_EXN(a >= c);
  BOOST_C_EXN(a == c);
  BOOST_C_EXN(a != c);
  BOOST_C_EXN(b < d);
  BOOST_C_EXN(b <= d);
  BOOST_C_EXN(b > d);
  BOOST_C_EXN(b >= d);
  BOOST_C_EXN(b == d);
  BOOST_C_EXN(b != d);
}

static void test_lex()
{
  I const a(I::empty()), b(1,2);
  int const c = 0, d = my_checking::nan();
  using namespace boost::numeric::interval_lib::compare::lexicographic;

  BOOST_C_EXN(a < b);
  BOOST_C_EXN(a <= b);
  BOOST_C_EXN(a > b);
  BOOST_C_EXN(a >= b);
  BOOST_C_EXN(a == b);
  BOOST_C_EXN(a != b);
  BOOST_C_EXN(b < a);
  BOOST_C_EXN(b <= a);
  BOOST_C_EXN(b > a);
  BOOST_C_EXN(b >= a);
  BOOST_C_EXN(b == a);
  BOOST_C_EXN(b != a);

  BOOST_C_EXN(a < c);
  BOOST_C_EXN(a <= c);
  BOOST_C_EXN(a > c);
  BOOST_C_EXN(a >= c);
  BOOST_C_EXN(a == c);
  BOOST_C_EXN(a != c);
  BOOST_C_EXN(b < d);
  BOOST_C_EXN(b <= d);
  BOOST_C_EXN(b > d);
  BOOST_C_EXN(b >= d);
  BOOST_C_EXN(b == d);
  BOOST_C_EXN(b != d);
}

static void test_pos()
{
  I const a(I::empty()), b(1,2);
  int const c = 0, d = my_checking::nan();
  using namespace boost::numeric::interval_lib::compare::possible;

  BOOST_C_EXN(a < b);
  BOOST_C_EXN(a <= b);
  BOOST_C_EXN(a > b);
  BOOST_C_EXN(a >= b);
  BOOST_C_EXN(a == b);
  BOOST_C_EXN(a != b);
  BOOST_C_EXN(b < a);
  BOOST_C_EXN(b <= a);
  BOOST_C_EXN(b > a);
  BOOST_C_EXN(b >= a);
  BOOST_C_EXN(b == a);
  BOOST_C_EXN(b != a);

  BOOST_C_EXN(a < c);
  BOOST_C_EXN(a <= c);
  BOOST_C_EXN(a > c);
  BOOST_C_EXN(a >= c);
  BOOST_C_EXN(a == c);
  BOOST_C_EXN(a != c);
  BOOST_C_EXN(b < d);
  BOOST_C_EXN(b <= d);
  BOOST_C_EXN(b > d);
  BOOST_C_EXN(b >= d);
  BOOST_C_EXN(b == d);
  BOOST_C_EXN(b != d);
}

static void test_set()
{
  I const a(I::empty()), b(1,2);
  int const c = 0;
  using namespace boost::numeric::interval_lib::compare::set;

  BOOST_C_EXN(a < c);
  BOOST_C_EXN(a <= c);
  BOOST_C_EXN(a > c);
  BOOST_C_EXN(a >= c);
  BOOST_C_EXN(a == c);
  BOOST_C_EXN(a != c);
  BOOST_C_EXN(b < c);
  BOOST_C_EXN(b <= c);
  BOOST_C_EXN(b > c);
  BOOST_C_EXN(b >= c);
  BOOST_C_EXN(b == c);
  BOOST_C_EXN(b != c);
}

static void test_tri()
{
  I const a(I::empty()), b(1,2);
  int const c = 0, d = my_checking::nan();
  using namespace boost::numeric::interval_lib::compare::tribool;

  BOOST_C_EXN(a < b);
  BOOST_C_EXN(a <= b);
  BOOST_C_EXN(a > b);
  BOOST_C_EXN(a >= b);
  BOOST_C_EXN(a == b);
  BOOST_C_EXN(a != b);
  BOOST_C_EXN(b < a);
  BOOST_C_EXN(b <= a);
  BOOST_C_EXN(b > a);
  BOOST_C_EXN(b >= a);
  BOOST_C_EXN(b == a);
  BOOST_C_EXN(b != a);

  BOOST_C_EXN(a < c);
  BOOST_C_EXN(a <= c);
  BOOST_C_EXN(a > c);
  BOOST_C_EXN(a >= c);
  BOOST_C_EXN(a == c);
  BOOST_C_EXN(a != c);
  BOOST_C_EXN(b < d);
  BOOST_C_EXN(b <= d);
  BOOST_C_EXN(b > d);
  BOOST_C_EXN(b >= d);
  BOOST_C_EXN(b == d);
  BOOST_C_EXN(b != d);
}

int main() {
  test_cer();
  test_def();
  test_lex();
  test_pos();
  test_set();
  test_tri();

  return boost::report_errors();
}


/**
 * @file encodeDescriptorsf32.cpp
 * @author Louai KASSA BAGHDOUCHE
 * @brief generate chemical descriptors from smi files
 * @version 0.1
 * @date 2022-04-05
 *
 * @copyright Copyright (c) 2022
 *
 */

#include <iostream>
#include <string>
#include <GraphMol/Descriptors/MolDescriptors.h>
#include <GraphMol/SmilesParse/SmilesParse.h>
#include <GraphMol/FileParsers/MolSupplier.h>
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>

using namespace std;
using namespace RDKit;
using namespace RDKit::MolOps;
using namespace RDKit::Descriptors;
using namespace boost::filesystem;
using boost_ofstream = boost::filesystem::ofstream;

int main(int argc, char* argv[])
{
	if (argc != 2)
	{
		cerr << "usage: ./descriptors [INPUT SMI]" << endl;
		return 1;
	}
	const auto input_smi = argv[1];
	int pos = string(input_smi).find('.');
	path four_chem_desc = string(input_smi).substr(0, pos) + "_4properties.txt";
	path five_chem_desc = string(input_smi).substr(0, pos) + "_5properties.txt";
	boost_ofstream real4prop(four_chem_desc, ios::app);
	boost_ofstream real5prop(five_chem_desc, ios::app);
	SmilesMolSupplier smi(input_smi, "\t", 1, 0);
	size_t counter = 1;
	while (!smi.atEnd())
	{
		const unique_ptr<ROMol> mol_ptr(smi.next());
		const auto& mol = *mol_ptr;
		std::cout << "Processing molecule " << counter << endl;
		// generate 4 chemical properties for the molecules
		real4prop << calcExactMW(mol) << '\t'
							<< calcClogP(mol) << '\t'
							<< calcTPSA(mol) << '\t'
							<< calcLabuteASA(mol) << '\n';
		// generate 5 chemical properties for the molecules
		real5prop << mol.getNumHeavyAtoms() << '\t'
							<< calcNumHBD(mol) << '\t'
							<< calcNumHBA(mol) << '\t'
							<< calcNumRotatableBonds(mol) << '\t'
							<< calcNumRings(mol) << '\n';
		counter++;
	}
}