#pragma once
#include "detail/config.hpp"
#include "numeric/sparseref.hpp"

#include <Eigen/SparseCore>

namespace cpb {

template <class scalar_t>
using SparseMatrixX = Eigen::SparseMatrix<scalar_t, Eigen::RowMajor, storage_idx_t>;

using SparseMatrixXf = SparseMatrixX<float>;
using SparseMatrixXcf = SparseMatrixX<std::complex<float>>;
using SparseMatrixXd = SparseMatrixX<double>;
using SparseMatrixXcd = SparseMatrixX<std::complex<double>>;

using num::RealCsrConstRef;
using num::ComplexCsrConstRef;
using num::RealEllConstRef;
using num::ComplexEllConstRef;

/**
 Return a CSR matrix reference
 */
template<class scalar_t>
inline num::CsrConstRef<scalar_t> csrref(SparseMatrixX<scalar_t> const& m) {
    return {static_cast<int>(m.rows()), static_cast<int>(m.cols()), static_cast<int>(m.nonZeros()),
            m.valuePtr(), m.innerIndexPtr(), m.outerIndexPtr()};
};

template<class scalar_t>
class CompressedInserter {
public:
    CompressedInserter(SparseMatrixX<scalar_t>& mat, idx_t size)
        : matrix(mat) { matrix.reserve(size); }

    void start_row() {
        matrix.outerIndexPtr()[row++] = static_cast<storage_idx_t>(idx);
    }

    void start_row(idx_t row_index) {
        while (row <= row_index)
            matrix.outerIndexPtr()[row++] = static_cast<storage_idx_t>(idx);
    }

    void insert(idx_t column, scalar_t value) {
        auto start_idx = matrix.outerIndexPtr()[row-1];
        auto n = idx++;
        while (n > start_idx && matrix.innerIndexPtr()[n - 1] > column) {
            matrix.innerIndexPtr()[n] = matrix.innerIndexPtr()[n - 1];
            matrix.valuePtr()[n] = matrix.valuePtr()[n - 1];
            --n;
        }
        matrix.innerIndexPtr()[n] = static_cast<storage_idx_t>(column);
        matrix.valuePtr()[n] = value;
    }

    void compress() {
        // close outerIndexPtr
        start_row(matrix.outerSize());
        // trim valuePtr and innerIndexPtr
        matrix.resizeNonZeros(idx);
    }

private:
    idx_t idx = 0;
    idx_t row = 0;
    SparseMatrixX<scalar_t>& matrix;
};

template<class scalar_t>
inline CompressedInserter<scalar_t> compressed_inserter(SparseMatrixX<scalar_t>& mat, idx_t size) {
    return {mat, size};
}

template<class SparseMatrix, class Index>
inline auto sparse_row(const SparseMatrix& mat, Index outer_index)
    -> typename SparseMatrix::InnerIterator
{
    return {mat, outer_index};
}

namespace num {

template<class scalar_t>
SparseMatrixX<scalar_t> force_cast(SparseMatrixXcd const& m) { return m.cast<scalar_t>(); }
template<>
inline SparseMatrixX<double> force_cast<double>(SparseMatrixXcd const& m) { return m.real(); }
template<>
inline SparseMatrixX<float> force_cast<float>(SparseMatrixXcd const& m) {
    return m.real().cast<float>();
}

} // namespace num

namespace sparse {

/// SparseMatrix wrapper with several functions for efficient CSR matrix element access
template<class scalar_t>
class Loop {
public:
    Loop(SparseMatrixX<scalar_t> const& matrix)
        : outer_size(matrix.outerSize()), data(matrix.valuePtr()),
          indices(matrix.innerIndexPtr()), indptr(matrix.outerIndexPtr()) {}

    /// Visit each index and value of the sparse matrix:
    ///     lambda(idx_t outer, idx_t inner, scalar_t value)
    template<class F>
    void for_each(F lambda) const {
        for (auto outer = idx_t{0}; outer < outer_size; ++outer) {
            for (auto idx = indptr[outer]; idx < indptr[outer + 1]; ++idx) {
                lambda(outer, indices[idx], data[idx]);
            }
        }
    }

    /// Visit each index and value of the sparse matrix:
    ///     lambda(idx_t outer, idx_t inner, scalar_t value, idx_t buffer_position)
    /// After every 'buffer_size' iterations, the 'process_buffer' function is called:
    ///     process_buffer(idx_t start_outer, idx_t start_data, idx_t last_buffer_size)
    template<class F1, class F2>
    void buffered_for_each(idx_t buffer_size, F1 lambda, F2 process_buffer) const {
        auto n = idx_t{0};
        auto previous_outer = idx_t{0};
        auto previous_idx = static_cast<idx_t>(indptr[0]);

        for (auto outer = idx_t{0}; outer < outer_size; ++outer) {
            for (auto idx = indptr[outer]; idx < indptr[outer + 1]; ++idx, ++n) {
                if (n == buffer_size) {
                    process_buffer(previous_outer, previous_idx, buffer_size);
                    previous_outer = outer;
                    previous_idx = idx;
                    n = 0;
                }

                lambda(outer, indices[idx], data[idx], n);
            }
        }

        process_buffer(previous_outer, previous_idx, n);
    }

    /// Iterate over all elements in a single row (or column) at the 'outer' index:
    ///     lambda(idx_t inner, scalar_t value)
    template<class F>
    void for_each_in_row(idx_t outer, F lambda) const {
        for (auto idx = indptr[outer]; idx < indptr[outer + 1]; ++idx) {
            lambda(indices[idx], data[idx]);
        }
    }

    /// Start iteration from some position given by 'outer' and 'data' indices
    /// and loop for 'slice_size' iterations:
    ///     lambda(idx_t outer, idx_t inner, scalar_t value, idx_t current_iteration)
    template<class F>
    void slice_for_each(idx_t outer, idx_t idx, idx_t slice_size, F lambda) const {
        auto n = idx_t{0};
        for (; outer < outer_size; ++outer) {
            for (; idx < indptr[outer + 1]; ++idx, ++n) {
                if (n == slice_size)
                    return;

                lambda(outer, indices[idx], data[idx], n);
            }
        }
    }

private:
    idx_t const outer_size;
    scalar_t const* const data;
    storage_idx_t const* const indices;
    storage_idx_t const* const indptr;
};

template<class scalar_t>
inline Loop<scalar_t> make_loop(SparseMatrixX<scalar_t> const& m) { return {m}; }

/**
 Return the maximum number of non-zeros per row
 */
template<class scalar_t>
idx_t max_nnz_per_row(SparseMatrixX<scalar_t> const& m) {
    auto max = 0;
    for (auto i = 0; i < m.outerSize(); ++i) {
        auto const nnz = m.outerIndexPtr()[i + 1] - m.outerIndexPtr()[i];
        if (nnz > max) {
            max = nnz;
        }
    }
    return max;
}

}} // namespace cpb::sparse


#ifndef ASLAM_NON_SQUARED_ERROR_TERM_HPP
#define ASLAM_NON_SQUARED_ERROR_TERM_HPP

#include <sparse_block_matrix/sparse_block_matrix.h>
#include <boost/shared_ptr.hpp>
#include "backend.hpp"
#include "JacobianContainer.hpp"
#include "util/CommonDefinitions.hpp"
#include <aslam/Exceptions.hpp>
#include <sm/eigen/NumericalDiff.hpp>
#include <sm/timing/Timer.hpp>
#include "MEstimatorPolicies.hpp"
#include <sm/eigen/matrix_sqrt.hpp>
#include <sm/timing/NsecTimeUtilities.hpp>

namespace aslam {
  namespace backend {
    class MEstimator;

    /**
     * \class NonSquaredErrorTerm
     *
     * \brief a class representing a single scalar error term in a general
     * optimization problem of the form \f$ \sum_{i=0}^N w_i \cdot e_i \f$.
     */
    class ScalarNonSquaredErrorTerm {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

      typedef boost::shared_ptr<aslam::backend::ScalarNonSquaredErrorTerm> Ptr;

      ScalarNonSquaredErrorTerm();
      virtual ~ScalarNonSquaredErrorTerm();

      /// \brief evaluate the error term and return the effective error.
      ///        This is equivalent to first call updateRawError() and then taking the result of getWeightedError();
      ///        After this is called, the _error is filled in with \f$ w \cdot e \f$
      double evaluateError() {
        updateRawError();
        return getWeightedError();
      }

      /// \brief update (compute and store) the raw error
      ///        After this is called, the _error is filled in with \f$ w \cdot e \f$
      ///        The raw error is returned.
      double updateRawError();

      /// \brief evaluate the Jacobians. Equivalent to evaluateWeightedJacobians.
      inline void evaluateJacobians(JacobianContainer & outJacobians) {
        Timer t("ScalarNonSquaredErrorTerm: evaluateJacobians", false);
        evaluateWeightedJacobians(outJacobians);
      }

      /// \brief evaluate the Jacobians using finite differences.
      void evaluateJacobiansFiniteDifference(JacobianContainer & outJacobians);

      /// \brief evaluate the raw Jacobians of \f$ w \cdot e \f$, not applying M-estimator policy and design variable scaling
      void evaluateRawJacobians(JacobianContainer & outJacobians);

      /// \brief evaluate the Jacobians of \f$ w \cdot e \f$ applying M-estimator policy and design variable scaling
      void evaluateWeightedJacobians(JacobianContainer& outJc);

      /// \brief Get the jacobians with or without M-estimator policy
      void evaluateJacobians(JacobianContainer& outJc, bool useMEstimator) {
        if(useMEstimator) evaluateWeightedJacobians(outJc);
        else evaluateRawJacobians(outJc);
      }

      /// \brief Get the error (before weighting by the M-estimator policy)
      double getRawError() const { return _error; }
      /// \brief Get the current, weighted error, i.e. with the M-estimator weight already applied.
      double getWeightedError() const { return getCurrentMEstimatorWeight() * getRawError(); }
      /// \brief Get the error with or without M-estimator policy
      inline double getError(bool useMEstimator) const {
        return useMEstimator ? getWeightedError() : getRawError();
      }

      /// \brief Getter for the weight
      inline double getWeight() const;

      /// \brief set the weight
      inline void setWeight(const double w);

      /// \brief returns a pointer to the MEstimator used. Return Null if the
      /// MEstimator used is not a MEstimatorType
      template <typename MEstimatorType>
      boost::shared_ptr<MEstimatorType> getMEstimatorPolicy();

      /// \brief set the M-Estimator policy. This function takes a squared error
      ///        and returns a weight to apply to that error term.
      void setMEstimatorPolicy(const boost::shared_ptr<MEstimator> & mEstimator);

      /// \brief clear the m-estimator policy.
      void clearMEstimatorPolicy();

      /// \brief compute the M-estimator weight from a squared error.
      double getMEstimatorWeight(double error) const { return _mEstimatorPolicy->getWeight(fabs(error)); }

      double getCurrentMEstimatorWeight() const { return getMEstimatorWeight(getRawError()); }

      /// \brief get the name of the M-Estimator.
      std::string getMEstimatorName();

      /// \brief How many design variables is this error term connected to?
      size_t numDesignVariables() const;

      /// \brief Get design variable i.
      DesignVariable* designVariable(size_t i);

      /// \brief Get design variable i.
      const DesignVariable* designVariable(size_t i) const;

      /// \brief Fill the set with all design variables.
      void getDesignVariables(DesignVariable::set_t& dvs);

      /// \brief Get the design variables
      const std::vector<DesignVariable*> & designVariables() const;

      void setTime(const sm::timing::NsecTime& t);
      sm::timing::NsecTime getTime() const { return _timestamp; }

      /// \brief Get the error term dimension. For compatibility with squared error term interface.
      inline size_t dimension() const { return 1UL; }

    protected:

      /// \brief evaluate the error term and return the scalar error \f$ e \f$
      virtual double evaluateErrorImplementation() = 0;

      /// \brief evaluate the Jacobians for \f$ e \f$
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) = 0;

      /// \brief child classes should set the set of design variables using this function.
      void setDesignVariables(const std::vector<DesignVariable*> & designVariables);

      void setDesignVariables(DesignVariable* dv1);
      void setDesignVariables(DesignVariable* dv1, DesignVariable* dv2);
      void setDesignVariables(DesignVariable* dv1, DesignVariable* dv2, DesignVariable* dv3);
      void setDesignVariables(DesignVariable* dv1, DesignVariable* dv2, DesignVariable* dv3, DesignVariable* dv4);

      template<typename ITERATOR_T>
      void setDesignVariablesIterator(ITERATOR_T start, ITERATOR_T end);

      /// \brief the MEstimator policy for this error term
      boost::shared_ptr<MEstimator> _mEstimatorPolicy;

    private:
      /// \brief the error \f$ w \cdot e \f$
      double _error;

      /// \brief the weight in \f$ w \cdot e \f$
      double _w;

      /// \brief The list of design variables.
      std::vector<DesignVariable*> _designVariables;

      sm::timing::NsecTime _timestamp;
    };

  } // namespace backend
} // namespace aslam

#include "implementation/ScalarNonSquaredErrorTerm.hpp"

#endif /* ASLAM_NON_SQUARED_ERROR_TERM_HPP */


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

// A simple example showing three different ways to initialize a heap, using
// different underlying datastructures
#include <binminheap.h>
#include <map>
#include <boost/unordered_map.hpp>

using namespace binminheap;
using namespace std;

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

  // Ex 1
  {
    // Since we use a std::vector for the heapscratch,
    // we must make sure that the highest key can be
    // mapped in the vector, hence the need for 4 elements
    // instead of 3.
    cout << "example #1 using heapscratch type std::vector: " << endl;
    std::vector<size_t> heapscratch(4, std::numeric_limits<size_t>::max());
    BinaryMinHeap<double, int> heap(heapscratch);
    heap.push(make_pair(.1, 2));
    heap.push(make_pair(.1, 3));
    heap.push(make_pair(.1, 1));
    cout << "heap: " << endl << heap << endl;
    heap.pop();
    cout << "heap: " << endl << heap << endl;
  }

  // Ex 2
  {
    cout << "example #2 using heapscratch type boost::unordered_map: " << endl;
    typedef boost::unordered_map<int, size_t> heapscratch_map;
    typedef HeapscratchConverter<heapscratch_map> heapscratch_type;
    heapscratch_map heapscratchMap;
    heapscratch_type heapscratch(heapscratchMap);
    BinaryMinHeap<double, int, heapscratch_type> heap(heapscratch);
    heap.push(make_pair(.1, 2));
    heap.push(make_pair(.1, 3));
    heap.push(make_pair(.1, 1));
    cout << "heap: " << endl << heap << endl;
    heap.pop();
    cout << "heap: " << endl << heap << endl;
  }

  // Ex 3
  {
    cout << "example #3 using heapscratch type boost::unordered_map with O(n) "
            "constructor: " << endl;
    typedef boost::unordered_map<int, size_t> heapscratch_map;
    typedef HeapscratchConverter<heapscratch_map> heapscratch_type;
    heapscratch_map heapscratchMap;
    heapscratch_type heapscratch(heapscratchMap);
    map<double, int> prioritykeys;
    for (size_t i = 0; i < 10; i++) {
      prioritykeys[2 * i] = i;
    }
    BinaryMinHeap<double, int, heapscratch_type> heap(
        heapscratch, prioritykeys.begin(), prioritykeys.end());
    cout << "heap: " << endl << heap << endl;
    heap.pop();
    cout << "heap: " << endl << heap << endl;
  }

  return 0;
}


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

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DIFFERENCE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DIFFERENCE_HPP

#include <algorithm>

#include <boost/geometry/algorithms/detail/overlay/intersection_insert.hpp>
#include <boost/geometry/policies/robustness/get_rescale_policy.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace difference
{

/*!
\brief_calc2{difference} \brief_strategy
\ingroup difference
\details \details_calc2{difference_insert, spatial set theoretic difference}
    \brief_strategy. \details_inserter{difference}
\tparam GeometryOut output geometry type, must be specified
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam OutputIterator output iterator
\tparam Strategy \tparam_strategy_overlay
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param out \param_out{difference}
\param strategy \param_strategy{difference}
\return \return_out

\qbk{distinguish,with strategy}
*/
template
<
    typename GeometryOut,
    typename Geometry1,
    typename Geometry2,
    typename RobustPolicy,
    typename OutputIterator,
    typename Strategy
>
inline OutputIterator difference_insert(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            RobustPolicy const& robust_policy,
            OutputIterator out,
            Strategy const& strategy)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();
    concept::check<GeometryOut>();

    return geometry::dispatch::intersection_insert
        <
            Geometry1, Geometry2,
            GeometryOut,
            overlay_difference,
            geometry::detail::overlay::do_reverse<geometry::point_order<Geometry1>::value>::value,
            geometry::detail::overlay::do_reverse<geometry::point_order<Geometry2>::value, true>::value
        >::apply(geometry1, geometry2, robust_policy, out, strategy);
}

/*!
\brief_calc2{difference}
\ingroup difference
\details \details_calc2{difference_insert, spatial set theoretic difference}.
    \details_insert{difference}
\tparam GeometryOut output geometry type, must be specified
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam OutputIterator output iterator
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param out \param_out{difference}
\return \return_out

\qbk{[include reference/algorithms/difference_insert.qbk]}
*/
template
<
    typename GeometryOut,
    typename Geometry1,
    typename Geometry2,
    typename RobustPolicy,
    typename OutputIterator
>
inline OutputIterator difference_insert(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            RobustPolicy const& robust_policy,
            OutputIterator out)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();
    concept::check<GeometryOut>();

    typedef strategy_intersection
        <
            typename cs_tag<GeometryOut>::type,
            Geometry1,
            Geometry2,
            typename geometry::point_type<GeometryOut>::type,
            RobustPolicy
        > strategy;

    return difference_insert<GeometryOut>(geometry1, geometry2,
            robust_policy, out, strategy());
}

}} // namespace detail::difference
#endif // DOXYGEN_NO_DETAIL

/*!
\brief_calc2{difference}
\ingroup difference
\details \details_calc2{difference, spatial set theoretic difference}.
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam Collection \tparam_output_collection
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param output_collection the output collection

\qbk{[include reference/algorithms/difference.qbk]}
*/
template
<
    typename Geometry1,
    typename Geometry2,
    typename Collection
>
inline void difference(Geometry1 const& geometry1,
            Geometry2 const& geometry2, Collection& output_collection)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();

    typedef typename boost::range_value<Collection>::type geometry_out;
    concept::check<geometry_out>();

    typedef typename geometry::rescale_overlay_policy_type
        <
            Geometry1,
            Geometry2
        >::type rescale_policy_type;

    rescale_policy_type robust_policy
            = geometry::get_rescale_policy<rescale_policy_type>(geometry1, geometry2);

    detail::difference::difference_insert<geometry_out>(
            geometry1, geometry2, robust_policy,
            range::back_inserter(output_collection));
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DIFFERENCE_HPP


#include <iostream>
#include <fstream>
#include <mapnik/datasource.hpp>
#include <mapnik/datasource_cache.hpp>
#include <mapnik/geometry.hpp>
#include <mapnik/geometry/boost_adapters.hpp>
#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/algorithms/comparable_distance.hpp>
#include <boost/geometry/extensions/algorithms/closest_point.hpp>
#include <boost/geometry/io/svg/svg_mapper.hpp>
#include <mapnik/util/geometry_to_wkt.hpp>

namespace mapnik { namespace geometry {

struct closest_point
{
    using result_type = boost::geometry::closest_point_result<mapnik::geometry::point<double>>;

    closest_point(mapnik::geometry::point<double> const& pt)
        : pt_(pt) {}

    result_type operator() (mapnik::geometry::geometry_empty const&) const
    {
        return result_type(); // FIXME: consider std::optional<result_type>
    }

    result_type operator() (mapnik::geometry::point<double> const& pt) const
    {
        result_type info;
        boost::geometry::closest_point(pt_ ,pt, info);
        return info;
    }

    result_type operator() (mapnik::geometry::line_string<double> const& line) const
    {
        result_type info;
        boost::geometry::closest_point(pt_ ,line, info);
        return info;
    }

    result_type operator() (mapnik::geometry::polygon<double> const& poly) const
    {
        result_type info;
        if (boost::geometry::within(pt_, poly))
        {
            info.closest_point = pt_;
            info.distance = 0.0;
            return info;
        }
        bool first = true;
        for (auto const& ring : poly)
        {
            result_type ring_info;
            boost::geometry::closest_point(pt_ ,ring, ring_info);
            if (first)
            {
                first = false;
                info = std::move(ring_info);
            }
            else if (ring_info.distance < info.distance)
            {
                info = std::move(ring_info);
            }
        }
        return info;
    }

    // Multi* + GeometryCollection
    result_type operator() (mapnik::geometry::geometry<double> const& geom) const
    {
        return mapnik::util::apply_visitor(*this, geom);
    }

    template <typename T>
    result_type operator() (T const& multi_geom) const
    {
        result_type info;
        bool first = true;
        for (auto const& geom : multi_geom)
        {
            if (first)
            {
                first = false;
                info = std::move(operator()(geom));
            }
            else
            {
                auto sub_info = operator()(geom);
                if (sub_info.distance < info.distance)
                {
                    info = std::move(sub_info);
                }
            }
        }
        return info;
    }

    mapnik::geometry::point<double> pt_;
};

}}

int main(int argc, char** argv)
{
    std::cerr << "Comparable distance test" << std::endl;
    mapnik::datasource_cache::instance().register_datasources("/opt/mapnik/lib/mapnik/input");
    if (argc != 4)
    {
        std::cerr << "Usage: <geojson-filename> <x> <y>" << std::endl;
        return EXIT_FAILURE;
    }
    mapnik::parameters params;
    params["type"] = "geojson";
    params["file"] = std::string(argv[1]);
    double x = std::stod(argv[2]);
    double y = std::stod(argv[3]);
    auto ds = mapnik::datasource_cache::instance().create(params);
    if (ds == nullptr)
    {
        std::cerr << "Failed to create datasource" << std::endl;
        return EXIT_FAILURE;
    }
    mapnik::query q(ds->envelope());
    auto features = ds->features(q);
    auto feature =  features->next();

    mapnik::geometry::point<double> pt(x, y);

    std::ofstream svg("point_to_geometry_distance.svg");
    boost::geometry::svg_mapper<mapnik::geometry::point<double>> mapper(svg, 600, 400,
                                                                        "width=\"800\" height=\"600\" fill=\"white\"");

    std::array<std::string, 4> four_colors = {{"red", "blue", "green", "yellow"}};
    std::size_t counter = 0;
    while (feature != nullptr)
    {
        auto const& geom = feature->get_geometry();
        if (geom.is<mapnik::geometry::polygon<double>>())
        {

            auto const& poly = geom.get<mapnik::geometry::polygon<double>>();
            mapper.add(poly);
            std::string style = "stroke:blue;stroke-width:1; fill-opacity:0.5;fill:" + four_colors[counter % four_colors.size()];
            ++counter;
            mapper.map(poly, style);
        }

        auto result = mapnik::util::apply_visitor(mapnik::geometry::closest_point(pt), geom);
        std::string wkt;
        mapnik::util::to_wkt(wkt, result.closest_point);
        std::cerr << "closest point:" << wkt
                  << " distance=" << result.distance << std::endl;

        mapnik::geometry::line_string<double> line;
        line.push_back(pt);
        line.push_back(result.closest_point);
        mapper.add(line);
        mapper.map(line,"stroke:red;stroke-width:1;stroke-dasharray:4,4;comp-op:color-burn");

        mapper.add(result.closest_point);
        mapper.map(result.closest_point,"fill-opacity:1.0;fill:yellow;stroke:blue;stroke-width:1");
        feature = features->next();
    }
    mapper.add(pt);
    mapper.map(pt,"fill-opacity:1.0;fill:salmon;stroke:blue;stroke-width:1");
    std::cerr << "Done" << std::endl;
    return EXIT_SUCCESS;
}


// Copyright 2015-2022 The ALMA Project Developers
//
// 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.

#pragma once

/// @file
/// Code implementing the full BTE solution by the Omini-Sparavigna
/// iterative method, as implemented in ShengBTE.

#include <Eigen/Dense>
#include <processes.hpp>
#include <isotopic_scattering.hpp>

namespace alma {
/// Class implementing an iterative solution to the BTE following
/// the scheme devised by Omini and Sparavigna.
class ShengBTE_iterator {
private:
    /// Number of points in the grid.
    const std::size_t nqpoints;
    /// Number of irreducible q points.
    const std::size_t nirred;
    /// Number of branches.
    const std::size_t nbranches;
    /// Volume of the unit cell.
    const double V;
    /// Iteration number.
    std::size_t n;
    /// Angular frequencies for all points on the grid [rad / ps].
    Eigen::MatrixXd omega;
    /// Velocities for all points on the grid [nm / ps].
    Eigen::MatrixXd vg;
    /// Lifetimes for all points on the grid [nm / ps]
    Eigen::MatrixXd tau0;
    /// Value of the intermediate quantity F, measuring the distance to
    /// equilibrium, for the current iteration.
    Eigen::MatrixXd F;
    /// MPI communicator used to coordinate different processes.
    const boost::mpi::communicator& comm;

public:
    /// Basic constructor to initialize F.
    ///
    /// @param[in] poscar - description of the unit cell
    /// @param[in] grid - phonon spectrum on a regular q-point grid
    /// @param[in] syms - symmetry operations object
    /// @param[inout] threeph_procs - list of 3-phonon processes
    /// @param[inout] twoph_procs - list of 2-phonon processes
    /// @param[in] T - temperature in K
    /// @param[in] comm - communicator used to synchronize all processes
    ShengBTE_iterator(const Crystal_structure& poscar,
                      const Gamma_grid& grid,
                      const Symmetry_operations& syms,
                      std::vector<Threeph_process>& threeph_procs,
                      std::vector<Twoph_process>& twoph_procs,
                      double T,
                      const boost::mpi::communicator& comm_);

    /// Advance to the next iteration.
    ///
    /// @param[in] poscar - description of the unit cell
    /// @param[in] grid - phonon spectrum on a regular q-point grid
    /// @param[in] syms - symmetry operations object
    /// @param[inout] threeph_procs - list of 3-phonon processes
    /// @param[inout] twoph_procs - list of 2-phonon processes
    /// @param[in] T - temperature in K
    void next(const Crystal_structure& poscar,
              const Gamma_grid& grid,
              const Symmetry_operations& syms,
              std::vector<Threeph_process>& threeph_procs,
              std::vector<Twoph_process>& twoph_procs,
              double T);

    /// Get the current estimate of the thermal conductivity tensor.
    ///
    /// @param[in] T - temperature in K
    /// @return a 3x3 matrix with all the components of kappa
    /// [W / (m K)]
    Eigen::Matrix3d calc_current_kappa(double T) const;

    /// Get the current estimate of the contribution to the thermal conductivity
    /// tensor from a single branch.
    ///
    /// In this context, a branch is defined as the set of modes with the same
    /// index
    /// when energies are sorted in ascending order at each q point.
    /// @param[in] T - temperature in K
    /// @param[in] branch - branch index
    /// @return a 3x3 matrix with all the components of kappa
    /// [W / (m K)]
    Eigen::Matrix3d calc_current_kappa_branch(double T,
                                              std::size_t branch) const;

    /// Obtain the cumulative histogram of contributions to the thermal
    /// conductivity as a function of angular frequency.
    ///
    /// @param[in] T - temperature in K
    /// @param[ticks] - upper edges of the histogram bins
    /// @return a std::vector of thermal conductivity tensors with
    /// the cumulative histogram
    std::vector<Eigen::Matrix3d> calc_cumulative_kappa_omega(
        double T,
        Eigen::ArrayXd ticks);

    /// Obtain the cumulative histogram of contributions to the thermal
    /// conductivity as a function of pseudo-mean free path.
    ///
    /// @param[in] T - temperature in K
    /// @param[ticks] - upper edges of the histogram bins
    /// @return a std::vector of thermal conductivity tensors with
    /// the cumulative histogram
    std::vector<Eigen::Matrix3d> calc_cumulative_kappa_lambda(
        double T,
        Eigen::ArrayXd ticks);

    /// Obtain a set of pseudo-scattering rates from the current estimate of the
    /// solution.
    ///
    /// Note that these are not the inverse of any real relaxation time, since
    /// no such thing exist in the full linearized BTE formalism for phonons.
    /// Substituting these into the RTA expression for kappa will not yield
    /// the right thermal conductivity. Moreover, some elements can be negative.
    /// See the ShengBTE paper for details.
    /// @return an array of pseudo-scattering rates for each q point and each
    /// mode [ps ** (-1)]
    Eigen::ArrayXXd calc_w() const;

    /// Obtain a set of pseudo-mean free paths from the current estimate of the
    /// solution.
    ///
    /// Note that these are not real MFPs, since no such thing exist in the full
    /// linearized BTE formalism for phonons. Substituting these into the RTA
    /// expression for kappa will not yield the right thermal conductivity.
    /// Moreover, some elements can be negative.See the ShengBTE paper for
    /// details.
    /// @return an array of pseudo-scattering rates for each q point and each
    /// mode [ps ** (-1)]
    Eigen::ArrayXXd calc_lambda() const;
};

/// Compute the full thermal conductivity tensor use the
/// Omini-Sparavigna
/// iterative approach.
///
/// @param[in] poscar - description of the unit cell
/// @param[in] grid - phonon spectrum on a regular q-point grid
/// @param[in] syms - symmetry operations object
/// @param[inout] threeph_procs - list of 3-phonon processes
/// @param[inout] twoph_procs - list of 2-phonon processes
/// @param[in] T - temperature in K
/// @param[in] comm - communicator used to synchronize all processes
/// @param[in] tolerance - maximum change in the norm betwwen iterations
/// used as the convergence criterion
/// @param[in] maxiter - maximum number of iterations before giving up
/// @return the thermal conductivity tensor in SI units
Eigen::MatrixXd calc_shengbte_kappa(
    const alma::Crystal_structure& poscar,
    const alma::Gamma_grid& grid,
    const alma::Symmetry_operations& syms,
    std::vector<alma::Threeph_process>& threeph_procs,
    std::vector<alma::Twoph_process>& twoph_procs,
    double T,
    const boost::mpi::communicator& comm,
    double tolerance = 1e-4,
    std::size_t maxiter = 1000);
} // namespace alma


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver 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 Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#ifndef COLLOCATIONINTEGRATOR_HPP
#define COLLOCATIONINTEGRATOR_HPP

#include <cmath>
#include <iosfwd>
#include <vector>

#include "Config.hpp"

#include <Eigen/Core>

#include "IntegratorHelperFunctions.hpp"
#include "cavity/Element.hpp"
#include "green/AnisotropicLiquid.hpp"
#include "green/IonicLiquid.hpp"
#include "green/SphericalDiffuse.hpp"
#include "green/UniformDielectric.hpp"
#include "green/Vacuum.hpp"

/*! \file CollocationIntegrator.hpp
 *  \struct CollocationIntegrator
 *  \brief Implementation of the single and double layer operators matrix representation using one-point collocation
 *  \author Roberto Di Remigio
 *  \date 2015
 *
 *  Calculates the diagonal elements of S as:
 *  \f[
 *  	S_{ii} = factor * \sqrt{\frac{4\pi}{a_i}}
 *  \f]
 *  while the diagonal elements of D are:
 *  \f[
 *  	D_{ii} = -factor * \sqrt{\frac{\pi}{a_i}} \frac{1}{R_I}
 *  \f]
 */

struct CollocationIntegrator
{
    CollocationIntegrator() : factor(1.07) {}
    CollocationIntegrator(double f) : factor(f) {}
    ~CollocationIntegrator() {}

    /**@{ Single and double layer potentials for a Vacuum Green's function by collocation */
    /*! \tparam DerivativeTraits how the derivatives of the Greens's function are calculated
     *  \param[in] gf Green's function
     *  \param[in] e  list of finite elements
     */
    template <typename DerivativeTraits>
    Eigen::MatrixXd singleLayer(const Vacuum<DerivativeTraits, CollocationIntegrator> & gf, const std::vector<Element> & e) const {
        return integrator::singleLayer(e,
                pcm::bind(integrator::SI, this->factor, 1.0, pcm::_1),
                gf.exportKernelS());
    }
    /*! \tparam DerivativeTraits how the derivatives of the Greens's function are calculated
     *  \param[in] gf Green's function
     *  \param[in] e  list of finite elements
     */
    template <typename DerivativeTraits>
    Eigen::MatrixXd doubleLayer(const Vacuum<DerivativeTraits, CollocationIntegrator> & gf, const std::vector<Element> & e) const {
        return integrator::doubleLayer(e,
                                       pcm::bind(integrator::DI, this->factor, pcm::_1),
                                       gf.exportKernelD());
    }
    /**@}*/

    /**@{ Single and double layer potentials for a UniformDielectric Green's function by collocation */
    /*! \tparam DerivativeTraits how the derivatives of the Greens's function are calculated
     *  \param[in] gf Green's function
     *  \param[in] e  list of finite elements
     */
    template <typename DerivativeTraits>
    Eigen::MatrixXd singleLayer(const UniformDielectric<DerivativeTraits, CollocationIntegrator> & gf, const std::vector<Element> & e) const {
        return integrator::singleLayer(e,
                pcm::bind(integrator::SI, this->factor, gf.epsilon(), pcm::_1),
                gf.exportKernelS());
    }
    /*! \tparam DerivativeTraits how the derivatives of the Greens's function are calculated
     *  \param[in] gf Green's function
     *  \param[in] e  list of finite elements
     */
    template <typename DerivativeTraits>
    Eigen::MatrixXd doubleLayer(const UniformDielectric<DerivativeTraits, CollocationIntegrator> & gf, const std::vector<Element> & e) const {
        return integrator::doubleLayer(e,
                                       pcm::bind(integrator::DI, this->factor, pcm::_1),
                                       gf.exportKernelD());
    }
    /**@}*/

    /**@{ Single and double layer potentials for a IonicLiquid Green's function by collocation */
    template <typename DerivativeTraits>
    Eigen::MatrixXd singleLayer(const IonicLiquid<DerivativeTraits, CollocationIntegrator> & /* gf */, const std::vector<Element> & e) const {
        PCMSOLVER_ERROR("CollocationIntegrator::singleLayer not implemented yet for IonicLiquid", BOOST_CURRENT_FUNCTION);
        return Eigen::MatrixXd::Zero(e.size(), e.size());
    }
    template <typename DerivativeTraits>
    Eigen::MatrixXd doubleLayer(const IonicLiquid<DerivativeTraits, CollocationIntegrator> & /* gf */, const std::vector<Element> & e) const {
        PCMSOLVER_ERROR("CollocationIntegrator::doubleLayer not implemented yet for IonicLiquid", BOOST_CURRENT_FUNCTION);
        return Eigen::MatrixXd::Zero(e.size(), e.size());
    }
    /**@}*/

    /**@{ Single and double layer potentials for an AnisotropicLiquid Green's function by collocation */
    template <typename DerivativeTraits>
    Eigen::MatrixXd singleLayer(const AnisotropicLiquid<DerivativeTraits, CollocationIntegrator> & /* gf */, const std::vector<Element> & e) const {
        PCMSOLVER_ERROR("CollocationIntegrator::singleLayer not implemented yet for AnisotropicLiquid", BOOST_CURRENT_FUNCTION);
        return Eigen::MatrixXd::Zero(e.size(), e.size());
    }
    template <typename DerivativeTraits>
    Eigen::MatrixXd doubleLayer(const AnisotropicLiquid<DerivativeTraits, CollocationIntegrator> & /* gf */, const std::vector<Element> & e) const {
        PCMSOLVER_ERROR("CollocationIntegrator::doubleLayer not implemented yet for AnisotropicLiquid", BOOST_CURRENT_FUNCTION);
        return Eigen::MatrixXd::Zero(e.size(), e.size());
    }
    /**@}*/

    /**@{ Single and double layer potentials for a SphericalDiffuse Green's function by collocation */
    /*! \tparam ProfilePolicy the permittivity profile for the diffuse interface
     *  \param[in] gf Green's function
     *  \param[in] e  list of finite elements
     */
    template <typename ProfilePolicy>
    Eigen::MatrixXd singleLayer(const SphericalDiffuse<CollocationIntegrator, ProfilePolicy> & gf, const std::vector<Element> & e) const {
        // The singular part is "integrated" as usual, while the nonsingular part is evaluated in full
        PCMSolverIndex mat_size = e.size();
        Eigen::MatrixXd S = Eigen::MatrixXd::Zero(mat_size, mat_size);
        for (PCMSolverIndex i = 0; i < mat_size; ++i) {
            // Fill diagonal
            // Diagonal of S inside the cavity
            double Sii_I = factor * std::sqrt(4 * M_PI / e[i].area());
            // "Diagonal" of Coulomb singularity separation coefficient
            double coulomb_coeff = gf.coefficientCoulomb(e[i].center(), e[i].center());
            // "Diagonal" of the image Green's function
            double image = gf.imagePotential(e[i].center(), e[i].center());
            S(i, i) = Sii_I / coulomb_coeff + image;
            Eigen::Vector3d source = e[i].center();
            for (PCMSolverIndex j = 0; j < mat_size; ++j) {
                // Fill off-diagonal
                Eigen::Vector3d probe = e[j].center();
                if (i != j) S(i, j) = gf.kernelS(source, probe);
            }
        }
        return S;
    }
    /*! \tparam ProfilePolicy the permittivity profile for the diffuse interface
     *  \param[in] gf Green's function
     *  \param[in] e  list of finite elements
     */
    template <typename ProfilePolicy>
    Eigen::MatrixXd doubleLayer(const SphericalDiffuse<CollocationIntegrator, ProfilePolicy> & gf, const std::vector<Element> & e) const {
        // The singular part is "integrated" as usual, while the nonsingular part is evaluated in full
        PCMSolverIndex mat_size = e.size();
        Eigen::MatrixXd D = Eigen::MatrixXd::Zero(mat_size, mat_size);
        for (PCMSolverIndex i = 0; i < mat_size; ++i) {
            // Fill diagonal
            double area = e[i].area();
            double radius = e[i].sphere().radius;
            // Diagonal of S inside the cavity
            double Sii_I = factor * std::sqrt(4 * M_PI / area);
            // Diagonal of D inside the cavity
            double Dii_I = -factor * std::sqrt(M_PI/ area) * (1.0 / radius);
            // "Diagonal" of Coulomb singularity separation coefficient
            double coulomb_coeff = gf.coefficientCoulomb(e[i].center(), e[i].center());
            // "Diagonal" of the directional derivative of the Coulomb singularity separation coefficient
            double coeff_grad = gf.coefficientCoulombDerivative(e[i].normal(), e[i].center(), e[i].center()) / std::pow(coulomb_coeff, 2);
            // "Diagonal" of the directional derivative of the image Green's function
            double image_grad = gf.imagePotentialDerivative(e[i].normal(), e[i].center(), e[i].center());

            double eps_r2 = 0.0;
            pcm::tie(eps_r2, pcm::ignore) = gf.epsilon(e[i].center());

            D(i, i) = eps_r2 * (Dii_I / coulomb_coeff - Sii_I * coeff_grad + image_grad);
            Eigen::Vector3d source = e[i].center();
            for (PCMSolverIndex j = 0; j < mat_size; ++j) {
                // Fill off-diagonal
                Eigen::Vector3d probe = e[j].center();
                Eigen::Vector3d probeNormal = e[j].normal();
                probeNormal.normalize();
                if (i != j) D(i, j) = gf.kernelD(probeNormal, source, probe);
            }
        }
        return D;
    }
    /**@}*/

    /// Scaling factor for the collocation formulas
    double factor;
};

#endif // COLLOCATIONINTEGRATOR_HPP


#pragma warning( disable: 4996 )
#include "Generic/common/leak_detection.h"

// the following is needed to get around a VC++ bug
// see http://lists.boost.org/MailArchives/ublas/2005/08/0674.php
#define BOOST_UBLAS_TYPE_CHECK 0
#include <iostream>
#include <set>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/regex.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/algorithm/string/classification.hpp>
#include "Generic/common/SessionLogger.h"
#include "Generic/common/ParamReader.h"
#include "ActiveLearning/ActiveLearningData.h"
#include "ActiveLearning/DataView.h"
#include "ActiveLearning/strategies/CoverageStrategy.h"
#include "ActiveLearning/AnnotatedFeatureRecord.h"
#include "ActiveLearning/alphabet/FeatureAlphabet.h"

using namespace Eigen;
using std::wstring;
using std::vector;
using boost::split; using boost::is_any_of;
using boost::trim;
using boost::lexical_cast;
using boost::wregex; using boost::wsmatch; using boost::regex_match;

// Coverage selection strategy
// see Druck, Settles, and McCallum. "Active Learning by Labeling Features."
// EMNLP 2009

CoverageStrategy::CoverageStrategy(ActiveLearningData_ptr al_data,
			DataView_ptr data, FeatureAlphabet_ptr alphabet) :
MaxScoreStrategy(al_data, data->nFeatures()), _data(data), _alphabet(alphabet),
	_frequencies(VectorXd::Zero(_data->nFeatures())),
	_feature_totals(_data->nInstances()), _labelled(),
	_phi(VectorXd::Zero(_data->nFeatures())), _n_instances(_data->nFeatures(), 0.0),
	_freq_modifiers(_data->nFeatures(), 1.0)
{
	SessionLogger::info("LEARNIT") << "Remember to cite Druck, Settles, and McCallum. \"Active Learning "
		<< "by Labeling Features\". EMNLP 2009." << std::endl;
	load_frequency_modifiers();

	BOOST_FOREACH(AnnotatedFeatureRecord_ptr ann, _alData->annotatedFeatures()) {
		_labelled.insert(ann->idx);
	}
	gather_feature_data();

	for (unsigned int i=0; i < _data->nFeatures(); ++i) {
		if (!labelled(i)) {
			push_queue(i, score(i));
		}
	}
}

void CoverageStrategy::load_frequency_modifiers() {
	const std::string sData =
		ParamReader::getRequiredParam("coverage_strategy_frequency_modifiers");
	std::wstring data(sData.begin(), sData.end());

	trim(data);
	wregex name_and_val(L"(\\w+)\\(([0-9.]+)\\)");
	wsmatch matches;
	std::vector<std::wstring> entries;
	split(entries, data, is_any_of(","));
	BOOST_FOREACH(const wstring& entry, entries) {
		if (regex_match(entry, matches, name_and_val)) {
			const wstring feature_class_name(matches[1].first, matches[1].second);
			double val = lexical_cast<double>(wstring(matches[2].first, matches[2].second));
			bool marked=false;
			BOOST_FOREACH(int i, _alphabet->featureIndicesOfClass(feature_class_name)) {
				_freq_modifiers[i] = val;
				marked=true;
			}
			BOOST_FOREACH(int i, _alphabet->featureIndicesOfClass(feature_class_name, true)) {
				_freq_modifiers[i] = val;
				marked=true;
			}
			if (!marked) {
				SessionLogger::warn("active_learning") << L"When setting coverage strategy "
					<< "active learning frequency modifiers, failed to find any "
					"features of class " << feature_class_name;
			}
		}
	}
}

void CoverageStrategy::gather_feature_data() {
	// we store not straight frequencies but rather log frequencies
	for (unsigned int i=0; i<_data->nFeatures(); ++i) {
		double freq = _data->frequency(i) * _freq_modifiers[i];
		if (freq > 0.0) {
			_frequencies(i) = log(freq);
		} else {
			_frequencies(i) = -1000000.0;
		}
	}

	// feature_totals the total number of times each instance is seen with
	// an annotated feature
	BOOST_FOREACH(int feat, _labelled) {
		_feature_totals += _data->instancesWithFeature(feat);
	}

	// store number of instances each feature appears in
	for (unsigned int k=0; k<_data->nFeatures(); ++k) {
		_n_instances[k] = _data->instancesWithFeature(k).nonZeros();
	}

	// phi_k is the dot product of feature_totals with the instance vector
	// for feature k. This is the sum over the number of shared instances
	// of feature k with each annotated feature.  So if instance x is shared
	// by feature k with three annotated features, it will be counted
	// three times.
	for (unsigned int k=0; k<_data->nFeatures(); ++k) {
		_phi(k) = _data->instancesWithFeature(k).dot(_feature_totals);
	}
}

double CoverageStrategy::score(int k) {
	if (_labelled.size() == 0) {
		return _frequencies(k);
	} else {
		if (_n_instances[k] > 0) {
			return _frequencies(k) * (1.0 - _phi(k)/(_labelled.size()*_n_instances[k]));
		} else {
			return -10000.0;
		}
	}
}

void CoverageStrategy::label(int l) {
}

bool CoverageStrategy::labelled(int feat) const {
	return _labelled.find(feat) != _labelled.end();
}

void CoverageStrategy::registerPop(int l) {
	// this can be made more efficient if needed, though it may
	// require implementing a custom priority queue
	_labelled.insert(l);

	std::set<int> dirty;

	for (SparseVector<double>::InnerIterator inst_it(_data->instancesWithFeature(l)); inst_it; ++inst_it) {
		int inst = inst_it.index();
		for (SparseVector<double>::InnerIterator it(_data->features(inst)); it; ++it) {
			dirty.insert(it.index());
		}
	}

	BOOST_FOREACH(int feat, dirty) {
		_phi(feat) += _data->instancesWithFeature(feat).dot(_data->instancesWithFeature(l));
	}

	for (unsigned int feat = 0; feat < _data->nFeatures(); ++feat) {
		if (!labelled(feat)) {
			update_queue(feat, score(feat));
		}
	}
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2001, 2002, 2003 Sadruddin Rejeb
 Copyright (C) 2006 Cristina Duminuco
 Copyright (C) 2006 Marco Bianchetti
 Copyright (C) 2007 StatPro Italia srl
 Copyright (C) 2014 Ferdinando Ametrano
 Copyright (C) 2016 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

#include <ql/instruments/swaption.hpp>
#include <ql/pricingengines/swaption/blackswaptionengine.hpp>
#include <ql/math/solvers1d/newtonsafe.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/exercise.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

    namespace {

        class ImpliedSwaptionVolHelper {
          public:
            ImpliedSwaptionVolHelper(const Swaption&,
                                     const Handle<YieldTermStructure>& discountCurve,
                                     Real targetValue,
                                     Real displacement,
                                     VolatilityType type);
            Real operator()(Volatility x) const;
            Real derivative(Volatility x) const;
          private:
            boost::shared_ptr<PricingEngine> engine_;
            Handle<YieldTermStructure> discountCurve_;
            Real targetValue_;
            boost::shared_ptr<SimpleQuote> vol_;
            const Instrument::results* results_;
        };

        ImpliedSwaptionVolHelper::ImpliedSwaptionVolHelper(
                              const Swaption& swaption,
                              const Handle<YieldTermStructure>& discountCurve,
                              Real targetValue,
                              Real displacement,
                              VolatilityType type)
        : discountCurve_(discountCurve), targetValue_(targetValue) {

            // set an implausible value, so that calculation is forced
            // at first ImpliedSwaptionVolHelper::operator()(Volatility x) call
            vol_ = boost::shared_ptr<SimpleQuote>(new SimpleQuote(-1.0));
            Handle<Quote> h(vol_);

            switch (type) {
            case ShiftedLognormal:
                engine_ = boost::make_shared<BlackSwaptionEngine>(
                    discountCurve_, h, Actual365Fixed(), displacement);
                break;
            case Normal:
                engine_ = boost::make_shared<BachelierSwaptionEngine>(
                    discountCurve_, h, Actual365Fixed());
                break;
            default:
                QL_FAIL("unknown VolatilityType (" << type << ")");
                break;
            }
            swaption.setupArguments(engine_->getArguments());
            results_ = dynamic_cast<const Instrument::results *>(
                engine_->getResults());
        }

        Real ImpliedSwaptionVolHelper::operator()(Volatility x) const {
            if (x!=vol_->value()) {
                vol_->setValue(x);
                engine_->calculate();
            }
            return results_->value-targetValue_;
        }

        Real ImpliedSwaptionVolHelper::derivative(Volatility x) const {
            if (x!=vol_->value()) {
                vol_->setValue(x);
                engine_->calculate();
            }
            std::map<std::string,boost::any>::const_iterator vega_ =
                results_->additionalResults.find("vega");
            QL_REQUIRE(vega_ != results_->additionalResults.end(),
                       "vega not provided");
            return boost::any_cast<Real>(vega_->second);
        }
    }

    std::ostream& operator<<(std::ostream& out,
                             Settlement::Type t) {
        switch (t) {
          case Settlement::Physical:
            return out << "Delivery";
          case Settlement::Cash:
            return out << "Cash";
          default:
            QL_FAIL("unknown Settlement::Type(" << Integer(t) << ")");
        }
    }

    Swaption::Swaption(const boost::shared_ptr<VanillaSwap>& swap,
                       const boost::shared_ptr<Exercise>& exercise,
                       Settlement::Type delivery)
    : Option(boost::shared_ptr<Payoff>(), exercise), swap_(swap),
      settlementType_(delivery) {
        registerWith(swap_);
        registerWithObservables(swap_);
    }

    bool Swaption::isExpired() const {
        return detail::simple_event(exercise_->dates().back()).hasOccurred();
    }

    void Swaption::setupArguments(PricingEngine::arguments* args) const {

        swap_->setupArguments(args);

        Swaption::arguments* arguments =
            dynamic_cast<Swaption::arguments*>(args);

        QL_REQUIRE(arguments != 0, "wrong argument type");

        arguments->swap = swap_;
        arguments->settlementType = settlementType_;
        arguments->exercise = exercise_;
    }

    void Swaption::arguments::validate() const {
        VanillaSwap::arguments::validate();
        QL_REQUIRE(swap, "vanilla swap not set");
        QL_REQUIRE(exercise, "exercise not set");
    }

    Volatility Swaption::impliedVolatility(Real targetValue,
                                           const Handle<YieldTermStructure>& d,
                                           Volatility guess,
                                           Real accuracy,
                                           Natural maxEvaluations,
                                           Volatility minVol,
                                           Volatility maxVol,
                                           VolatilityType type,
                                           Real displacement) const {
        //calculate();
        QL_REQUIRE(!isExpired(), "instrument expired");

        ImpliedSwaptionVolHelper f(*this, d, targetValue, displacement, type);
        //Brent solver;
        NewtonSafe solver;
        solver.setMaxEvaluations(maxEvaluations);
        return solver.solve(f, accuracy, guess, minVol, maxVol);
    }

    Volatility Swaption::impliedVolatility(Real targetValue,
                                           const Handle<YieldTermStructure>& d,
                                           Volatility guess,
                                           Real accuracy,
                                           Natural maxEvaluations,
                                           Volatility minVol,
                                           Volatility maxVol,
                                           Real displacement,
                                           VolatilityType type) const {
        return impliedVolatility(targetValue, d, guess, accuracy,
                                 maxEvaluations, minVol, maxVol,
                                 type, displacement);
    }

}


#include <iostream>
#include <fstream>
#include <memory>
#include <thread>
#include <vector>

#include <Eigen/Eigen>

#include "ouster/client.h"
#include "ouster/lidar_scan.h"
#include "ouster/packet.h"
#include "ouster/types.h"

namespace sensor = ouster::sensor;

// PLY file implementaiton (see below)
void ply_write_header(std::ostream& os, int W, int H, int frame_count);
void ply_write_pixel(std::ostream& os,
		     Eigen::Array<double, Eigen::Dynamic, 3> xyz,
		     Eigen::Array<ouster::LidarScan::raw_t, Eigen::Dynamic, 4> pix);

/*
  With the exception of PLY file construction, the code below illustrates how to
  use the ouster_client API to construct 3D points from a live sensor.
 */
int
main(int argc, char** argv)
{
  if(argc < 4) {
    std::cout << "usage: " << argv[0]
	      << " <sensor hostname> <udp receiver> <points.ply> <frame_count [min 5 or -1 for infinite]>"
	      << std::endl;
    return 0;
  }

  // Connect to the sensor given hostname and receiver IP
  auto client = sensor::init_client(argv[1], argv[2]);

  if(!client) {
    std::cerr << "error: client failed to initialize" << std::endl;
    return -1;
  }

  // Query the sensor for config information
  auto metadata = sensor::get_metadata(*client);
  auto info = sensor::parse_metadata(metadata);
  auto pf = sensor::get_format(info);

  // Grab specific sensor config parameters
  int W = info.format.columns_per_frame;
  int H = info.format.pixels_per_column;

  // Allocate a buffer for a single spin
  ouster::LidarScan ls(W, H);
  auto xyz_lut = ouster::make_xyz_lut(info);

  // Create the PLY file to store data
  std::ofstream ply_file(argv[3]);

  if(!ply_file.is_open()) {
    std::cerr << "error: can't write to " << argv[3] << std::endl;
    return -1;
  }

  // Write the PLY file header
  int frame_count = std::atoi(argv[4]);
  ply_write_header(ply_file, W, H, frame_count);

  // handle_single_scan is called for each full revolation of the sensor
  bool running = true;

  auto handle_single_scan =
    [&](std::chrono::nanoseconds scan_ts) mutable
    {
      std::cout << "[" << scan_ts.count() << "] new scan" << std::endl;

      // Convert the sensor image to 3D points
      auto points = ouster::cartesian(ls, xyz_lut);

      // Write each point and pixel data to the PLY file, done by row and column
      // for illutrattive purposes
      for (auto u = 0; u < ls.h; u++) {
	for (auto v = 0; v < ls.w; v++) {

	  // 'points' and 'data' are containers for accessing point and point
	  // property data
	  const auto xyz = points.row(u * ls.w + v);
	  const auto pix = ls.data.row(u * ls.w + v);

	  // Do the actual write to the PLY file
	  ply_write_pixel(ply_file, xyz, pix);
	}
      }

      // Flush everything to disk
      ply_file.flush();

      if(--frame_count == 0) {
	running = false;
      }
    };

  // Convert handle_single_scan function to a function that can process
  // lidar packets from the sensor
  auto batch_and_update = sensor::batch_to_scan(W, pf, handle_single_scan);

  uint8_t lidar_buf[pf.lidar_packet_size + 1];
  uint8_t imu_buf[pf.lidar_packet_size + 1];

  // Mainloop for processing data packets from the sensor
  while (running) {
    // Poll the client for data and add to our lidar scan
    sensor::client_state st = sensor::poll_client(*client);

    if(st & sensor::client_state::CLIENT_ERROR) break;

    // It's a lidar packet, add it to the partial scan
    if (st & sensor::client_state::LIDAR_DATA
	&& sensor::read_lidar_packet(*client, lidar_buf, pf)) {
      batch_and_update(lidar_buf, ls);
    }

    // It's an IMU packet, ignore for now.
    if (st & sensor::client_state::IMU_DATA) {
      sensor::read_imu_packet(*client, imu_buf, pf);
    }
  }

  // close the PLY file
  ply_file.close();

  return -1;
}

/*
  The following are helper functions that format the PLY file.
 */

// Write a PLY file header
void ply_write_header(std::ostream& os, int W, int H, int frame_count)
{
  auto pixel_count = W * H * frame_count;

  os << "ply" << std::endl
     << "format binary_little_endian 1.0" << std::endl
     << "comment Created by simple.cpp from Ouster (c) 2020" << std::endl
     << "element vertex " << pixel_count << std::endl
     << "property float x" << std::endl
     << "property float y" << std::endl
     << "property float z" << std::endl
     << "property uint intensity" << std::endl
     << "property uint reflectivity" << std::endl
     << "property uint noise" << std::endl
     << "end_header" << std::endl
    ;
}

// Write a PLY file pixel
void ply_write_pixel(std::ostream& os,
		     Eigen::Array<double, Eigen::Dynamic, 3> xyz,
		     Eigen::Array<ouster::LidarScan::raw_t, Eigen::Dynamic, 4> pix)
{
  float val;

  for(int i = 0; i < 3; ++i) {
    val = xyz(i);
    os.write((char*)&val, sizeof(val));
  }

  // Index the 'pix' array
  std::vector< ouster::LidarScan::LidarScanIndex > pix_types =
    {
     ouster::LidarScan::INTENSITY,
     ouster::LidarScan::REFLECTIVITY,
     ouster::LidarScan::NOISE,
    };

  std::uint32_t t_val;

  for(auto t : pix_types) {
    t_val = pix(t);
    os.write((char*)&t_val, sizeof(t_val));
  }
}

C++ so lib for OSClient_Only 18.04 Support/simple.cpp


/*
 * File: inet.hpp
 * Project: nets
 * File Created: Monday, 17th January 2022 11:48:11 am
 * Author: Xu En (xuen@mokar.com)
 * -----
 * Last Modified: Sunday, 20th March 2022 1:52:28 pm
 * Modified By: Xu En (xuen@mokahr.com)
 * -----
 * Copyright 2021 - 2022 Your Company, Moka
 */
#ifndef SRC_NETS_INET_HPP_
#define SRC_NETS_INET_HPP_
#include <Eigen/Dense>

class Linear {
   private:
    Eigen::MatrixXf w;
    Eigen::VectorXf b;

   public:

    Eigen::Matrix call(Eigen::Matrix input) {
        return input * w + b; }
}

#endif  // SRC_NETS_INET_HPP_


//==================================================================================================
/**
  Copyright 2016 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_ARCH_COMMON_SIMD_FUNCTION_ABS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ABS_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/traits.hpp>
#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/bitwise_notand.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/detail/constant/maxleftshift.hpp>
#include <boost/simd/constant/mzero.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  //------------------------------------------------------------------------------------------------
  // abs on signed values
  BOOST_DISPATCH_OVERLOAD_IF( abs_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_<bd::signed_<A0>, X>
                            )
   {
      BOOST_FORCEINLINE A0 operator()(const A0& a0) const BOOST_NOEXCEPT
      {
        A0 const s = shift_right(a0, Maxleftshift<typename A0::value_type>());
        return (a0+s)^s;
      }
   };

  //------------------------------------------------------------------------------------------------
  // abs on unsigned values is identity
  BOOST_DISPATCH_OVERLOAD( abs_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::unsigned_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(const A0& a0) const BOOST_NOEXCEPT
      {
        return a0;
      }
   };

  //------------------------------------------------------------------------------------------------
  // abs on real values cleans the sign bit
  BOOST_DISPATCH_OVERLOAD_IF( abs_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_<bd::floating_<A0>, X>
                            )
  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
    {
      return bitwise_notand(Mzero<A0>(),a0);
    }
  };

} } }

#endif


/* Copyright 2019 Ignacio Torroba (torroba@kth.se)
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the copyright holder 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.
 */

#ifndef UTILS_HPP
#define UTILS_HPP

#include <fstream>
#include <iostream>

#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/registration/icp.h>

#include <Eigen/Geometry>
#include <Eigen/Core>

using namespace std;
typedef pcl::PointCloud<pcl::PointXYZ> PointCloudT;
typedef pcl::PointXYZ PointT;

struct submap {
public:
    int submap_id;
    PointCloudT submap_pcl;
    Eigen::Matrix4f tf_submap_map;
    std::vector<Eigen::VectorXf, Eigen::aligned_allocator<Eigen::VectorXf>> auv_pose_;   // AUV pose for each ping
    std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f>> pcl_covariances;
    Eigen::MatrixXf cov_submap_frame;   // 6x6 cov of submap frame
    unsigned int pings_num_;
    unsigned int beams_per_ping_;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct icp_match{
public:

    icp_match(PointT trg_point, PointT src_point, Eigen::Vector3f normal, Eigen::Vector3f error_mean, Eigen::Matrix3f error_sigma){
        trg_point_ = trg_point;
        src_point_ = src_point;
        normal_ = normal;
        error_mean_ = error_mean;
        error_sigma_ = error_sigma;
    }

    PointT trg_point_;
    PointT src_point_;
    Eigen::Vector3f normal_;
    // Components of error pdf
    Eigen::Vector3f error_mean_;
    Eigen::Matrix3f error_sigma_;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

void subsampleMap(submap& submap_input);

void outlierFilter(submap& submap_input);

Eigen::Vector3f computePCAPcl(PointCloudT& set_Ai);

Eigen::Affine3d create_rotation_matrix(double ax, double ay, double az);

Eigen::Matrix4f inverseTfMatrix(Eigen::Matrix4f tf_mat);

void plotSubmapsSet(const std::vector<submap, Eigen::aligned_allocator<submap> > &submaps_set);

void print4x4Matrix (const Eigen::Matrix4f & matrix);

#endif // UTILS_HPP


/**
 * @file OccupancyGridMap.hpp
 * @author bwu
 * @brief Grid map define and generation algorithm
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_GEOMETRY_OCCUPANCYGRIDMAP_HPP
#define GENERIC_GEOMETRY_OCCUPANCYGRIDMAP_HPP
#include "generic/thread/ThreadPool.hpp"
#include "generic/math/MathUtility.hpp"
#include "generic/common/Exception.hpp"
#include "generic/tools/Tools.hpp"
#include "BooleanOperation.hpp"
#include "Triangulation.hpp"
#include "Rasterization.hpp"
#include "Triangulator.hpp"
#include "Geometries.hpp"
#include "GeometryIO.hpp"
#include "Trapezoid.hpp"
#include "Utility.hpp"
#include <boost/lockfree/queue.hpp>
#include <vector>
#include <atomic>

#ifdef BOOST_GIL_IO_PNG_SUPPORT
#include "generic/tools/FileSystem.hpp"
#include "generic/tools/Color.hpp"
#include <boost/gil/extension/io/png.hpp>
#include <boost/gil.hpp>
#include <png.h>
#endif

namespace generic {
namespace geometry {

/**
 * @brief represents a 2d dense grid map with Occupancy objects
 *
 * @tparam Occupancy occupancy object type, should be trivial
 */
template <typename Occupancy>
class OccupancyGridMap
{
    using Container = std::vector<std::vector<Occupancy> >;
public:
    using ResultType = Occupancy;
    OccupancyGridMap(size_t width, size_t height)
     : m_width(width),
       m_height(height),
       m_grids(width, std::vector<Occupancy>(height, Occupancy{}))
    {}
    ~OccupancyGridMap() = default;

    ///@brief accesses Occupancy reference by 1d index `i`, `i` = x * height + y
    Occupancy & operator[] (size_t i) { return m_grids[i / m_height][i % m_height]; }
    const Occupancy & operator[](size_t i) const { return m_grids[i / m_height][i % m_height]; }

    ///@brief  accesses Occupancy reference by 2d index `x`, `y`
    Occupancy & operator() (size_t x, size_t y) { return m_grids[x][y]; }
    const Occupancy & operator() (size_t x, size_t y) const { return m_grids[x][y]; }

    ///@brief resizes grid map with `width` and `height`
    void Resize(size_t width, size_t height)
    {
        m_width = width;
        m_height = height;
        m_grids.resize(width);
        for(auto & col : m_grids)
            col.resize(height);
    }

    ///@brief resets all occupancy values in grid map with default data
    void Reset()
    {
        for(auto & col : m_grids){
            std::fill(col.begin(), col.end(), Occupancy{});
        }
    }

    ///@brief clears all occupancy data
    void Clear() { m_grids.clear(); }

    ///@brief returns grid map width
    size_t Width() const { return m_width; }

    ///@brief returns grid map height
    size_t Height() const { return m_height; }

    ///@brief returns grid map size
    size_t Size() const { return Width() * Height(); }

    /**
     * @brief gets max occupancy object with the compare functioner
     *
     * @tparam Compare the compare functioner
     * @param cmp the Compare functioner object
     * @return const reference of Occupancy object
     */
    template <typename Compare>
    const Occupancy & MaxOccupancy(Compare && cmp) const
    {
        auto [i, j] = MaxElement(std::forward<Compare>(cmp));
        return m_grids[i][j];
    }

    /**
     * @brief gets x, y index of max occupancy object with the compare functioner
     *
     * @tparam Compare the compare functioner
     * @param cmp the Compare functioner object
     * @return x, y index of max ocupancy object
     */
    template <typename Compare>
    std::pair<size_t, size_t> MaxElement(Compare && cmp) const
    {
        size_t xMax(0), yMax(0);
        for(size_t i = 0; i < m_width; ++i){
            for(size_t j = 0; j < m_height; ++j){
                if(cmp(m_grids[i][j], m_grids[xMax][yMax])){
                    xMax = i; yMax = j;
                }
            }
        }
        return std::make_pair(xMax, yMax);
    }

#ifdef BOOST_GIL_IO_PNG_SUPPORT
    /**
     * @brief converts grid map to png image with the color mapping functioner
     *
     * @tparam RGBaFunc rgba functioner that take Occupancy as input and return r, g, b, a values range from 0 to 255
     * @param[in] filename output image file path
     * @param[in] rgbaFunc RGBaFunc functioner object
     * @return whether write the image file successfully
     */
    template <typename RGBaFunc>
    bool WriteImgProfile(const std::string & filename, RGBaFunc && rgbaFunc)
    {
        auto dir = generic::filesystem::DirName(filename);
        if(!generic::filesystem::PathExists(dir))
            generic::filesystem::CreateDir(dir);

        using namespace boost::gil;
        rgb8_image_t img(m_width, m_height);
        rgb8_image_t::view_t v = view(img);
        for(auto i = 0; i < m_width; ++i){
            for(auto j = 0; j < m_height; ++j){
                auto [r, g, b, a] = rgbaFunc(m_grids[i][j]);
                v(i, m_height - j - 1) = rgba8_pixel_t(r, g, b, a);
            }
        }
        write_view(filename, view(img), png_tag());
        return true;
    }
#endif

private:
    size_t m_width;
    size_t m_height;
    Container m_grids;
};

///@brief represents a factory class that calculate occupancy grid map from input geometries
class OccupancyGridMappingFactory
{
public:
    ///@brief grid map data type
    template <typename Occupancy>
    using GridMap = OccupancyGridMap<Occupancy>;

    /**
     * @brief grid mapping factory product
     *
     * @tparam property_type user defined property
     * @param x grid x index
     * @param y grid y index
     * @param property user defined property mark
     * @param ratio the proportion that the piece of the geometry ocuupying the grid area
     */
    template <typename property_type>
    struct Product { int x, y; property_type property; double ratio; };

    ///@brief a lockfree queue that hold and consume the grid mapping product
    template <typename property_type>
    using ProductPipe = boost::lockfree::queue<Product<property_type>, boost::lockfree::fixed_sized<false> >;

    template <typename num_type>
    struct GridCtrl
    {
        size_t threads = 1;
        Box2D<num_type> bbox;
        Point2D<num_type> ref;
        Vector2D<num_type> stride;

        GridCtrl(const Box2D<num_type> & _bbox, const Vector2D<num_type> & _stride, size_t _threads = 1)
         : GridCtrl(_bbox, _bbox[0], _stride, _threads) {}

        GridCtrl(const Box2D<num_type> & _bbox, const Point2D<num_type> & _ref, const Vector2D<num_type> & _stride, size_t _threads = 1)
         : threads(_threads), bbox(_bbox), ref(_ref), stride(_stride) {}
    };

    template <typename num_type>
    struct GridWorkItem
    {
        size_t begin, end;
        GridCtrl<num_type> ctrl;
    };

    /**
     * @brief function that calculate grid map size from bounding box and x, y stride
     * @param[in] bbox input bounding box
     * @param[in] stride grid width and length in x, y direction
     * @return std::pair<size_t, size_t> x, y size of the grid map
     */
    template <typename num_type>
    static std::pair<size_t, size_t> GetGridMapSize(const Box2D<num_type> & bbox, const Vector2D<num_type> & stride)
    {
        using float_t = common::float_type<num_type>;
        size_t x = static_cast<size_t>(std::ceil(float_t(bbox.Length()) / stride[0]));
        size_t y = static_cast<size_t>(std::ceil(float_t(bbox.Width() ) / stride[1]));
        return std::make_pair(x, y);
    }

    /**
     * @brief mapping the property of geometries to grid based on occupying area proportion
     *
     * @tparam property_type geometry property type, for call back function when mapping the result
     * @tparam Object input object type used for generating grid map
     * @tparam GeomGetter functor tpye to convert user input object type to internal geometry type
     * @note the GeomGetter functor should have operator() (const & T) function, the return type could be one of Box2D or Polygon2D
     * @tparam Occupancy occupancy object type, should be trivial
     * @tparam BlendFunc blend function type, used for calculating the occupying area proportion, for example, blend function usually different with solid and hole geometry
     * @param[in] properties the properties of each input object
     * @param[in] objects input objects
     * @param[in] getter GeomGetter object
     * @param[in] ctrl grid map generation ctrl parameters
     * @param[out] gridMap output grid map that hold the the data of `Occupancy` each grid
     * @param[in] blend BlendFunc object
     */
    template <typename property_type, typename Object, typename GeomGetter, typename Occupancy, typename BlendFunc,
              typename std::enable_if<traits::is_2d_geometry_t<typename std::result_of<GeomGetter(const Object&)>::type>::value &&
                                     (traits::is_polygon_t<typename std::result_of<GeomGetter(const Object&)>::type>::value ||
                                      traits::is_box_t<typename std::result_of<GeomGetter(const Object&)>::type>::value), bool>::type = true>
    static void Map2Grid(const std::vector<property_type> & properties, const std::vector<Object> & objects, GeomGetter && getter,
                         const GridCtrl<typename std::result_of<GeomGetter(const Object&)>::type::coor_t> & ctrl, GridMap<Occupancy> & gridMap, BlendFunc && blend)
    {
        static_assert(std::is_trivial<Product<property_type> >::value, "only trivial property supported!");
        GENERIC_ASSERT(properties.size() == objects.size())

        using geom_t = typename std::result_of<GeomGetter(const Object&)>::type;
        using coor_t = typename geom_t::coor_t;
        std::vector<geom_t> geometries;
        geometries.reserve(objects.size());
        for(const auto & object : objects){
            geometries.push_back(getter(object));
        }

        std::vector<const geom_t *> geomPtrs;
        geomPtrs.reserve(geometries.size());
        for(const auto & geom : geometries) geomPtrs.push_back(&geom);
        Map2Grid<property_type, geom_t, Occupancy, BlendFunc>(properties, geomPtrs, ctrl, gridMap, std::forward<BlendFunc>(blend));
    }

    ///@brief Map2Grid implementation for internal geometry_type
    ///@note the geometry_type should be one of Box2D or Polygon2D
    template <typename property_type, typename geometry_type, typename Occupancy, typename BlendFunc>
    static void Map2Grid(const std::vector<property_type> & properties, const std::vector<const geometry_type * > & geometries, const GridCtrl<typename geometry_type::coor_t> & ctrl, GridMap<Occupancy> & gridMap, BlendFunc && blend)
    {
        static_assert(std::is_trivial<Product<property_type> >::value, "only trivial property supported!");
        GENERIC_ASSERT(properties.size() == geometries.size())

        std::atomic_bool done{false};
        auto pipeline = std::make_unique<ProductPipe<property_type> >(32767);

        // For single-thread
        // Gridding<property_type, geometry_type>(properties, geometries, ctrl, *pipeline);
        // done.store(true);
        // Mapping<property_type, Occupancy, BlendFunc>(*pipeline, done, gridMap, std::forward<BlendFunc>(blend));

        std::thread gridding(&OccupancyGridMappingFactory::Gridding<property_type, geometry_type>, std::ref(properties), std::ref(geometries), std::ref(ctrl), std::ref(*pipeline));
        std::thread mapping(&OccupancyGridMappingFactory::Mapping<property_type, Occupancy, BlendFunc>, std::ref(*pipeline), std::ref(done), std::ref(gridMap), std::ref(blend));

        gridding.join();
        done.store(true);
        mapping.join();
    }

private:
    template <typename property_type, typename Occupancy, typename BlendFunc>
    static void Mapping(ProductPipe<property_type> & pipeline, std::atomic_bool & done, GridMap<Occupancy> & gridMap, BlendFunc && blend)
    {
        using Result = Product<property_type>;
        auto width = static_cast<int>(gridMap.Width());
        auto height = static_cast<int>(gridMap.Height());
        auto mapping = [&](const Result & res)
        {
            if(res.x < 0 || res.x >= width) return;
            if(res.y < 0 || res.y >= height) return;
            auto & origin = gridMap(res.x, res.y);
            blend(origin, res);
        };

        while(!done.load()){
            while(pipeline.consume_one(mapping)){}
        }
        pipeline.consume_all(mapping);
    }

    template <typename property_type, typename geometry_type>
    static void Gridding(const std::vector<property_type> & properties, const std::vector<const geometry_type * > & geometries, const GridCtrl<typename geometry_type::coor_t> & ctrl, ProductPipe<property_type> & pipeline)
    {
        thread::ThreadPool pool(ctrl.threads);
        size_t size = geometries.size();
        size_t blocks = pool.Threads();
        size_t blockSize = size / blocks;

        size_t begin = 0;
        using num_type = typename geometry_type::coor_t;
        for(size_t i = 0; i < blocks && blockSize > 0; ++i){
            size_t end = begin + blockSize;
            pool.Submit(std::bind(&OccupancyGridMappingFactory::GriddingImp<property_type, geometry_type>, std::ref(properties), std::ref(geometries), GridWorkItem<num_type>{begin, end, ctrl}, std::ref(pipeline)));
            begin = end;
        }
        size_t end = size;
        if(begin != end)
            pool.Submit(std::bind(&OccupancyGridMappingFactory::GriddingImp<property_type, geometry_type>, std::ref(properties), std::ref(geometries), GridWorkItem<num_type>{begin, end, ctrl}, std::ref(pipeline)));

        pool.Wait();
    }

    template <typename property_type, typename geometry_type,
              typename std::enable_if<traits::is_2d_geometry_t<geometry_type>::value && traits::is_polygon_t<geometry_type>::value, bool>::type = true>
    static void GriddingImp(const std::vector<property_type> & properties, const std::vector<const geometry_type * > & polygons, GridWorkItem<typename geometry_type::coor_t> workItem, ProductPipe<property_type> & pipeline)
    {
        //TriangulationGriddingImp<property_type, geometry_type>(properties, polygons, workItem, pipeline);
        TrapezoidationGriddingImp<property_type, typename geometry_type::coor_t>(properties, polygons, workItem, pipeline);
    }

    template <typename property_type, typename geometry_type,
              typename std::enable_if<traits::is_2d_geometry_t<geometry_type>::value && traits::is_box_t<geometry_type>::value, bool>::type = true>
    static void GriddingImp(const std::vector<property_type> & properties, const std::vector<const geometry_type * > & boxes, GridWorkItem<typename geometry_type::coor_t> workItem, ProductPipe<property_type> & pipeline)
    {
        using num_type = typename geometry_type::coor_t;
        for(size_t i = workItem.begin; i < workItem.end; ++i){
            const auto & property = properties[i];
            const auto & box = boxes[i];
            if(nullptr == box) continue;

            GriddingRectangle<property_type, num_type>(property, *box, workItem.ctrl, pipeline);
        }
    }

    template <typename property_type, typename num_type>
    static void TriangulationGriddingImp(const std::vector<property_type> & properties, const std::vector<const Polygon2D<num_type> * > & polygons, GridWorkItem<num_type> workItem, ProductPipe<property_type> & pipeline)
    {
        for(size_t i = workItem.begin; i < workItem.end; ++i){
            const auto & property = properties[i];
            const auto & polygon = polygons[i];
            if(nullptr == polygon) continue;

            tri::Triangulation<Point2D<num_type> > triangulation;
            GENERIC_ASSERT(TriangulationPolygon(polygon, triangulation))

            GriddingTriangulation<property_type, num_type>(property, triangulation, workItem.ctrl, pipeline);
        }
    }

    template <typename property_type, typename num_type>
    static void TrapezoidationGriddingImp(const std::vector<property_type> & properties, const std::vector<const Polygon2D<num_type> * > & polygons, GridWorkItem<num_type> workItem, ProductPipe<property_type> & pipeline,  const Orientation2D o = Orientation2D::Vertical)
    {
        for(size_t i = workItem.begin; i < workItem.end; ++i){
            const auto & property = properties[i];
            const auto & polygon = polygons[i];
            if(nullptr == polygon) continue;

            std::vector<Polygon2D<num_type> > trapezoids;
            TrapezoidationPolygon(polygon, trapezoids, o);

            GriddingTrapezoidation<property_type, num_type>(property, trapezoids, o, workItem.ctrl, pipeline);
        }
    }

    template <typename num_type>
    static bool TrapezoidationPolygon(const Polygon2D<num_type> * polygon, std::vector<Polygon2D<num_type> > & trapezoids, const Orientation2D o)
    {
        using namespace boost::polygon;
        trapezoids.clear();
        boolean::PolygonSet2D<num_type> ps;
        ps.insert(*polygon);
        ps.get_trapezoids(trapezoids, (o == Orientation2D::Horizontal) ? orientation_2d_enum::HORIZONTAL : orientation_2d_enum::VERTICAL);
    }

    template <typename num_type>
    static bool TriangulationPolygon(const Polygon2D<num_type> * polygon, tri::Triangulation<Point2D<num_type> > & triangulation)
    {
        using Point = Point2D<num_type>;
        using Edge = tri::IndexEdge;
        using Triangulator = tri::Triangulator2D<num_type>;
        if(nullptr == polygon) return false;

        std::list<Edge> edges;
        size_t size = polygon->Size();
        for(size_t i = 0; i < size; ++i)
            edges.push_back(Edge{i, (i + 1) % size});

        auto points = polygon->GetPoints();
        if constexpr (std::is_integral<num_type>::value){
            tri::RemoveDuplicatesAndRemapEdges(points, edges, num_type(2));
        }

        triangulation.Clear();
        Triangulator triangulator(triangulation);
        try {
            triangulator.InsertVertices(points.begin(), points.end(), [](const Point & p){ return p[0]; }, [](const Point & p){ return p[1]; });
            triangulator.InsertEdges(edges.begin(), edges.end(), [](const Edge & e){ return e.v1(); }, [](const Edge & e){ return e.v2(); });
            triangulator.EraseOuterTriangles();
        }
        catch ( ... ){
            return false;
        }

        return true;
    }

    template <typename property_type, typename num_type>
    static void GriddingTriangulation(const property_type & property, const tri::Triangulation<Point2D<num_type> > & triangulation, const GridCtrl<num_type> & ctrl, ProductPipe<property_type> & pipeline)
    {
        using Utility = tri::TriangulationUtility<Point2D<num_type> >;
        for(auto it = 0; it < triangulation.triangles.size(); ++it){
            auto triangle = Utility::GetTriangle(triangulation, it);
            GriddingTriangle<property_type, num_type>(property, triangle, ctrl, pipeline);
        }
    }

    template <typename property_type, typename num_type>
    static void GriddingTrapezoidation(const property_type & property, std::vector<Polygon2D<num_type> > & trapezoidation, const Orientation2D o, const GridCtrl<num_type> & ctrl, ProductPipe<property_type> & pipeline)
    {
        for(auto & trapezoid : trapezoidation){
            if(trapezoid.Front() == trapezoid.Back()) trapezoid.PopBack();
            GriddingTrapezoid<property_type, num_type>(property, trapezoid, o, ctrl, pipeline);
        }
    }

    template <typename property_type, typename num_type>
    static void GriddingTrapezoid(const property_type & property, const Polygon2D<num_type> & trapezoid, const Orientation2D o, const GridCtrl<num_type> & ctrl, ProductPipe<property_type> & pipeline)
    {
        std::list<Box2D<num_type> > rects;
        std::list<Triangle2D<num_type> > triangles;
        DecomposeTrapezoid(trapezoid, o, rects, triangles);

        for(const auto & rect : rects)
            GriddingRectangle<property_type, num_type>(property, rect, ctrl, pipeline);

        for(const auto & triangle : triangles)
            GriddingTriangle<property_type, num_type>(property, triangle, ctrl, pipeline);
    }

    template <typename num_type>
    static void DecomposeTrapezoid(const Polygon2D<num_type> & trapezoid, const Orientation2D o, std::list<Box2D<num_type> > & rects, std::list<Triangle2D<num_type> > & triangles)
    {
        rects.clear();
        triangles.clear();
        auto size = trapezoid.Size();
        if(3 == size){
            triangles.push_back({trapezoid[0], trapezoid[1], trapezoid[2]});
            return;
        }
        else if(4 == size){
            bool res;
            auto points = std::array<Point2D<num_type>, 4>{trapezoid[0], trapezoid[1], trapezoid[2], trapezoid[3]};
            auto t = toTrapezoid(points, o, res);
            GENERIC_ASSERT(res);

            DecomposeTrapezoid(t, rects, triangles);
            return;
        }
        else {
            auto polygon = trapezoid;
            Polygon2D<num_type>::Clean(polygon);
            GENERIC_ASSERT(polygon.Size() != trapezoid.Size())
            DecomposeTrapezoid(polygon, o, rects, triangles);
        }
    }

    template <typename num_type>
    static void DecomposeTrapezoid(const Trapezoid<num_type> & trapezoid, std::list<Box2D<num_type> > & rects, std::list<Triangle2D<num_type> > & triangles)
    {
        GENERIC_ASSERT(trapezoid.isValid())
        Point2D<num_type> p[4] = { trapezoid[0], trapezoid[1], trapezoid[2], trapezoid[3] };
        if(math::EQ<num_type>(trapezoid.length[0], 0)){
            triangles.push_back({p[0], p[2], p[3]});
            return;
        }
        if(math::EQ<num_type>(trapezoid.length[1], 0)){
            triangles.push_back({p[0], p[1], p[3]});
            return;
        }

        auto i = trapezoid.direction == Orientation2D::Horizontal ? 0 : 1;
        auto h = trapezoid.direction == Orientation2D::Horizontal ? true : false;
        auto d03 = p[3][i] - p[0][i];
        auto d12 = p[2][i] - p[1][i];
        if(math::isNegative(d03) && math::isNegative(d03 + trapezoid.length[1])){
            triangles.push_back({p[0], p[1], p[2]});
            triangles.push_back({p[0], p[2], p[3]});
            return;
        }
        if(math::isPositive(d03) && math::isPositive(d03 - trapezoid.length[0])){
            triangles.push_back({p[0], p[1], p[2]});
            triangles.push_back({p[0], p[2], p[3]});
            return;
        }
        auto s03 = h ? Point2D<num_type>(d03, 0) : Point2D<num_type>(0, d03);
        auto s12 = h ? Point2D<num_type>(d12, 0) : Point2D<num_type>(0, d12);
        auto p03 = math::isPositive(d03) ? p[0] + s03 : p[3] - s03;
        auto p12 = math::isPositive(d12) ? p[2] - s12 : p[1] + s12;

        if(math::NE<num_type>(d03, 0)) triangles.push_back({p[0], p03, p[3]});
        if(math::NE<num_type>(d12, 0)) triangles.push_back({p[1], p[2], p12});
        Box2D<num_type> rect;
        rect |= p03;
        rect |= p12;
        rect |= math::isNegative(d03) ? p[0] : p[3];
        rect |= math::isNegative(d12) ? p[2] : p[1];
        rects.push_back(std::move(rect));
    }

    template <typename property_type, typename num_type>
    static void GriddingRectangle(const property_type & property, const Box2D<num_type> & rect, const GridCtrl<num_type> & ctrl, ProductPipe<property_type> & pipeline)
    {
        auto [sx, sy] = Rasterization::Rasterize(rect[0], ctrl.stride, ctrl.ref);
        auto [ex, ey] = Rasterization::Rasterize(rect[1], ctrl.stride, ctrl.ref);

        bool swapped = false;
        if(math::GT(ey - sy, ex - sx)){
            std::swap(sx, sy);
            std::swap(ex, ey);
            swapped = true;
        }

        auto boundLLStride = [&rect, &ctrl](int index, size_t coor) { return ctrl.ref[coor] + (index + 1) * ctrl.stride[coor] - rect[0][coor]; };
        auto boundURStride = [&rect, &ctrl](int index, size_t coor) { return rect[1][coor] - index * ctrl.stride[coor] - ctrl.ref[coor]; };
        auto middleStride  = [&rect](size_t coor) {return rect[1][coor] - rect[0][coor]; };
        auto gridLength = [&](int i, int lb, int ub, size_t coor)
        {
            auto len = ctrl.stride[coor];
            if(lb == ub) len = middleStride(coor);
            else if(i == lb) len = boundLLStride(i, coor);
            else if(i == ub) len = boundURStride(i, coor);
            return len;
        };

        auto area = ctrl.stride[0] * ctrl.stride[1];
        using Result = Product<property_type>;
        for(auto i = sx; i <= ex; ++i){
            auto len1 = gridLength(i, sx, ex, swapped ? 1 : 0);
            GENERIC_ASSERT(len1 >= 0)
            for(auto j = sy; j <= ey; ++j){
                auto len2 = gridLength(j, sy, ey, swapped ? 0 : 1);
                GENERIC_ASSERT(len2 >= 0)
                auto ratio = double(len1 * len2) / area;
                auto res = swapped ? Result{j, i, property, ratio} : Result{i, j, property, ratio};
                while(!pipeline.push(res));
            }
        }
    }

    template <typename property_type, typename num_type>
    static void GriddingTriangle(const property_type & property, const Triangle2D<num_type> & triangle, const GridCtrl<num_type> & ctrl, ProductPipe<property_type> & pipeline)
    {
        std::vector<std::array<int, 2> > grids;
        std::map<int, std::set<int> > orderedGrids;
        Rasterization::Rasterize(triangle, ctrl.stride, grids, ctrl.ref);
        for(const auto & grid : grids){
            if(!orderedGrids.count(grid[0]))
                orderedGrids.insert(std::make_pair(grid[0], std::set<int>{}));
            orderedGrids[grid[0]].insert(grid[1]);
        }

        auto getBox = [&ctrl](int x, int y)
        {
            Point2D<num_type> ll = ctrl.ref + Point2D<num_type>(x * ctrl.stride[0], y * ctrl.stride[1]);
            Point2D<num_type> ur = ll + ctrl.stride;
            return Box2D<num_type>(ll, ur);
        };

        auto area = ctrl.stride[0] * ctrl.stride[1];
        using Result = Product<property_type>;
        auto iter_x = orderedGrids.begin();
        for(; iter_x != orderedGrids.end(); ++iter_x){
            const auto & x = iter_x->first;
            const auto & orderedYs = iter_x->second;
            auto sy = *orderedYs.begin();
            auto ey = *orderedYs.rbegin();
            for(auto y = sy; y <= ey; ++y){
                double ratio(0);
                auto box = getBox(x, y);
                if(Contains(box, triangle)) ratio = double(triangle.Area()) / area;
                else if(Contains(triangle, box)) ratio = 1.0;
                else {
                    std::list<Polygon2D<num_type> > intersects;
                    boolean::Intersect(box, triangle, intersects);
                    for(const auto & intersect : intersects)
                        ratio += double(boost::polygon::area(intersect)) / area;
                }
                while(!pipeline.push(Result{x, y, property, ratio}));
            }
        }
    }
};

///@brief a demo class that use `OccupancyGridMappingFactory` to calculate grid metal density for a collection of geometries
template <typename num_type>
class DensityGridMapCalculator
{
    using Occupancy = float;
    using Property = const Polygon2D<num_type> *;
public:
    using DensityGridMap = OccupancyGridMap<Occupancy>;
    using Factory = OccupancyGridMappingFactory;
    using Product = Factory::Product<Property>;

    /**
     * @brief insert a polygon object to the calculator
     * @param[in] polygon input polygon object
     * @param[in] isHole whether input is a hole polyogn
     */
    void Insert(const Polygon2D<num_type> & polygon, bool isHole = false)
    {
        if(!isHole){
            m_solids.push_back(&polygon);
            m_solidPropties.push_back(&polygon);
        }
        else{
            m_holes.push_back(&polygon);
            m_holePropties.push_back(&polygon);
        }
    }

    /**
     * @brief insert a polygon with hole object to the calculator
     * @param[in] pwh input polygon with hole object
     */
    void Insert(const PolygonWithHoles2D<num_type> & pwh)
    {
        Insert(pwh.outline);
        for(const auto & hole : pwh.holes)
            Insert(hole, true);
    }

    /**
     * @brief calculate metal density grid map of inserted geometries
     * @param[in] bbox the bounding region of input geometries
     * @param[in] stride the grid width and length
     * @param[in] threads thread number when generating the grid map parallelly
     * @return[in] an unique pointer that hold the generated density grid map
     */
    std::unique_ptr<DensityGridMap> CalculateGridMap(const Box2D<num_type> & bbox, const Vector2D<num_type> & stride, size_t threads = 1)
    {
        auto [width, height] = Factory::GetGridMapSize(bbox, stride);
        auto gridMap = std::make_unique<DensityGridMap>(width, height);

        auto ctrl = typename Factory::GridCtrl<num_type>(bbox, stride, threads);

        auto solidBlend = [](typename DensityGridMap::ResultType & res, const Product & p) { res += p.ratio;};
        auto holeBlend =  [](typename DensityGridMap::ResultType & res, const Product & p) { res -= p.ratio;};

        Factory::Map2Grid<Property>(m_solidPropties, m_solids, ctrl, *gridMap, solidBlend);
        Factory::Map2Grid<Property>(m_holePropties, m_holes, ctrl, *gridMap, holeBlend);

        return gridMap;
    }
private:
    std::vector<Property> m_solidPropties;
    std::vector<Property> m_holePropties;
    std::vector<const Polygon2D<num_type> * > m_solids;
    std::vector<const Polygon2D<num_type> * > m_holes;
};

}//namespace geometry
}//namespace generic
#endif//GENERIC_GEOMETRY_OCCUPANCYGRIDMAP_HPP