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


/* -*- 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);
    }

}


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


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

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>
namespace nt2
{
  namespace tag
  {
   /*!
     @brief Pio2_3t generic tag

     Represents the Pio2_3t constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    // 8.47842766036889956997e-32
    BOOST_SIMD_CONSTANT_REGISTER( Pio2_3t, double
                                , 0, 0x248d3132
                                , 0x397B839A252049C1ll
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Pio2_3t, Site> dispatching_Pio2_3t(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Pio2_3t, Site>();
   }
   template<class... Args>
   struct impl_Pio2_3t;
  }
  /*!
    Constant used in modular computation involving \f$\pi\f$

    @par Semantic:

    For type T0:

    @code
    T0 r = Pio2_3t<T0>();
    @endcode

    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Pio2_3t, Pio2_3t);
}

#endif


/*****************************************************************************
 * This file is huge, it contains everything from parsing yaml files, linear
 * algebra functions to networking code used for robotics.
 *
 * Contents:
 * - Data Type
 * - Macros
 * - Data
 * - Filesystem
 * - Algebra
 * - Linear Algebra
 * - Geometry
 * - Differential Geometry
 * - Statistics
 * - Transform
 * - Time
 *
 ****************************************************************************/
#pragma once
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <sys/time.h>
#include <inttypes.h>
#include <dirent.h>
#include <execinfo.h>
#include <signal.h>
#include <unistd.h>
#include <errno.h>
#include <pthread.h>
#include <netdb.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/poll.h>

#include <iostream>
#include <sstream>
#include <fstream>
#include <string>
#include <random>
#include <set>
#include <list>
#include <deque>
#include <vector>
#include <map>
#include <unordered_map>
#include <unordered_set>
#include <type_traits>

#include <Eigen/Dense>

/******************************************************************************
 *                                DATA TYPE
 *****************************************************************************/

/* PRECISION TYPE */
#define PRECISION 1 // Single Precision
// #define PRECISION 2 // Double Precision

#if PRECISION == 1
  #define real_t float
#elif PRECISION == 2
  #define real_t double
#else
  #define real_t double
#endif

#define col_major_t Eigen::ColMajor
#define row_major_t Eigen::RowMajor

typedef Eigen::Matrix<real_t, 2, 1> vec2_t;
typedef Eigen::Matrix<real_t, 3, 1> vec3_t;
typedef Eigen::Matrix<real_t, 4, 1> vec4_t;
typedef Eigen::Matrix<real_t, 5, 1> vec5_t;
typedef Eigen::Matrix<real_t, 6, 1> vec6_t;
typedef Eigen::Matrix<real_t, Eigen::Dynamic, 1> vecx_t;
typedef Eigen::Matrix<real_t, 2, 2> mat2_t;
typedef Eigen::Matrix<real_t, 3, 3> mat3_t;
typedef Eigen::Matrix<real_t, 4, 4> mat4_t;
typedef Eigen::Matrix<real_t, Eigen::Dynamic, Eigen::Dynamic> matx_t;
typedef Eigen::Matrix<real_t, 3, 4> mat34_t;
typedef Eigen::Quaternion<real_t> quat_t;
typedef Eigen::AngleAxis<real_t> angle_axis_t;
typedef Eigen::Matrix<real_t, 1, Eigen::Dynamic> row_vector_t;
typedef Eigen::Matrix<real_t, Eigen::Dynamic, 1> col_vector_t;
typedef Eigen::Array<real_t, Eigen::Dynamic, 1> arrayx_t;

typedef std::vector<vec2_t, Eigen::aligned_allocator<vec2_t>> vec2s_t;
typedef std::vector<vec3_t, Eigen::aligned_allocator<vec3_t>> vec3s_t;
typedef std::vector<vec4_t, Eigen::aligned_allocator<vec4_t>> vec4s_t;
typedef std::vector<vec5_t, Eigen::aligned_allocator<vec5_t>> vec5s_t;
typedef std::vector<vec6_t, Eigen::aligned_allocator<vec6_t>> vec6s_t;
typedef std::vector<vecx_t> vecxs_t;
typedef std::vector<mat2_t, Eigen::aligned_allocator<mat2_t>> mat2s_t;
typedef std::vector<mat3_t, Eigen::aligned_allocator<mat3_t>> mat3s_t;
typedef std::vector<mat4_t, Eigen::aligned_allocator<mat4_t>> mat4s_t;
typedef std::vector<matx_t, Eigen::aligned_allocator<matx_t>> matxs_t;
typedef std::vector<quat_t, Eigen::aligned_allocator<quat_t>> quats_t;

template <int LENGTH, Eigen::StorageOptions STRIDE_TYPE = Eigen::ColMajor>
using vec_t = Eigen::Matrix<real_t, LENGTH, 1, STRIDE_TYPE>;

template <int ROWS,
          int COLS,
          Eigen::StorageOptions STRIDE_TYPE = Eigen::ColMajor>
using mat_t = Eigen::Matrix<real_t, ROWS, COLS, STRIDE_TYPE>;

template <int ROWS,
          int COLS,
          Eigen::StorageOptions STRIDE_TYPE = Eigen::ColMajor>
using map_mat_t = Eigen::Map<Eigen::Matrix<real_t, ROWS, COLS, STRIDE_TYPE>>;

template <int ROWS>
using map_vec_t = Eigen::Map<Eigen::Matrix<real_t, ROWS, 1>>;

typedef uint64_t timestamp_t;
typedef std::vector<timestamp_t> timestamps_t;

/******************************************************************************
 *                                MACROS
 *****************************************************************************/

#define __FILENAME__                                                           \
  (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__)

#define LOG_ERROR(M, ...)                                                      \
  fprintf(stderr,                                                              \
          "\033[31m[ERROR] [%s:%d] " M "\033[0m\n",                            \
          __FILENAME__,                                                        \
          __LINE__,                                                            \
          ##__VA_ARGS__)

#define LOG_INFO(M, ...) fprintf(stdout, "[INFO] " M "\n", ##__VA_ARGS__)
#define LOG_WARN(M, ...)                                                       \
  fprintf(stdout, "\033[33m[WARN] " M "\033[0m\n", ##__VA_ARGS__)

#define FATAL(M, ...)                                                          \
  fprintf(stdout,                                                              \
          "\033[31m[FATAL] [%s:%d] " M "\033[0m\n",                            \
          __FILENAME__,                                                        \
          __LINE__,                                                            \
          ##__VA_ARGS__);                                                      \
  exit(-1)

#ifdef NDEBUG
#define DEBUG(M, ...)
#else
#define DEBUG(M, ...) fprintf(stdout, "[DEBUG] " M "\n", ##__VA_ARGS__)
#endif

#define UNUSED(expr)                                                           \
  do {                                                                         \
    (void) (expr);                                                             \
  } while (0)

#ifndef CHECK
#define CHECK(A, M, ...)                                                       \
  if (!(A)) {                                                                  \
    LOG_ERROR(M, ##__VA_ARGS__);                                               \
    goto error;                                                                \
  }
#endif

/******************************************************************************
 *                                  DATA
 *****************************************************************************/

/**
 * Convert bytes to signed 8bit number
 */
int8_t int8(const uint8_t *data, const size_t offset);

/**
 * Convert bytes to unsigned 8bit number
 */
uint8_t uint8(const uint8_t *data, const size_t offset);

/**
 * Convert bytes to signed 16bit number
 */
int16_t int16(const uint8_t *data, const size_t offset);

/**
 * Convert bytes to unsigned 16bit number
 */
uint16_t uint16(const uint8_t *data, const size_t offset);

/**
 * Convert bytes to signed 32bit number
 */
int32_t int32(const uint8_t *data, const size_t offset);

/**
 * Convert bytes to unsigned 32bit number
 */
uint32_t uint32(const uint8_t *data, const size_t offset);

/**
 * Allocate memory for a C-style string
 */
char *malloc_string(const char *s);

/**
 * Get number of rows in CSV file.
 * @returns Number of rows in CSV file else -1 for failure.
 */
int csv_rows(const char *fp);

/**
 * Get number of cols in CSV file.
 * @returns Number of cols in CSV file else -1 for failure.
 */
int csv_cols(const char *fp);

/**
 * Return csv fields as strings and number of fields in csv file.
 */
char **csv_fields(const char *fp, int *nb_fields);

/**
 * Load data in csv file `fp`. Assumming the data are real_ts. Also returns
 * number of rows and cols in `nb_rows` and `nb_cols` respectively.
 */
real_t **csv_data(const char *fp, int *nb_rows, int *nb_cols);

/**
 * Load integer arrays in csv file located at `csv_path`. The number of arrays
 * is returned in `nb_arrays`.
 */
int **load_iarrays(const char *csv_path, int *nb_arrays);

/**
 * Load real_t arrays in csv file located at `csv_path`. The number of arrays
 * is returned in `nb_arrays`.
 */
real_t **load_darrays(const char *csv_path, int *nb_arrays);

/**
 * Get number of rows in CSV file.
 * @returns Number of rows in CSV file else -1 for failure.
 */
int csv_rows(const std::string &file_path);

/**
 * Get number of columns in CSV file.
 * @returns Number of columns in CSV file else -1 for failure.
 */
int csv_cols(const std::string &file_path);

/**
 * Convert CSV file to matrix.
 * @returns 0 for success, -1 for failure
 */
int csv2mat(const std::string &file_path, const bool header, matx_t &data);

/**
 * Convert matrix to csv file.
 * @returns 0 for success, -1 for failure
 */
int mat2csv(const std::string &file_path, const matx_t &data);

/**
 * Convert vector to csv file.
 * @returns 0 for success, -1 for failure
 */
int vec2csv(const std::string &file_path, const std::deque<vec3_t> &data);

/**
 * Convert timestamps to csv file.
 * @returns 0 for success, -1 for failure
 */
int ts2csv(const std::string &file_path, const std::deque<timestamp_t> &data);

/**
 * Print progress to screen
 */
void print_progress(const real_t percentage);

/**
 * Check if vector `x` is all true.
 */
bool all_true(const std::vector<bool> x);

int check_jacobian(const std::string &jac_name,
                   const matx_t &fdiff,
                   const matx_t &jac,
                   const real_t threshold,
                   const bool print = false);

/******************************************************************************
 *                                FILESYSTEM
 *****************************************************************************/

/**
 * Open file in `path` with `mode` and set `nb_rows`.
 * @returns File pointer on success, nullptr on failure.
 */
FILE *file_open(const std::string &path,
                const std::string &mode,
                int *nb_rows = nullptr);

/**
 * Skip file line in file `fp`.
 */
void skip_line(FILE *fp);

/**
 * Get number of rows in file.
 * @returns Number of rows in file else -1 for failure.
 */
int file_rows(const std::string &file_path);

/**
 * Copy file from path `src` to path `dest.
 *
 * @returns 0 for success else -1 if `src` file could not be opend, or -2 if
 * `dest` file could not be opened.
 */
int file_copy(const std::string &src, const std::string &dest);

/**
 * Return file extension in `path`.
 */
std::string parse_fext(const std::string &path);

/**
 * Return basename
 */
std::string parse_fname(const std::string &path);

/**
 * Check if file exists
 *
 * @param path Path to file
 * @returns true or false
 */
bool file_exists(const std::string &path);

/**
 * Check if path exists
 *
 * @param path Path
 * @returns true or false
 */
bool dir_exists(const std::string &path);

/**
 * Create directory
 *
 * @param path Path
 * @returns 0 for success, -1 for failure
 */
int dir_create(const std::string &path);

/**
 * Return directory name
 *
 * @param path Path
 * @returns directory name
 */
std::string dir_name(const std::string &path);

/**
 * Strips a target character from the start and end of a string
 *
 * @param s String to strip
 * @param target Target character to strip
 * @returns Stripped string
 */
std::string strip(const std::string &s, const std::string &target = " ");

/**
 * Strips a target character from the end of a string
 *
 * @param s String to strip
 * @param target Target character to strip
 * @returns Stripped string
 */
std::string strip_end(const std::string &s, const std::string &target = " ");

/**
 * Create directory
 *
 * @param path Path to directory
 * @returns 0 for success, -1 for failure
 */
int create_dir(const std::string &path);

/**
 * Remove directory
 *
 * @param path Path to directory
 * @returns 0 for success, -1 for failure
 */
int remove_dir(const std::string &path);

/**
 * Remove file extension
 *
 * @param path Path to directory
 * @returns File path without extension
 */
std::string remove_ext(const std::string &path);

/**
 * List directory
 *
 * @param path Path to directory
 * @param results List of files and directories
 * @returns 0 for success, -1 for failure
 */
int list_dir(const std::string &path, std::vector<std::string> &results);

/**
 * Split path into a number of elements
 *
 * @param path Path
 * @returns List of path elements
 */
std::vector<std::string> path_split(const std::string path);

/**
 * Combine `path1` and `path2`
 *
 * @param path1 Path 1
 * @param path2 Path 22
 * @returns Combined path
 */
std::string paths_combine(const std::string path1, const std::string path2);

/******************************************************************************
 *                                  ALGEBRA
 *****************************************************************************/

/**
 * Sign of number
 *
 * @param[in] x Number to check sign
 * @return
 *    - 0: Number is zero
 *    - 1: Positive number
 *    - -1: Negative number
 */
int sign(const real_t x);

/**
 * Floating point comparator
 *
 * @param[in] f1 First value
 * @param[in] f2 Second value
 * @return
 *    - 0: if equal
 *    - 1: if f1 > f2
 *    - -1: if f1 < f2
 */
int fltcmp(const real_t f1, const real_t f2);

/**
 * Calculate binomial coefficient
 *
 * @param[in] n
 * @param[in] k
 * @returns Binomial coefficient
 */
real_t binomial(const real_t n, const real_t k);

/**
 * Return evenly spaced numbers over a specified interval.
 */
template <typename T>
std::vector<T> linspace(const T start, const T end, const int num) {
  std::vector<T> linspaced;

  if (num == 0) {
    return linspaced;
  }
  if (num == 1) {
    linspaced.push_back(start);
    return linspaced;
  }

  const real_t diff = static_cast<real_t>(end - start);
  const real_t delta = diff / static_cast<real_t>(num - 1);
  for (int i = 0; i < num - 1; ++i) {
    linspaced.push_back(start + delta * i);
  }
  linspaced.push_back(end);
  return linspaced;
}

/******************************************************************************
 *                              LINEAR ALGEBRA
 *****************************************************************************/

/**
 * Print shape of a matrix
 *
 * @param[in] name Name of matrix
 * @param[in] A Matrix
 */
void print_shape(const std::string &name, const matx_t &A);

/**
 * Print shape of a vector
 *
 * @param[in] name Name of vector
 * @param[in] v Vector
 */
void print_shape(const std::string &name, const vecx_t &v);

/**
 * Print array
 *
 * @param[in] name Name of array
 * @param[in] array Target array
 * @param[in] size Size of target array
 */
void print_array(const std::string &name,
                 const real_t *array,
                 const size_t size);

/**
 * Print vector `v` with a `name`.
 */
void print_vector(const std::string &name, const vecx_t &v);

/**
 * Print matrix `m` with a `name`.
 */
void print_matrix(const std::string &name, const matx_t &m);

/**
 * Print quaternion `q` with a `name`.
 */
void print_quaternion(const std::string &name, const quat_t &q);

/**
 * Array to string
 *
 * @param[in] array Target array
 * @param[in] size Size of target array
 * @returns String of array
 */
std::string array2str(const real_t *array, const size_t size);

/**
 * Convert real_t array to Eigen::Vector
 *
 * @param[in] x Input array
 * @param[in] size Size of input array
 * @param[out] y Output vector
 */
void array2vec(const real_t *x, const size_t size, vecx_t &y);

/**
 * Vector to array
 *
 * @param[in] v Vector
 * @returns Array
 */
real_t *vec2array(const vecx_t &v);

/**
 * Matrix to array
 *
 * @param[in] m Matrix
 * @returns Array
 */
real_t *mat2array(const matx_t &m);

/**
 * Quaternion to array
 *
 * *VERY IMPORTANT*: The returned array is (x, y, z, w).
 *
 * @param[in] q Quaternion
 * @returns Array
 */
real_t *quat2array(const quat_t &q);

/**
 * Vector to array
 *
 * @param[in] v Vector
 * @param[out] out Output array
 */
void vec2array(const vecx_t &v, real_t *out);

/**
 * Matrix to array
 *
 * @param[in] m Matrix
 * @param[in] out Output array
 */
void mat2array(const matx_t &m, real_t *out);

/**
 * Matrix to list of vectors
 *
 * @param[in] m Matrix
 * @param[in] row_wise Row wise
 * @returns Vectors
 */
std::vector<vecx_t> mat2vec(const matx_t &m, const bool row_wise = true);

/**
 * Matrix to list of vectors of size 3
 *
 * @param[in] m Matrix
 * @param[in] row_wise Row wise
 * @returns Vectors
 */
vec3s_t mat2vec3(const matx_t &m, const bool row_wise = true);

/**
 * Matrix to list of vectors of size 3
 *
 * @param[in] m Matrix
 * @param[in] row_wise Row wise
 * @returns Vectors
 */
vec2s_t mat2vec2(const matx_t &m, const bool row_wise = true);

/**
 * Vectors to matrix
 */
matx_t vecs2mat(const vec3s_t &vs);

/**
 * Vector to string
 *
 * @param[in] v Vector
 * @param[in] brackets Brakcets around vector string
 * @returns Vector as a string
 */
std::string vec2str(const vecx_t &v, const bool brackets = true);

/**
 * Array to string
 *
 * @param[in] arr Array
 * @param[in] len Length of array
 * @param[in] brackets Brakcets around vector string
 * @returns Array as a string
 */
std::string arr2str(const real_t *arr, const size_t len, bool brackets = true);

/**
 * Matrix to string
 *
 * @param[in] m Matrix
 * @param[in] indent Indent string
 * @returns Array as a string
 */
std::string mat2str(const matx_t &m, const std::string &indent = "  ");

/**
 * Normalize vector x
 */
vec2_t normalize(const vec2_t &x);

/**
 * Normalize vector `v`.
 */
vec3_t normalize(const vec3_t &v);

/**
 * Condition number of `A`.
 */
real_t cond(const matx_t &A);

/**
 * Zeros-matrix
 *
 * @param rows Number of rows
 * @param cols Number of cols
 * @returns Zeros matrix
 */
matx_t zeros(const int rows, const int cols);

/**
 * Zeros square matrix
 *
 * @param size Square size of matrix
 * @returns Zeros matrix
 */
matx_t zeros(const int size);

/**
 * Identity-matrix
 *
 * @param rows Number of rows
 * @param cols Number of cols
 * @returns Identity matrix
 */
matx_t I(const int rows, const int cols);

/**
 * Identity square matrix
 *
 * @param size Square size of matrix
 * @returns Identity square matrix
 */
matx_t I(const int size);

/**
 * Ones-matrix
 *
 * @param rows Number of rows
 * @param cols Number of cols
 * @returns Ones square matrix
 */
matx_t ones(const int rows, const int cols);

/**
 * Ones square matrix
 *
 * @param size Square size of matrix
 * @returns Ones square matrix
 */
matx_t ones(const int size);

/**
 * Horizontally stack matrices A and B
 *
 * @param A Matrix A
 * @param B Matrix B
 * @returns Stacked matrix
 */
matx_t hstack(const matx_t &A, const matx_t &B);

/**
 * Vertically stack matrices A and B
 *
 * @param A Matrix A
 * @param B Matrix B
 * @returns Stacked matrix
 */
matx_t vstack(const matx_t &A, const matx_t &B);

/**
 * Diagonally stack matrices A and B
 *
 * @param A Matrix A
 * @param B Matrix B
 * @returns Stacked matrix
 */
matx_t dstack(const matx_t &A, const matx_t &B);

/**
 * Skew symmetric-matrix
 *
 * @param w Input vector
 * @returns Skew symmetric matrix
 */
mat3_t skew(const vec3_t &w);

/**
 * Skew symmetric-matrix squared
 *
 * @param w Input vector
 * @returns Skew symmetric matrix squared
 */
mat3_t skewsq(const vec3_t &w);

/**
 * Enforce Positive Semi-Definite
 *
 * @param A Input matrix
 * @returns Positive semi-definite matrix
 */
matx_t enforce_psd(const matx_t &A);

/**
 * Null-space of A
 *
 * @param A Input matrix
 * @returns Null space of A
 */
matx_t nullspace(const matx_t &A);

/**
 * Load std::vector of real_ts to an Eigen::Matrix
 *
 * @param[in] x Matrix values
 * @param[in] rows Number of matrix rows
 * @param[in] cols Number of matrix colums
 * @param[out] y Output matrix
 */
void load_matrix(const std::vector<real_t> &x,
                 const int rows,
                 const int cols,
                 matx_t &y);

/**
 * Load an Eigen::Matrix into a std::vector of real_ts
 *
 * @param[in] A Matrix
 * @param[out] x Output vector of matrix values
 */
void load_matrix(const matx_t A, std::vector<real_t> &x);

/**
 * Perform Schur's Complement
 */
void schurs_complement(const matx_t &H, const vecx_t &b,
                       const size_t m, const size_t r,
                       matx_t &H_marg, vecx_t &b_marg,
                       const bool precond=false, const bool debug=false);

/******************************************************************************
 *                                 Geometry
 *****************************************************************************/

/**
 * Sinc function.
 */
real_t sinc(const real_t x);

/**
 * Degrees to radians
 *
 * @param[in] d Degree to be converted
 * @return Degree in radians
 */
real_t deg2rad(const real_t d);

/**
 * Degrees to radians
 *
 * @param[in] d Degree to be converted
 * @return Degree in radians
 */
vec3_t deg2rad(const vec3_t d);

/**
 * Radians to degree
 *
 * @param[in] r Radian to be converted
 * @return Radian in degrees
 */
real_t rad2deg(const real_t r);

/**
 * Radians to degree
 *
 * @param[in] r Radian to be converted
 * @return Radian in degrees
 */
vec3_t rad2deg(const vec3_t &r);

/**
 * Wrap angle in degrees to 180
 *
 * @param[in] d Degrees
 * @return Angle wraped to 180
 */
real_t wrap180(const real_t d);

/**
 * Wrap angle in degrees to 360
 *
 * @param[in] d Degrees
 * @return Angle wraped to 360
 */
real_t wrap360(const real_t d);

/**
 * Wrap angle in radians to PI
 *
 * @param[in] r Radians
 * @return Angle wraped to PI
 */
real_t wrapPi(const real_t r);

/**
 * Wrap angle in radians to 2 PI
 *
 * @param[in] r Radians
 * @return Angle wraped to 2 PI
 */
real_t wrap2Pi(const real_t r);

/**
 * Create a circle point of radius `r` at angle `theta` radians.
 */
vec2_t circle(const real_t r, const real_t theta);

/**
 * Create the sphere point with sphere radius `rho` at longitude `theta`
 * [radians] and latitude `phi` [radians].
 */
vec3_t sphere(const real_t rho, const real_t theta, const real_t phi);

/**
 * Create look at matrix.
 */
mat4_t lookat(const vec3_t &cam_pos,
              const vec3_t &target,
              const vec3_t &up_axis = vec3_t{0.0, -1.0, 0.0});

/**
 * Cross-Track error based on waypoint line between p1, p2, and robot position
 *
 * @param[in] p1 Waypoint 1
 * @param[in] p2 Waypoint 2
 * @param[in] pos Robot position
 * @return Cross track error
 */
real_t cross_track_error(const vec2_t &p1, const vec2_t &p2, const vec2_t &pos);

/**
 * Check if point `pos` is left or right of line formed by `p1` and `p2`
 *
 * @param[in] p1 Waypoint 1
 * @param[in] p2 Waypoint 2
 * @param[in] pos Robot position
 * @returns
 *    - 1: Point is left of waypoint line formed by `p1` and `p2`
 *    - 2: Point is right of waypoint line formed by `p1` and `p2`
 *    - 0: Point is colinear with waypoint line formed by `p1` and `p2`
 */
int point_left_right(const vec2_t &p1, const vec2_t &p2, const vec2_t &pos);

/**
 * Calculate closest point given waypoint line between `p1`, `p2` and robot
 * position
 *
 * @param[in] p1 Waypoint 1
 * @param[in] p2 Waypoint 2
 * @param[in] p3 Robot position
 * @param[out] closest Closest point
 * @returns
 *    Unit number denoting where the closest point is on waypoint line. For
 *    example, a return value of 0.5 denotes the closest point is half-way
 *    (50%) of the waypoint line, alternatively a negative number denotes the
 *    closest point is behind the first waypoint.
 */
real_t closest_point(const vec2_t &p1,
                     const vec2_t &p2,
                     const vec2_t &p3,
                     vec2_t &closest);

#define EARTH_RADIUS_M 6378137.0

/**
 * Calculate new latitude and logitude coordinates with an offset in North and
 * East direction.
 *
 * IMPORTANT NOTE: This function is only an approximation. As such do not rely
 * on this function for precise latitude, longitude offsets.
 *
 * @param lat_ref Latitude of origin (decimal format)
 * @param lon_ref Longitude of origin (decimal format)
 * @param offset_N Offset in North direction (meters)
 * @param offset_E Offset in East direction (meters)
 * @param lat_new New latitude (decimal format)
 * @param lon_new New longitude (decimal format)
 */
void latlon_offset(real_t lat_ref,
                   real_t lon_ref,
                   real_t offset_N,
                   real_t offset_E,
                   real_t *lat_new,
                   real_t *lon_new);

/**
 * Calculate difference in distance in North and East from two GPS coordinates
 *
 * IMPORTANT NOTE: This function is only an approximation. As such do not rely
 * on this function for precise latitude, longitude diffs.
 *
 * @param lat_ref Latitude of origin (decimal format)
 * @param lon_ref Longitude of origin (decimal format)
 * @param lat Latitude of point of interest (decimal format)
 * @param lon Longitude of point of interest (decimal format)
 * @param dist_N Distance of point of interest in North axis [m]
 * @param dist_E Distance of point of interest in East axis [m]
 */
void latlon_diff(real_t lat_ref,
                 real_t lon_ref,
                 real_t lat,
                 real_t lon,
                 real_t *dist_N,
                 real_t *dist_E);

/**
 * Calculate Euclidean distance between two GPS coordintes
 *
 * IMPORTANT NOTE: This function is only an approximation. As such do not rely
 * on this function for precise latitude, longitude distance.
 *
 * @param lat_ref Latitude of origin (decimal format)
 * @param lon_ref Longitude of origin (decimal format)
 * @param lat Latitude of point of interest (decimal format)
 * @param lon Longitude of point of interest (decimal format)
 *
 * @returns Euclidean distance between two GPS coordinates [m]
 */
real_t latlon_dist(real_t lat_ref, real_t lon_ref, real_t lat, real_t lon);

/*****************************************************************************
 *                         DIFFERENTIAL GEOMETRY
 *****************************************************************************/

namespace lie {

mat3_t Exp(const vec3_t &phi);
vec3_t Log(const mat3_t &C);
mat3_t Jr(const vec3_t &psi);

} // namespace lie

/******************************************************************************
 *                                STATISTICS
 *****************************************************************************/

/**
 * Create random integer
 *
 * @param[in] ub Upper bound
 * @param[in] lb Lower bound
 * @return Random integer
 */
int randi(const int ub, const int lb);

/**
 * Create random real_t
 *
 * @param[in] ub Upper bound
 * @param[in] lb Lower bound
 * @return Random floating point
 */
real_t randf(const real_t ub, const real_t lb);

/**
 * Calculate median given an array of numbers
 *
 * @param[in] v Array of numbers
 * @return Median of given array
 */
real_t median(const std::vector<real_t> &v);

/**
 * Mean vector
 *
 * @param[in] x List of vectors
 * @return Mean vector
 */
vec3_t mean(const vec3s_t &x);

/**
 * Shannon Entropy of a given covariance matrix `covar`.
 */
real_t shannon_entropy(const matx_t &covar);

/**
 * Multivariate normal.
 */
vec3_t mvn(std::default_random_engine &engine,
           const vec3_t &mu = vec3_t{0.0, 0.0, 0.0},
           const vec3_t &stdev = vec3_t{1.0, 1.0, 1.0});

/**
 * Gassian normal.
 * http://c-faq.com/lib/gaussian.html
 */
real_t gauss_normal();

/*****************************************************************************
 *                               TRANSFORM
 *****************************************************************************/

/**
 * Form a 4x4 homogeneous transformation matrix from a
 * rotation matrix `C` and translation vector `r`.
 */
template <typename T>
Eigen::Matrix<T, 4, 4> tf(const Eigen::Matrix<T, 3, 3> &C,
                          const Eigen::Matrix<T, 3, 1> &r) {
  Eigen::Matrix<T, 4, 4> transform = Eigen::Matrix<T, 4, 4>::Identity();
  transform.block(0, 0, 3, 3) = C;
  transform.block(0, 3, 3, 1) = r;
  return transform;
}

/**
 * Form a 4x4 homogeneous transformation matrix from a pointer to real_t array
 * containing (quaternion + translation) 7 elements: (qw, qx, qy, qz, x, y, z)
 */
mat4_t tf(const vecx_t &params);

/**
 * Form a 4x4 homogeneous transformation matrix from a
 * rotation matrix `C` and translation vector `r`.
 */
mat4_t tf(const mat3_t &C, const vec3_t &r);

/**
 * Form a 4x4 homogeneous transformation matrix from a
 * Hamiltonian quaternion `q` and translation vector `r`.
 */
mat4_t tf(const quat_t &q, const vec3_t &r);

/**
 * Extract rotation from transform
 */
inline mat3_t tf_rot(const mat4_t &tf) { return tf.block<3, 3>(0, 0); }

/**
 * Extract rotation and convert to quaternion from transform
 */
inline quat_t tf_quat(const mat4_t &tf) { return quat_t{tf.block<3, 3>(0, 0)}; }

/**
 * Extract translation from transform
 */
inline vec3_t tf_trans(const mat4_t &tf) { return tf.block<3, 1>(0, 3); }

/**
 * Perturb the rotation element in the tranform `T` by `step_size` at index
 * `i`. Where i = 0 for x-axis, i = 1 for y-axis, and i = 2 for z-axis.
 */
mat4_t tf_perturb_rot(const mat4_t &T, real_t step_size, const int i);

/**
 * Perturb the translation element in the tranform `T` by `step_size` at index
 * `i`. Where i = 0 for x-axis, i = 1 for y-axis, and i = 2 for z-axis.
 */
mat4_t tf_perturb_trans(const mat4_t &T, real_t step_size, const int i);

/**
 * Transform point `p` with transform `T`.
 */
vec3_t tf_point(const mat4_t &T, const vec3_t &p);

/**
 * Rotation matrix around x-axis (counter-clockwise, right-handed).
 * @returns Rotation matrix
 */
mat3_t rotx(const real_t theta);

/**
 * Rotation matrix around y-axis (counter-clockwise, right-handed).
 * @returns Rotation matrix
 */
mat3_t roty(const real_t theta);

/**
 * Rotation matrix around z-axis (counter-clockwise, right-handed).
 * @returns Rotation matrix
 */
mat3_t rotz(const real_t theta);

/**
 * Convert euler sequence 123 to rotation matrix R
 * This function assumes we are performing a body fixed intrinsic rotation.
 *
 * Source:
 *
 *     Kuipers, Jack B. Quaternions and Rotation Sequences: A Primer with
 *     Applications to Orbits, Aerospace, and Virtual Reality. Princeton, N.J:
 *     Princeton University Press, 1999. Print.
 *
 *     Page 86.
 *
 * @returns Rotation matrix
 */
mat3_t euler123(const vec3_t &euler);

/**
 * Convert euler sequence 321 to rotation matrix R
 * This function assumes we are performing a body fixed intrinsic rotation.
 *
 * Source:
 *
 *     Kuipers, Jack B. Quaternions and Rotation Sequences: A Primer with
 *     Applications to Orbits, Aerospace, and Virtual Reality. Princeton, N.J:
 *     Princeton University Press, 1999. Print.
 *
 *     Page 86.
 *
 * @returns Rotation matrix
 */
mat3_t euler321(const vec3_t &euler);

/**
 * Convert roll, pitch and yaw to quaternion.
 */
quat_t euler2quat(const vec3_t &euler);

/**
 * Convert rotation vectors to rotation matrix using measured acceleration
 * `a_m` from an IMU and gravity vector `g`.
 */
mat3_t vecs2rot(const vec3_t &a_m, const vec3_t &g);

/**
 * Convert rotation vector `rvec` to rotation matrix.
 */
mat3_t rvec2rot(const vec3_t &rvec, const real_t eps = 1e-5);

/**
 * Convert quaternion to euler angles.
 */
vec3_t quat2euler(const quat_t &q);

/**
 * Convert quaternion to rotation matrix.
 */
mat3_t quat2rot(const quat_t &q);

/**
 * Convert small angle euler angle to quaternion.
 */
quat_t quat_delta(const vec3_t &dalpha);

/**
 * Return left quaternion product matrix.
 */
mat4_t quat_lmul(const quat_t &q);

/**
 * Return left quaternion product matrix (but only for x, y, z components).
 */
mat3_t quat_lmul_xyz(const quat_t &q);

/**
 * Return right quaternion product matrix.
 */
mat4_t quat_rmul(const quat_t &q);

/**
 * Return right quaternion product matrix (but only for x, y, z components).
 */
mat3_t quat_rmul_xyz(const quat_t &q);

/**
 * Return only the x, y, z, components of a quaternion matrix.
 */
mat3_t quat_mat_xyz(const mat4_t &Q);

/**
 * Initialize attitude using IMU gyroscope `w_m` and accelerometer `a_m`
 * measurements. The calculated attitude outputted into to `C_WS`. Note: this
 * function does not calculate initial yaw angle in the world frame. Only the
 * roll, and pitch are inferred from IMU measurements.
 */
void imu_init_attitude(const vec3s_t w_m,
                       const vec3s_t a_m,
                       mat3_t &C_WS,
                       const size_t buffer_size = 50);

/*****************************************************************************
 *                                    TIME
 *****************************************************************************/

/**
 * Print timestamp.
 */
void timestamp_print(const timestamp_t &ts, const std::string &prefix = "");

/**
 * Convert ts to second.
 */
real_t ts2sec(const timestamp_t &ts);

/**
 * Convert nano-second to second.
 */
real_t ns2sec(const uint64_t ns);

/**
 * Start timer.
 */
struct timespec tic();

/**
 * Stop timer and return number of seconds.
 */
float toc(struct timespec *tic);

/**
 * Stop timer and return miliseconds elasped.
 */
float mtoc(struct timespec *tic);

/**
 * Get time now in milliseconds since epoch
 */
real_t time_now();

/*****************************************************************************
 *                                BA DATA
 ****************************************************************************/

struct pose_t {
  vec_t<7> param;

  pose_t();
  pose_t(const mat4_t &T);

  quat_t rot() const;
  vec3_t trans() const;
  mat4_t T() const;

  quat_t rot();
  vec3_t trans();
  mat4_t T();

  void set_trans(const vec3_t &r);
  void set_rot(const quat_t &q);
  void set_rot(const mat3_t &C);
};
typedef std::vector<pose_t> poses_t;
typedef std::vector<vec2_t> keypoints_t;

void pose_print(const std::string &prefix, const pose_t &pose);

poses_t load_poses(const std::string &csv_path);
keypoints_t parse_keypoints_line(const char *line);
std::vector<keypoints_t> load_keypoints(const std::string &data_path);
void keypoints_print(const keypoints_t &keypoints);
poses_t load_poses(const std::string &csv_path);
keypoints_t parse_keypoints_line(const char *line);
std::vector<keypoints_t> load_keypoints(const std::string &data_path);
void keypoints_print(const keypoints_t &keypoints);
mat3_t load_camera(const std::string &data_path);
poses_t load_camera_poses(const std::string &data_path);
poses_t load_target_pose(const std::string &data_path);
real_t **load_points(const std::string &data_path, int *nb_points);
int **load_point_ids(const std::string &data_path, int *nb_points);


#include "expr.hpp"

#include "version.hpp"
#include "env.hpp"

#include <string_view>
#include <iostream>
#include <cmath>
#include <numeric>
#ifdef ENABLE_NIVALIS_BOOST_MATH
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/trigamma.hpp>
#include <boost/math/special_functions/polygamma.hpp>
#include <boost/math/special_functions/zeta.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/factorials.hpp>
#endif
#include "opcodes.hpp"
#include "util.hpp"

namespace nivalis {

namespace {
const double NONE = std::numeric_limits<double>::quiet_NaN();
using ::nivalis::OpCode::repr;
#ifndef ENABLE_NIVALIS_BOOST_MATH
void print_boost_warning(uint32_t opcode) {
    std::cerr << "Function " << OpCode::repr(opcode)
        << " requires Nivalis to be compiled with Boost" << std::endl;
}

// Falling factorial
double fa_fact (size_t x, size_t to_exc = 1) {
    if (x >= to_exc + 175) // too expensive
        return std::numeric_limits<double>::quiet_NaN();
    double result = 1.;
    while (x > to_exc) result *= x--;
    return result;
}
#endif
}  // namespace

namespace detail {
double eval_ast(Environment& env, const Expr::AST& ast,
        const std::vector<double>& arg_vals) {
    // Main AST evaluation stack
    thread_local std::vector<double> stk;
    // Stack of thunks available:
    // contains positions of thunk_jmp encountered
    thread_local std::vector<size_t> thunks;
    // Thunk call stack: contains positions to return to
    // after reaching thunk_ret
    thread_local std::vector<size_t> thunks_stk;

    // Max function call stack height
    static const size_t MAX_CALL_STK_HEIGHT = 256;
    // Curr function call stack height
    thread_local size_t call_stk_height = 0;

    thread_local size_t top = -1;
    size_t init_top = top;

    // Make sure there is enough space
    stk.resize(top + ast.size() + 1);

#ifdef ENABLE_NIVALIS_BOOST_MATH
    using namespace boost;
    using namespace boost::math;
#endif
    using namespace nivalis::OpCode;

    bool _is_thunk_ret = false;

    // Shorthands for first, 2nd, 3rd arguments to operator
#define ARG3 stk[top-2]
#define ARG2 stk[top-1]
#define ARG1 stk[top]
// Return value from thunk or func call
#define RET_VAL stk[top+1]
// Quit without messing up stack
#define FAIL_AND_QUIT do {top = init_top; return NONE; } while(0)

    for (size_t cidx = ast.size() - 1; ~cidx; --cidx) {
        const auto& node = ast[cidx];
        switch(node.opcode) {
            case null: stk[++top] = NONE; break;
            case val:
                stk[++top] = node.val;  break;
            case ref:
                stk[++top] = env.vars[node.ref]; break;
            case arg:
                stk[++top] = arg_vals[node.ref]; break;
            case thunk_jmp:
                thunks.push_back(cidx);
                cidx -= ast[cidx].ref;
                break;
            case thunk_ret:
                cidx = thunks_stk.back() + 1;
                thunks_stk.pop_back();
                _is_thunk_ret = true;
                break;
            case call:
                {
                    size_t n_args = node.call_info[1];
                    auto& func = env.funcs[node.call_info[0]];
                    std::vector<double> f_args(n_args);
                    for (size_t i = 0; i < n_args; ++i) {
                        f_args[i] = stk[top--];
                    }
                    if (n_args != func.n_args ||               // Should not happen
                        &func.expr.ast[0] == &ast[0] ||        // Disallow recursion
                        call_stk_height > MAX_CALL_STK_HEIGHT) // Too many nested calls
                            FAIL_AND_QUIT;
                    ++call_stk_height;
                    eval_ast(env, func.expr.ast, f_args);
                    --call_stk_height;
                    ++top;
                }
                break;
            case bnz:
                if (_is_thunk_ret) {
                    --top; _is_thunk_ret = false;
                    ARG1 = RET_VAL;
                } else {
                    thunks_stk.push_back(cidx);
                    cidx = thunks[thunks.size() - (ARG1 == 0.) - 1];
                    thunks.resize(thunks.size() - 2);
                }
                break;

            case sums: case prods:
                {
                    if (_is_thunk_ret) {
                        --top; _is_thunk_ret = false;
                        // update arg3 (the output)
                        if (node.opcode == prods)
                            ARG3 *= RET_VAL;
                        else
                            ARG3 += RET_VAL; // arg3 is output
                    } else {
                        // Move over the arguments and use arg3
                        // as output
                        ++top; ARG1 = ARG2; ARG2 = ARG3;
                        ARG3 = node.opcode == prods ? 1. : 0.;
                    }
                    uint64_t var_id = node.ref;
                    int64_t a = static_cast<int64_t>(ARG1),
                            b = static_cast<int64_t>(ARG2);
                    int64_t step = (a <= b) ? 1 : -1;
                    if (std::isnan(ARG1)) {
                        top -= 2; thunks.pop_back();
                    } else {
                        env.vars[var_id] = static_cast<double>(a);
                        a += step;
                        if (a == b + step) ARG1 = NONE;
                        else ARG1 = static_cast<double>(a);
                        thunks_stk.push_back(cidx);
                        cidx = thunks.back();
                    }
                }
                break;

            case bsel: --top; break;
            case add: ARG2 += ARG1; --top; break;
            case sub: ARG2 = ARG1 - ARG2; --top; break;
            case mul: ARG2 *= ARG1;  --top; break;
            case divi: ARG2 = ARG1 / ARG2; --top; break;
            case mod: ARG2 = std::fmod(ARG1, ARG2); --top; break;
            case power: ARG2 = std::pow(ARG1, ARG2); --top; break;
            case logbase: ARG2 = log(ARG1) / log(ARG2); --top; break;
            case max: ARG2 = std::max(ARG1, ARG2); --top; break;
            case min: ARG2 = std::min(ARG1, ARG2); --top; break;
            case land: ARG2 = static_cast<double>(ARG1 && ARG2); --top; break;
            case lor: ARG2 = static_cast<double>(ARG1 || ARG2); --top; break;
            case lxor: ARG2 = static_cast<double>(
                               (ARG1 != 0.) ^ (ARG2 != 0.)); --top; break;
            case gcd: ARG2 = static_cast<double>(
                              std::gcd((int64_t) ARG1,
                                       (int64_t) ARG2)); --top; break;
            case lcm: ARG2 = ARG1 * ARG2 /
                      static_cast<double>(std::gcd(
                          (int64_t) ARG1, (int64_t) ARG2)); --top; break;
#ifdef ENABLE_NIVALIS_BOOST_MATH
            case choose: ARG2 = binomial_coefficient<double>(
                            (uint32_t)ARG1, (uint32_t)ARG2); --top; break;
            case fafact: ARG2 = falling_factorial<double>(
                            (uint32_t)ARG1, (uint32_t)ARG2); --top; break;
            case rifact: ARG2 = rising_factorial<double>(
                            (uint32_t)ARG1, (uint32_t)ARG2); --top; break;
            case betab: ARG2 = beta<double>(ARG1, ARG2); --top; break;
            case polygammab:
                        ARG2 = polygamma<double>((int)ARG1, ARG2); --top;
                        break;
#else
            case choose: {
                             double ad = std::round(ARG1);
                             double bd = std::round(ARG2);
                             if (ad < 0 || bd < 0) {
                                 ARG2 = std::numeric_limits<double>::quiet_NaN(); --top; break;
                             }
                             size_t a = static_cast<size_t>(ad), b = static_cast<size_t>(bd);
                             b = std::min(b, a-b);
                             ARG2 = fa_fact(a, a-b) / fa_fact(b);
                             --top; break;
                         }
            case fafact: {
                             double ad = std::round(ARG1);
                             double bd = std::round(ARG2);
                             if (ad < 0 || bd < 0) {
                                 ARG2 = std::numeric_limits<double>::quiet_NaN(); --top; break;
                             }
                             size_t a = static_cast<size_t>(ad), b = static_cast<size_t>(bd);
                             ARG2 = fa_fact(a, a-b);
                             --top; break;
                         }
            case rifact: {
                             double ad = std::round(ARG1);
                             double bd = std::round(ARG2);
                             if (ad < 0 || bd < 0) {
                                 ARG2 = std::numeric_limits<double>::quiet_NaN(); --top; break;
                             }
                             size_t a = static_cast<size_t>(ad), b = static_cast<size_t>(bd);
                             ARG2 = fa_fact(a+b-1, a-1);
                             --top; break;
                         }
            case betab: case polygammab:
                ARG1 = NONE; print_boost_warning(node.opcode); break;
#endif
            case lt: ARG2 = static_cast<double>(ARG1 < ARG2); --top; break;
            case le: ARG2 = static_cast<double>(ARG1 <= ARG2); --top; break;
            case eq: ARG2 = static_cast<double>(ARG1 == ARG2); --top; break;
            case ne: ARG2 = static_cast<double>(ARG1 != ARG2); --top; break;
            case ge: ARG2 = static_cast<double>(ARG1 >= ARG2); --top; break;
            case gt: ARG2 = static_cast<double>(ARG1 > ARG2); --top; break;

            case unaryminus: ARG1 = -ARG1; break;
            case lnot: ARG1 = static_cast<double>(!(ARG1)); break;
            case absb: ARG1 = std::fabs(ARG1); break;
            case sqrtb: ARG1 = std::sqrt(ARG1); break;
            case sqrb: ARG1 *= ARG1; break;
            case sgn: ARG1 = ARG1 > 0 ? 1 : (ARG1 == 0 ? 0 : -1); break;
            case floorb: ARG1 = floor(ARG1); break;
            case ceilb: ARG1 = ceil(ARG1); break; case roundb: ARG1 = round(ARG1); break;

            case expb:   ARG1 = exp(ARG1); break; case exp2b: ARG1 = exp2(ARG1); break;
            case logb:   ARG1 = log(ARG1); break;
            case factb:
                        {
                            unsigned n = static_cast<unsigned>(std::max(
                                        ARG1, 0.));
#ifdef ENABLE_NIVALIS_BOOST_MATH
                            ARG1 = factorial<double>(n);
#else
                            ARG1 = fa_fact(n, 1);
#endif
                        }
                        break;
            case log2b: ARG1 = log2(ARG1); break;  case log10b: ARG1 = log10(ARG1); break;
            case sinb: ARG1 = sin(ARG1); break;   case cosb:   ARG1 = cos(ARG1); break;
            case tanb: ARG1 = tan(ARG1); break;   case asinb: ARG1 = asin(ARG1); break;
            case acosb: ARG1 = acos(ARG1); break;  case atanb: ARG1 = atan(ARG1); break;
            case sinhb: ARG1 = sinh(ARG1); break;  case coshb: ARG1 = cosh(ARG1); break;
            case tanhb: ARG1 = tanh(ARG1); break;
            case tgammab: ARG1 = std::tgamma(ARG1); break;
            case lgammab: ARG1 = std::lgamma(ARG1); break;
#ifdef ENABLE_NIVALIS_BOOST_MATH
            case digammab: ARG1 = digamma<double>(ARG1); break;
            case trigammab: ARG1 = trigamma<double>(ARG1); break;
            case zetab: ARG1 = zeta<double>(ARG1); break;
#else
           // The following functions are unavailable without Boost
            case digammab: case trigammab: case zetab:
                ARG1 = NONE; print_boost_warning(node.opcode); break;
#endif
            case erfb: ARG1 = erf(ARG1); break; break;
        }
    }
    return stk[top--];
}
}  // namespace detail

// Interface for evaluating expression
double Expr::operator()(Environment& env) const {
    return detail::eval_ast(env, ast);
}
double Expr::operator()(double arg, Environment& env) const {
    return detail::eval_ast(env, ast, {arg});
}
double Expr::operator()(const std::vector<double>& args,
        Environment& env) const {

    return detail::eval_ast(env, ast, args);
}

}  // namespace nivalis