// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_GN_SINU_HPP
#define BOOST_GEOMETRY_PROJECTIONS_GN_SINU_HPP

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/aasincos.hpp>
#include <boost/geometry/srs/projections/impl/pj_mlfn.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace gn_sinu
    {

            static const double epsilon10 = 1e-10;
            static const int max_iter = 8;
            static const double loop_tol = 1e-7;

            template <typename T>
            struct par_gn_sinu
            {
                detail::en<T> en;
                T    m, n, C_x, C_y;
            };

            /* Ellipsoidal Sinusoidal only */

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_gn_sinu_ellipsoid
                : public base_t_fi<base_gn_sinu_ellipsoid<T, Parameters>, T, Parameters>
            {
                par_gn_sinu<T> m_proj_parm;

                inline base_gn_sinu_ellipsoid(const Parameters& par)
                    : base_t_fi<base_gn_sinu_ellipsoid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(e_forward)  ellipsoid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    T s, c;

                    xy_y = pj_mlfn(lp_lat, s = sin(lp_lat), c = cos(lp_lat), this->m_proj_parm.en);
                    xy_x = lp_lon * c / sqrt(1. - this->m_par.es * s * s);
                }

                // INVERSE(e_inverse)  ellipsoid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T const& xy_y, T& lp_lon, T& lp_lat) const
                {
                    static const T half_pi = detail::half_pi<T>();

                    T s;

                    if ((s = fabs(lp_lat = pj_inv_mlfn(xy_y, this->m_par.es, this->m_proj_parm.en))) < half_pi) {
                        s = sin(lp_lat);
                        lp_lon = xy_x * sqrt(1. - this->m_par.es * s * s) / cos(lp_lat);
                    } else if ((s - epsilon10) < half_pi)
                        lp_lon = 0.;
                    else
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                }
                /* General spherical sinusoidals */

                static inline std::string get_name()
                {
                    return "gn_sinu_ellipsoid";
                }

            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_gn_sinu_spheroid
                : public base_t_fi<base_gn_sinu_spheroid<T, Parameters>, T, Parameters>
            {
                par_gn_sinu<T> m_proj_parm;

                inline base_gn_sinu_spheroid(const Parameters& par)
                    : base_t_fi<base_gn_sinu_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  sphere
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T lp_lat, T& xy_x, T& xy_y) const
                {
                    if (this->m_proj_parm.m == 0.0)
                        lp_lat = this->m_proj_parm.n != 1. ? aasin(this->m_proj_parm.n * sin(lp_lat)): lp_lat;
                    else {
                        T k, V;
                        int i;

                        k = this->m_proj_parm.n * sin(lp_lat);
                        for (i = max_iter; i ; --i) {
                            lp_lat -= V = (this->m_proj_parm.m * lp_lat + sin(lp_lat) - k) /
                                (this->m_proj_parm.m + cos(lp_lat));
                            if (fabs(V) < loop_tol)
                                break;
                        }
                        if (!i) {
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        }
                    }
                    xy_x = this->m_proj_parm.C_x * lp_lon * (this->m_proj_parm.m + cos(lp_lat));
                    xy_y = this->m_proj_parm.C_y * lp_lat;
                }

                // INVERSE(s_inverse)  sphere
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T const& xy_x, T xy_y, T& lp_lon, T& lp_lat) const
                {
                    xy_y /= this->m_proj_parm.C_y;
                    lp_lat = (this->m_proj_parm.m != 0.0) ? aasin((this->m_proj_parm.m * xy_y + sin(xy_y)) / this->m_proj_parm.n) :
                        ( this->m_proj_parm.n != 1. ? aasin(sin(xy_y) / this->m_proj_parm.n) : xy_y );
                    lp_lon = xy_x / (this->m_proj_parm.C_x * (this->m_proj_parm.m + cos(xy_y)));
                }

                static inline std::string get_name()
                {
                    return "gn_sinu_spheroid";
                }

            };

            template <typename Parameters, typename T>
            inline void setup(Parameters& par, par_gn_sinu<T>& proj_parm)
            {
                par.es = 0;

                proj_parm.C_x = (proj_parm.C_y = sqrt((proj_parm.m + 1.) / proj_parm.n))/(proj_parm.m + 1.);
            }

            // General Sinusoidal Series
            template <typename Params, typename Parameters, typename T>
            inline void setup_gn_sinu(Params const& params, Parameters& par, par_gn_sinu<T>& proj_parm)
            {
                if (pj_param_f<srs::spar::n>(params, "n", srs::dpar::n, proj_parm.n)
                 && pj_param_f<srs::spar::m>(params, "m", srs::dpar::m, proj_parm.m)) {
                    if (proj_parm.n <= 0 || proj_parm.m < 0)
                        BOOST_THROW_EXCEPTION( projection_exception(error_invalid_m_or_n) );
                } else
                    BOOST_THROW_EXCEPTION( projection_exception(error_invalid_m_or_n) );

                setup(par, proj_parm);
            }

            // Sinusoidal (Sanson-Flamsteed)
            template <typename Parameters, typename T>
            inline void setup_sinu(Parameters& par, par_gn_sinu<T>& proj_parm)
            {
                proj_parm.en = pj_enfn<T>(par.es);

                if (par.es != 0.0) {
                    /* empty */
                } else {
                    proj_parm.n = 1.;
                    proj_parm.m = 0.;
                    setup(par, proj_parm);
                }
            }

            // Eckert VI
            template <typename Parameters, typename T>
            inline void setup_eck6(Parameters& par, par_gn_sinu<T>& proj_parm)
            {
                proj_parm.m = 1.;
                proj_parm.n = 2.570796326794896619231321691;
                setup(par, proj_parm);
            }

            // McBryde-Thomas Flat-Polar Sinusoidal
            template <typename Parameters, typename T>
            inline void setup_mbtfps(Parameters& par, par_gn_sinu<T>& proj_parm)
            {
                proj_parm.m = 0.5;
                proj_parm.n = 1.785398163397448309615660845;
                setup(par, proj_parm);
            }

    }} // namespace detail::gn_sinu
    #endif // doxygen

    /*!
        \brief General Sinusoidal Series projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
        \par Projection parameters
         - m (real)
         - n (real)
        \par Example
        \image html ex_gn_sinu.gif
    */
    template <typename T, typename Parameters>
    struct gn_sinu_spheroid : public detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>
    {
        template <typename Params>
        inline gn_sinu_spheroid(Params const& params, Parameters const& par)
            : detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>(par)
        {
            detail::gn_sinu::setup_gn_sinu(params, this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Sinusoidal (Sanson-Flamsteed) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_sinu.gif
    */
    template <typename T, typename Parameters>
    struct sinu_ellipsoid : public detail::gn_sinu::base_gn_sinu_ellipsoid<T, Parameters>
    {
        template <typename Params>
        inline sinu_ellipsoid(Params const& , Parameters const& par)
            : detail::gn_sinu::base_gn_sinu_ellipsoid<T, Parameters>(par)
        {
            detail::gn_sinu::setup_sinu(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Sinusoidal (Sanson-Flamsteed) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
         - Ellipsoid
        \par Example
        \image html ex_sinu.gif
    */
    template <typename T, typename Parameters>
    struct sinu_spheroid : public detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>
    {
        template <typename Params>
        inline sinu_spheroid(Params const& , Parameters const& par)
            : detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>(par)
        {
            detail::gn_sinu::setup_sinu(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Eckert VI projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
        \par Example
        \image html ex_eck6.gif
    */
    template <typename T, typename Parameters>
    struct eck6_spheroid : public detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>
    {
        template <typename Params>
        inline eck6_spheroid(Params const& , Parameters const& par)
            : detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>(par)
        {
            detail::gn_sinu::setup_eck6(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief McBryde-Thomas Flat-Polar Sinusoidal projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Pseudocylindrical
         - Spheroid
        \par Example
        \image html ex_mbtfps.gif
    */
    template <typename T, typename Parameters>
    struct mbtfps_spheroid : public detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>
    {
        template <typename Params>
        inline mbtfps_spheroid(Params const& , Parameters const& par)
            : detail::gn_sinu::base_gn_sinu_spheroid<T, Parameters>(par)
        {
            detail::gn_sinu::setup_mbtfps(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_gn_sinu, gn_sinu_spheroid, gn_sinu_spheroid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_sinu, sinu_spheroid, sinu_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_eck6, eck6_spheroid, eck6_spheroid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_mbtfps, mbtfps_spheroid, mbtfps_spheroid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(gn_sinu_entry, gn_sinu_spheroid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI2(sinu_entry, sinu_spheroid, sinu_ellipsoid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(eck6_entry, eck6_spheroid)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI(mbtfps_entry, mbtfps_spheroid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(gn_sinu_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(gn_sinu, gn_sinu_entry);
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(sinu, sinu_entry);
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(eck6, eck6_entry);
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(mbtfps, mbtfps_entry);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_GN_SINU_HPP


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*
 Copyright (C) 2008 Roland Stamm
 Copyright (C) 2009 Jose Aparicio

 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 <qle/instruments/cdsoption.hpp>
#include <qle/pricingengines/blackcdsoptionengine.hpp>

#include <ql/exercise.hpp>
#include <ql/instruments/payoffs.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/volatility/equityfx/blackconstantvol.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {

namespace {

class ImpliedVolHelper {
public:
    ImpliedVolHelper(const CdsOption& cdsoption, const Handle<DefaultProbabilityTermStructure>& probability,
                     Real recoveryRate, const Handle<YieldTermStructure>& termStructure, Real targetValue)
        : targetValue_(targetValue) {

        vol_ = boost::shared_ptr<SimpleQuote>(new SimpleQuote(0.0));
        Handle<BlackVolTermStructure> h(
            boost::make_shared<BlackConstantVol>(0, NullCalendar(), Handle<Quote>(vol_), Actual365Fixed()));
        engine_ = boost::shared_ptr<PricingEngine>(
            new QuantExt::BlackCdsOptionEngine(probability, recoveryRate, termStructure, h));
        cdsoption.setupArguments(engine_->getArguments());

        results_ = dynamic_cast<const Instrument::results*>(engine_->getResults());
    }
    Real operator()(Volatility x) const {
        vol_->setValue(x);
        engine_->calculate();
        return results_->value - targetValue_;
    }

private:
    boost::shared_ptr<PricingEngine> engine_;
    Real targetValue_;
    boost::shared_ptr<SimpleQuote> vol_;
    const Instrument::results* results_;
};
} // namespace

CdsOption::CdsOption(const boost::shared_ptr<CreditDefaultSwap>& swap, const boost::shared_ptr<Exercise>& exercise,
                     bool knocksOut, const Real strike, const StrikeType strikeType)
    : Option(boost::shared_ptr<Payoff>(new NullPayoff), exercise), swap_(swap), knocksOut_(knocksOut),
      strike_(strike == Null<Real>() ? swap_->runningSpread() : strike), strikeType_(strikeType) {
    registerWith(swap_);
}

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

void CdsOption::setupExpired() const {
    Instrument::setupExpired();
    riskyAnnuity_ = 0.0;
}

void CdsOption::setupArguments(PricingEngine::arguments* args) const {
    swap_->setupArguments(args);
    Option::setupArguments(args);

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

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

    arguments->swap = swap_;
    arguments->knocksOut = knocksOut_;
    arguments->strike = strike_;
    arguments->strikeType = strikeType_;
}

void CdsOption::fetchResults(const PricingEngine::results* r) const {
    Option::fetchResults(r);
    const CdsOption::results* results = dynamic_cast<const CdsOption::results*>(r);
    QL_ENSURE(results != 0, "wrong results type");
    riskyAnnuity_ = results->riskyAnnuity;
}

Rate CdsOption::atmRate() const { return swap_->fairSpread(); }

Real CdsOption::riskyAnnuity() const {
    calculate();
    QL_REQUIRE(riskyAnnuity_ != Null<Real>(), "risky annuity not provided");
    return riskyAnnuity_;
}

Volatility CdsOption::impliedVolatility(Real targetValue, const Handle<YieldTermStructure>& termStructure,
                                        const Handle<DefaultProbabilityTermStructure>& probability, Real recoveryRate,
                                        Real accuracy, Size maxEvaluations, Volatility minVol,
                                        Volatility maxVol) const {
    calculate();
    QL_REQUIRE(!isExpired(), "instrument expired");

    Volatility guess = 0.10;

    ImpliedVolHelper f(*this, probability, recoveryRate, termStructure, targetValue);
    Brent solver;
    solver.setMaxEvaluations(maxEvaluations);
    return solver.solve(f, accuracy, guess, minVol, maxVol);
}

void CdsOption::arguments::validate() const {
    CreditDefaultSwap::arguments::validate();
    Option::arguments::validate();
    QL_REQUIRE(swap, "CDS not set");
    QL_REQUIRE(exercise, "exercise not set");
}

void CdsOption::results::reset() {
    Option::results::reset();
    riskyAnnuity = Null<Real>();
}
} // namespace QuantExt


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/functional/hash.hpp>  // IWYU pragma: keep
#include <cstddef>
#include <limits>
#include <string>
#include <type_traits>
#include <unordered_map>
#include <utility>

#include "DataStructures/Tensor/Tensor.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/Element.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/WenoGridHelpers.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/WenoHelpers.hpp"
#include "Evolution/DiscontinuousGalerkin/Limiters/WenoOscillationIndicator.hpp"
#include "NumericalAlgorithms/Interpolation/RegularGridInterpolant.hpp"
#include "NumericalAlgorithms/LinearOperators/MeanValue.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "Utilities/Gsl.hpp"

namespace Limiters::Weno_detail {

// Compute the Simple WENO solution for one tensor component
//
// This interface is intended for use in limiters that check the troubled-cell
// indicator independently for each tensor component. These limiters generally
// need to limit only a subset of the tensor components.
//
// When calling `simple_weno_impl`,
// - `interpolator_buffer` may be empty
// - `modified_neighbor_solution_buffer` should contain one DataVector for each
//   neighboring element (i.e. for each entry in `neighbor_data`)
template <typename Tag, size_t VolumeDim, typename PackagedData>
void simple_weno_impl(
    const gsl::not_null<std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        intrp::RegularGrid<VolumeDim>,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>*>
        interpolator_buffer,
    const gsl::not_null<std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, DataVector,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>*>
        modified_neighbor_solution_buffer,
    const gsl::not_null<typename Tag::type*> tensor,
    const double neighbor_linear_weight, const size_t tensor_storage_index,
    const Mesh<VolumeDim>& mesh, const Element<VolumeDim>& element,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data) noexcept {
  // Check that basis is LGL or LG
  // Note that the SimpleWeno implementation should generalize well to other
  // bases beyond LGL or LG, once the oscillation_indicator function has been
  // generalized.
  ASSERT(mesh.basis() == make_array<VolumeDim>(Spectral::Basis::Legendre),
         "Unsupported basis: " << mesh);
  ASSERT(mesh.quadrature() ==
                 make_array<VolumeDim>(Spectral::Quadrature::GaussLobatto) or
             mesh.quadrature() ==
                 make_array<VolumeDim>(Spectral::Quadrature::Gauss),
         "Unsupported quadrature: " << mesh);

  ASSERT(
      modified_neighbor_solution_buffer->size() == neighbor_data.size(),
      "modified_neighbor_solution_buffer->size() = "
          << modified_neighbor_solution_buffer->size()
          << "\nneighbor_data.size() = " << neighbor_data.size()
          << "\nmodified_neighbor_solution_buffer was incorrectly initialized "
             "before calling simple_weno_impl.");

  // Compute the modified neighbor solutions.
  // First extrapolate neighbor data onto local grid points, then shift the
  // extrapolated data so its mean matches the local mean.
  DataVector& component_to_limit = (*tensor)[tensor_storage_index];
  const double local_mean = mean_value(component_to_limit, mesh);
  for (const auto& neighbor_and_data : neighbor_data) {
    const auto& neighbor = neighbor_and_data.first;
    const auto& data = neighbor_and_data.second;

    if (interpolator_buffer->find(neighbor) == interpolator_buffer->end()) {
      // No interpolator found => create one
      const auto& direction = neighbor.first;
      const auto& source_mesh = data.mesh;
      const auto target_1d_logical_coords =
          Weno_detail::local_grid_points_in_neighbor_logical_coords(
              mesh, source_mesh, element, direction);
      interpolator_buffer->insert(std::make_pair(
          neighbor, intrp::RegularGrid<VolumeDim>(source_mesh, mesh,
                                                  target_1d_logical_coords)));
    }

    // Avoid allocations by working directly in the preallocated buffer
    DataVector& buffer = modified_neighbor_solution_buffer->at(neighbor);

    interpolator_buffer->at(neighbor).interpolate(
        make_not_null(&buffer),
        get<Tag>(data.volume_data)[tensor_storage_index]);
    const double neighbor_mean = mean_value(buffer, mesh);
    buffer += (local_mean - neighbor_mean);
  }

  // Sum local and modified neighbor polynomials for the WENO reconstruction
  Weno_detail::reconstruct_from_weighted_sum(
      make_not_null(&component_to_limit), neighbor_linear_weight,
      Weno_detail::DerivativeWeight::PowTwoEll, mesh,
      *modified_neighbor_solution_buffer);
}

// Compute the Simple WENO solution for one tensor
//
// This interface is intended for use in limiters that check the troubled-cell
// indicator for the whole cell, and apply the limiter to all fields.
//
// When calling `simple_weno_impl`,
// - `interpolator_buffer` may be empty
// - `modified_neighbor_solution_buffer` should contain one DataVector for each
//   neighboring element (i.e. for each entry in `neighbor_data`)
template <typename Tag, size_t VolumeDim, typename PackagedData>
void simple_weno_impl(
    const gsl::not_null<std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>,
        intrp::RegularGrid<VolumeDim>,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>*>
        interpolator_buffer,
    const gsl::not_null<std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, DataVector,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>*>
        modified_neighbor_solution_buffer,
    const gsl::not_null<typename Tag::type*> tensor,
    const double neighbor_linear_weight, const Mesh<VolumeDim>& mesh,
    const Element<VolumeDim>& element,
    const std::unordered_map<
        std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>, PackagedData,
        boost::hash<std::pair<Direction<VolumeDim>, ElementId<VolumeDim>>>>&
        neighbor_data) noexcept {
  for (size_t tensor_storage_index = 0; tensor_storage_index < tensor->size();
       ++tensor_storage_index) {
    simple_weno_impl<Tag>(interpolator_buffer,
                          modified_neighbor_solution_buffer, tensor,
                          neighbor_linear_weight, tensor_storage_index, mesh,
                          element, neighbor_data);
  }
}

}  // namespace Limiters::Weno_detail


#ifndef FSTCRITERIONNORMALDIVERGENCE_H
#define FSTCRITERIONNORMALDIVERGENCE_H

/*!======================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------

   \file    criterion_normal_divergence.hpp
   \brief   Implements Divergence (distance) based on normal (gaussian) model to serve as feature selection criterion
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    October 2010
   \version 3.0.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz)
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <iostream>
#include <cmath>
#include "error.hpp"
#include "global.hpp"
#include "criterion_normal.hpp"
#include "indexed_vector.hpp"
#include "indexed_matrix.hpp"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

/*! \brief Implements Divergence (distance) based on normal (gaussian) model to serve as feature selection criterion
    \note Defined for two-class problems only */
template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
class Criterion_Normal_Divergence : public Criterion_Normal<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> {
public:
	typedef Criterion_Normal<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> parent;
	typedef boost::shared_ptr<DATAACCESSOR> PDataAccessor;
	typedef boost::shared_ptr<SUBSET> PSubset;
	Criterion_Normal_Divergence() {notify("Criterion_Normal_Divergence constructor.");}
	virtual ~Criterion_Normal_Divergence() {notify("Criterion_Normal_Divergence destructor.");}

	virtual bool evaluate(RETURNTYPE &result, const PSubset sub);
	virtual bool initialize(PDataAccessor da);

	Criterion_Normal_Divergence* clone() const;
	Criterion_Normal_Divergence* sharing_clone() const {throw fst_error("Criterion_Normal_Divergence::sharing_clone() not supported, use Criterion_Normal_Divergence::clone() instead.");}
	Criterion_Normal_Divergence* stateless_clone() const {throw fst_error("Criterion_Normal_Divergence::stateless_clone() not supported, use Criterion_Normal_Divergence::clone() instead.");}

	virtual std::ostream& print(std::ostream& os) const {os << "Criterion_Normal_Divergence()"; return os;}
private:
	Criterion_Normal_Divergence(const Criterion_Normal_Divergence& cnd); // copy-constructor
private:
	boost::scoped_ptr<Indexed_Vector<DATATYPE,DIMTYPE,SUBSET> > _meandif; // to store mean[i]-mean[j]
	boost::scoped_ptr<Indexed_Vector<DATATYPE,DIMTYPE,SUBSET> > _diag; //
	boost::scoped_ptr<Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET> > _invbuf1; // to store matrix inversions
	boost::scoped_ptr<Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET> > _invbuf2; // to store matrix inversions
	boost::scoped_ptr<Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET> > _LUtemp; // to store matrix LU decomposition
};

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Criterion_Normal_Divergence(const Criterion_Normal_Divergence& cnd) :
	Criterion_Normal<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(cnd)
{
	notify("Criterion_Normal_Divergence copy-constructor.");
	if(cnd._meandif) _meandif.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>(*cnd._meandif));
	if(cnd._diag) _diag.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>(*cnd._diag));
	if(cnd._invbuf1) _invbuf1.reset(new Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>(*cnd._invbuf1));
	if(cnd._invbuf2) _invbuf2.reset(new Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>(*cnd._invbuf2));
	if(cnd._LUtemp) _LUtemp.reset(new Indexed_Matrix<DATATYPE,DIMTYPE,SUBSET>(*cnd._LUtemp));
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>* Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::clone() const
{
	Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> *clone=new Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(*this);
	clone->set_cloned();
	return clone;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::evaluate(RETURNTYPE &result, PSubset sub)
{
	notify("Criterion_Normal_Divergence::evaluate().");
	assert(parent::_model);
	assert(parent::get_n()>0);
	assert(_meandif);
	assert(_diag);
	assert(_invbuf1);
	assert(_invbuf2);
	assert(_LUtemp);
	assert(parent::get_n()==_meandif->get_n());
	assert(parent::get_n()==_diag->get_n());
	assert(parent::get_n()<=_invbuf1->get_n_max());
	assert(parent::get_n()<=_invbuf2->get_n_max());
	assert(parent::get_n()<=_LUtemp->get_n_max());
	assert(sub);

	if(sub->get_d_raw()==0) return false;

	parent::_model->narrow_to(sub);
	_meandif->narrow_to(sub);

	_invbuf1->redim(parent::get_d());
	_invbuf2->redim(parent::get_d());
	_LUtemp->redim(parent::get_d());

	Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET> &_cov1=(parent::_model->get_cov()[0]);
	Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET> &_cov2=(parent::_model->get_cov()[1]);

	_cov1.LUdecompose(*_LUtemp);
	_cov1.invert(*_invbuf1,*_LUtemp);
	_cov2.LUdecompose(*_LUtemp);
	_cov2.invert(*_invbuf2,*_LUtemp);

	RETURNTYPE left=0.0;
	RETURNTYPE pom;
	for(DIMTYPE j=0;j<parent::get_d();j++)
	{
		pom=0.0;
		for(DIMTYPE i=0;i<parent::get_d();i++)
		{
			pom+=_meandif->at(i)*(_invbuf1->at_raw(i,j)+_invbuf2->at_raw(i,j));
		}
		left+=pom*_meandif->at(j);
	}

	RETURNTYPE right=0.0;
	for(DIMTYPE i=0;i<parent::get_d();i++)
	{
		_diag->at_raw(i)=0.0;
		for(DIMTYPE k=0;k<parent::get_d();k++)
		{
			_diag->at_raw(i)+=_invbuf1->at_raw(i,k)*_cov2.at(k,i);
		}
	}
	for(DIMTYPE i=0;i<parent::get_d();i++)
	{
		for(DIMTYPE k=0;k<parent::get_d();k++)
		{
			_diag->at_raw(i)+=_invbuf2->at_raw(i,k)*_cov1.at(k,i);
		}
	}
	for(DIMTYPE i=0;i<parent::get_d();i++) right+=_diag->at_raw(i)-2.0;

	result=0.5*left+0.5*right;
	return true;
}

template<class RETURNTYPE, typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Criterion_Normal_Divergence<RETURNTYPE,DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::initialize(PDataAccessor da)
{
	if(da->getNoOfClasses()!=2) throw fst_error("Criterion_Normal_Divergence() defined for two-class problems only.");
	notify("Criterion_Normal_Divergence::initialize().");
	parent::initialize(da);

	if(!_meandif) _meandif.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>);
	if(parent::get_n()>_meandif->get_n()) _meandif->reset(parent::get_n());
	if(!_diag) _diag.reset(new Indexed_Vector<DATATYPE,DIMTYPE,SUBSET>);
	if(parent::get_n()>_diag->get_n()) _diag->reset(parent::get_n());
	if(!_invbuf1) _invbuf1.reset(new Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET>(parent::get_n()));
	if(parent::get_n()>_invbuf1->get_n()) _invbuf1->reset(parent::get_n());
	if(!_invbuf2) _invbuf2.reset(new Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET>(parent::get_n()));
	if(parent::get_n()>_invbuf2->get_n()) _invbuf2->reset(parent::get_n());
	if(!_LUtemp) _LUtemp.reset(new Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET>(parent::get_n(),true));
	if(parent::get_n()>_LUtemp->get_n()) _LUtemp->reset(parent::get_n(),true);

	_meandif->copy_raw(parent::_model->get_mean()[0]);
	_meandif->subtract_raw(parent::_model->get_mean()[1]);

	return true; }

} // namespace
#endif // FSTCRITERIONNORMALDIVERGENCE_H ///:~


/* -*- c++ -*- */
/*
 * Copyright 2015 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <gnuradio/io_signature.h>
#include <gnuradio/math.h>
#include "corr_est_cc_impl.h"
#include <volk/volk.h>
#include <boost/format.hpp>
#include <boost/math/special_functions/round.hpp>
#include <gnuradio/filter/pfb_arb_resampler.h>
#include <gnuradio/filter/firdes.h>

namespace gr {
  namespace digital {

    corr_est_cc::sptr
    corr_est_cc::make(const std::vector<gr_complex> &symbols,
                      float sps, unsigned int mark_delay,
                      float threshold)
    {
      return gnuradio::get_initial_sptr
        (new corr_est_cc_impl(symbols, sps, mark_delay, threshold));
    }

    corr_est_cc_impl::corr_est_cc_impl(const std::vector<gr_complex> &symbols,
                                       float sps, unsigned int mark_delay,
                                       float threshold)
      : sync_block("corr_est_cc",
                   io_signature::make(1, 1, sizeof(gr_complex)),
                   io_signature::make(1, 2, sizeof(gr_complex))),
        d_src_id(pmt::intern(alias()))
    {
      d_sps = sps;

      // In order to easily support the optional second output,
      // don't deal with an unbounded max number of output items.
      // For the common case of not using the optional second output,
      // this ensures we optimally call the volk routines.
      const size_t nitems = 24*1024;
      set_max_noutput_items(nitems);
      d_corr = (gr_complex *)
               volk_malloc(sizeof(gr_complex)*nitems, volk_get_alignment());
      d_corr_mag = (float *)
                   volk_malloc(sizeof(float)*nitems, volk_get_alignment());

      // Create time-reversed conjugate of symbols
      d_symbols = symbols;
      for(size_t i=0; i < d_symbols.size(); i++) {
          d_symbols[i] = conj(d_symbols[i]);
      }
      std::reverse(d_symbols.begin(), d_symbols.end());

      set_mark_delay(mark_delay);
      set_threshold(threshold);

      // Correlation filter
      d_filter = new kernel::fft_filter_ccc(1, d_symbols);

      // Per comments in gr-filter/include/gnuradio/filter/fft_filter.h,
      // set the block output multiple to the FFT filter kernel's internal,
      // assumed "nsamples", to ensure the scheduler always passes a
      // proper number of samples.
      int nsamples;
      nsamples = d_filter->set_taps(d_symbols);
      set_output_multiple(nsamples);

      // It looks like the kernel::fft_filter_ccc stashes a tail between
      // calls, so that contains our filtering history (I think).  The
      // fft_filter_ccc block (which calls the kernel::fft_filter_ccc) sets
      // the history to 1 (0 history items), so let's follow its lead.
      //set_history(1);

      // We'll (ab)use the history for our own purposes of tagging back in time.
      // Keep a history of the length of the sync word to delay for tagging.
      set_history(d_symbols.size()+1);

      declare_sample_delay(1, 0);
      declare_sample_delay(0, d_symbols.size());

      // Setting the alignment multiple for volk causes problems with the
      // expected behavior of setting the output multiple for the FFT filter.
      // Don't set the alignment multiple.
      //const int alignment_multiple =
      //  volk_get_alignment() / sizeof(gr_complex);
      //set_alignment(std::max(1,alignment_multiple));

      d_scale = 1.0f;
    }

    corr_est_cc_impl::~corr_est_cc_impl()
    {
      delete d_filter;
      volk_free(d_corr);
      volk_free(d_corr_mag);
    }

    std::vector<gr_complex>
    corr_est_cc_impl::symbols() const
    {
      return d_symbols;
    }

    void
    corr_est_cc_impl::set_symbols(const std::vector<gr_complex> &symbols)
    {
      gr::thread::scoped_lock lock(d_setlock);

      d_symbols = symbols;

      // Per comments in gr-filter/include/gnuradio/filter/fft_filter.h,
      // set the block output multiple to the FFT filter kernel's internal,
      // assumed "nsamples", to ensure the scheduler always passes a
      // proper number of samples.
      int nsamples;
      nsamples = d_filter->set_taps(d_symbols);
      set_output_multiple(nsamples);

      // It looks like the kernel::fft_filter_ccc stashes a tail between
      // calls, so that contains our filtering history (I think).  The
      // fft_filter_ccc block (which calls the kernel::fft_filter_ccc) sets
      // the history to 1 (0 history items), so let's follow its lead.
      //set_history(1);

      // We'll (ab)use the history for our own purposes of tagging back in time.
      // Keep a history of the length of the sync word to delay for tagging.
      set_history(d_symbols.size()+1);

      declare_sample_delay(1, 0);
      declare_sample_delay(0, d_symbols.size());

      _set_mark_delay(d_stashed_mark_delay);
      _set_threshold(d_stashed_threshold);
    }

    unsigned int
    corr_est_cc_impl::mark_delay() const
    {
      return d_mark_delay;
    }

    void
    corr_est_cc_impl::_set_mark_delay(unsigned int mark_delay)
    {
      d_stashed_mark_delay = mark_delay;

      if(mark_delay >= d_symbols.size()) {
        d_mark_delay = d_symbols.size()-1;
        GR_LOG_WARN(d_logger, boost::format("set_mark_delay: asked for %1% but due "
                                            "to the symbol size constraints, "
                                            "mark delay set to %2%.") \
                    % mark_delay % d_mark_delay);
      }
      else {
        d_mark_delay = mark_delay;
      }
    }

    void
    corr_est_cc_impl::set_mark_delay(unsigned int mark_delay)
    {
      gr::thread::scoped_lock lock(d_setlock);
      _set_mark_delay(mark_delay);
    }

    float
    corr_est_cc_impl::threshold() const
    {
      return d_thresh;
    }

    void
    corr_est_cc_impl::_set_threshold(float threshold)
    {
      d_stashed_threshold = threshold;
      d_pfa = -logf(1.0f-threshold);
    }

    void
    corr_est_cc_impl::set_threshold(float threshold)
    {
      gr::thread::scoped_lock lock(d_setlock);
      _set_threshold(threshold);
    }

    int
    corr_est_cc_impl::work(int noutput_items,
                           gr_vector_const_void_star &input_items,
                           gr_vector_void_star &output_items)
    {
      gr::thread::scoped_lock lock(d_setlock);

      const gr_complex *in = (gr_complex *)input_items[0];
      gr_complex *out = (gr_complex*)output_items[0];
      gr_complex *corr;
      if (output_items.size() > 1)
          corr = (gr_complex *) output_items[1];
      else
          corr = d_corr;

      // Our correlation filter length
      unsigned int hist_len = history() - 1;

      // Calculate the correlation of the non-delayed input with the
      // known symbols.
      d_filter->filter(noutput_items, &in[hist_len], corr);

      // Find the magnitude squared of the correlation
      volk_32fc_magnitude_squared_32f(&d_corr_mag[0], corr, noutput_items);

      float detection = 0;
      for(int i = 0; i < noutput_items; i++) {
        detection += d_corr_mag[i];
      }
      detection /= static_cast<float>(noutput_items);
      detection *= d_pfa;

      int isps = (int)(d_sps + 0.5f);
      int i = 0;
      while(i < noutput_items) {
        // Look for the correlator output to cross the threshold.
        // Sum power over two consecutive symbols in case we're offset
        // in time. If off by 1/2 a symbol, the peak of any one point
        // is much lower.
        float corr_mag = d_corr_mag[i] + d_corr_mag[i+1];
        if(corr_mag <= 4*detection) {
          i++;
          continue;
        }

        // Go to (just past) the current correlator output peak
        while ((i < (noutput_items-1)) &&
               (d_corr_mag[i] < d_corr_mag[i+1])) {
          i++;
        }
        // Delaying the primary signal output by the matched filter
        // length using history(), means that the the peak output of
        // the matched filter aligns with the start of the desired
        // sync word in the primary signal output.  This corr_start
        // tag is not offset to another sample, so that downstream
        // data-aided blocks (like adaptive equalizers) know exactly
        // where the start of the correlated symbols are.
        add_item_tag(0, nitems_written(0) + i, pmt::intern("corr_start"),
                     pmt::from_double(d_corr_mag[i]), d_src_id);

#if 0
        // Use Parabolic interpolation to estimate a fractional
        // sample delay. There are more accurate methods as
        // the sample delay estimate using this method is biased.
        // But this method is simple and fast.
        // center between [-0.5,0.5] units of samples
        // Paper Reference: "Discrete Time Techniques for Time Delay
        // Estimation" G. Jacovitti and G. Scarano
        double center = 0.0;
        if( i > 0 && i < (noutput_items - 1 )){
          double nom = d_corr_mag[i-1]-d_corr_mag[i+1];
          double denom = 2*(d_corr_mag[i-1]-2*d_corr_mag[i]+d_corr_mag[i+1]);
          center = nom/denom;
        }
#else
        // Calculates the center of mass between the three points around the peak.
        // Estimate is linear.
        double nom = 0, den = 0;
        nom = d_corr_mag[i-1] + 2*d_corr_mag[i] + 3*d_corr_mag[i+1];
        den = d_corr_mag[i-1] + d_corr_mag[i] + d_corr_mag[i+1];
        double center = nom / den;
        center = (center - 2.0); // adjust for bias in center of mass calculation
#endif

        // Estimated scaling factor for the input stream to normalize
        // the output to +/-1.
        uint32_t maxi;
        volk_32fc_index_max_32u_manual(&maxi, (gr_complex*)in, noutput_items, "generic");
        d_scale = 1 / std::abs(in[maxi]);

        // Calculate the phase offset of the incoming signal.
        //
        // The analytic cross-correlation is:
        //
        // 2A*e_bb(t-t_d)*exp(-j*2*pi*f*(t-t_d) - j*phi_bb(t-t_d) - j*theta_c)
        //

        // The analytic auto-correlation's envelope, e_bb(), has its
        // peak at the "group delay" time, t = t_d.  The analytic
        // cross-correlation's center frequency phase shift, theta_c,
        // is determined from the argument of the analytic
        // cross-correlation at the "group delay" time, t = t_d.
        //
        // Taking the argument of the analytic cross-correlation at
        // any other time will include the baseband auto-correlation's
        // phase term, phi_bb(t-t_d), and a frequency dependent term
        // of the cross-correlation, which I don't believe maps simply
        // to expected symbol phase differences.
        float phase = fast_atan2f(corr[i].imag(), corr[i].real());
        int index = i + d_mark_delay;

        add_item_tag(0, nitems_written(0) + index, pmt::intern("phase_est"),
                     pmt::from_double(phase), d_src_id);
        add_item_tag(0, nitems_written(0) + index, pmt::intern("time_est"),
                     pmt::from_double(center), d_src_id);
        // N.B. the appropriate d_corr_mag[] index is "i", not "index".
        add_item_tag(0, nitems_written(0) + index, pmt::intern("corr_est"),
                     pmt::from_double(d_corr_mag[i]), d_src_id);
        add_item_tag(0, nitems_written(0) + index, pmt::intern("amp_est"),
                     pmt::from_double(d_scale), d_src_id);

        if (output_items.size() > 1) {
          // N.B. these debug tags are not offset to avoid walking off out buf
          add_item_tag(1, nitems_written(0) + i, pmt::intern("phase_est"),
                       pmt::from_double(phase), d_src_id);
          add_item_tag(1, nitems_written(0) + i, pmt::intern("time_est"),
                       pmt::from_double(center), d_src_id);
          add_item_tag(1, nitems_written(0) + i, pmt::intern("corr_est"),
                       pmt::from_double(d_corr_mag[i]), d_src_id);
          add_item_tag(1, nitems_written(0) + i, pmt::intern("amp_est"),
                       pmt::from_double(d_scale), d_src_id);
        }

        // Skip ahead to the next potential symbol peak
        // (for non-offset/interleaved symbols)
        i += isps;
      }

      //if (output_items.size() > 1)
      //  add_item_tag(1, nitems_written(0) + noutput_items - 1,
      //               pmt::intern("ce_eow"), pmt::from_uint64(noutput_items),
      //               d_src_id);

      // Delay the output by our correlation filter length so we can
      // tag backwards in time
      memcpy(out, &in[0], sizeof(gr_complex)*noutput_items);

      return noutput_items;
    }

  } /* namespace digital */
} /* namespace gr */


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

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

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

#ifndef BOOST_GEOMETRY_PROJECTIONS_FACTORY_HPP
#define BOOST_GEOMETRY_PROJECTIONS_FACTORY_HPP

#include <map>
#include <string>

#include <boost/shared_ptr.hpp>

#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>
#include <boost/geometry/extensions/gis/projections/parameters.hpp>
#include <boost/geometry/extensions/gis/projections/proj/aea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/aeqd.hpp>
#include <boost/geometry/extensions/gis/projections/proj/airy.hpp>
#include <boost/geometry/extensions/gis/projections/proj/aitoff.hpp>
#include <boost/geometry/extensions/gis/projections/proj/august.hpp>
#include <boost/geometry/extensions/gis/projections/proj/bacon.hpp>
#include <boost/geometry/extensions/gis/projections/proj/bipc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/boggs.hpp>
#include <boost/geometry/extensions/gis/projections/proj/bonne.hpp>
#include <boost/geometry/extensions/gis/projections/proj/cass.hpp>
#include <boost/geometry/extensions/gis/projections/proj/cc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/cea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/chamb.hpp>
#include <boost/geometry/extensions/gis/projections/proj/collg.hpp>
#include <boost/geometry/extensions/gis/projections/proj/crast.hpp>
#include <boost/geometry/extensions/gis/projections/proj/denoy.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eck1.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eck2.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eck3.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eck4.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eck5.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eqc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/eqdc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/etmerc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/fahey.hpp>
#include <boost/geometry/extensions/gis/projections/proj/fouc_s.hpp>
#include <boost/geometry/extensions/gis/projections/proj/gall.hpp>
#include <boost/geometry/extensions/gis/projections/proj/geocent.hpp>
#include <boost/geometry/extensions/gis/projections/proj/geos.hpp>
#include <boost/geometry/extensions/gis/projections/proj/gins8.hpp>
#include <boost/geometry/extensions/gis/projections/proj/gn_sinu.hpp>
#include <boost/geometry/extensions/gis/projections/proj/gnom.hpp>
#include <boost/geometry/extensions/gis/projections/proj/goode.hpp>
#include <boost/geometry/extensions/gis/projections/proj/gstmerc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/hammer.hpp>
#include <boost/geometry/extensions/gis/projections/proj/hatano.hpp>
#include <boost/geometry/extensions/gis/projections/proj/healpix.hpp>
#include <boost/geometry/extensions/gis/projections/proj/krovak.hpp>
#include <boost/geometry/extensions/gis/projections/proj/igh.hpp>
#include <boost/geometry/extensions/gis/projections/proj/imw_p.hpp>
#include <boost/geometry/extensions/gis/projections/proj/isea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/laea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/labrd.hpp>
#include <boost/geometry/extensions/gis/projections/proj/lagrng.hpp>
#include <boost/geometry/extensions/gis/projections/proj/larr.hpp>
#include <boost/geometry/extensions/gis/projections/proj/lask.hpp>
#include <boost/geometry/extensions/gis/projections/proj/latlong.hpp>
#include <boost/geometry/extensions/gis/projections/proj/lcc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/lcca.hpp>
#include <boost/geometry/extensions/gis/projections/proj/loxim.hpp>
#include <boost/geometry/extensions/gis/projections/proj/lsat.hpp>
#include <boost/geometry/extensions/gis/projections/proj/mbtfpp.hpp>
#include <boost/geometry/extensions/gis/projections/proj/mbtfpq.hpp>
#include <boost/geometry/extensions/gis/projections/proj/mbt_fps.hpp>
#include <boost/geometry/extensions/gis/projections/proj/merc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/mill.hpp>
#include <boost/geometry/extensions/gis/projections/proj/mod_ster.hpp>
#include <boost/geometry/extensions/gis/projections/proj/moll.hpp>
#include <boost/geometry/extensions/gis/projections/proj/natearth.hpp>
#include <boost/geometry/extensions/gis/projections/proj/nell.hpp>
#include <boost/geometry/extensions/gis/projections/proj/nell_h.hpp>
#include <boost/geometry/extensions/gis/projections/proj/nocol.hpp>
#include <boost/geometry/extensions/gis/projections/proj/nsper.hpp>
#include <boost/geometry/extensions/gis/projections/proj/nzmg.hpp>
#include <boost/geometry/extensions/gis/projections/proj/ob_tran.hpp>
#include <boost/geometry/extensions/gis/projections/proj/ocea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/oea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/omerc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/ortho.hpp>
#include <boost/geometry/extensions/gis/projections/proj/qsc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/poly.hpp>
#include <boost/geometry/extensions/gis/projections/proj/putp2.hpp>
#include <boost/geometry/extensions/gis/projections/proj/putp3.hpp>
#include <boost/geometry/extensions/gis/projections/proj/putp4p.hpp>
#include <boost/geometry/extensions/gis/projections/proj/putp5.hpp>
#include <boost/geometry/extensions/gis/projections/proj/putp6.hpp>
#include <boost/geometry/extensions/gis/projections/proj/robin.hpp>
#include <boost/geometry/extensions/gis/projections/proj/rouss.hpp>
#include <boost/geometry/extensions/gis/projections/proj/rpoly.hpp>
#include <boost/geometry/extensions/gis/projections/proj/sconics.hpp>
#include <boost/geometry/extensions/gis/projections/proj/somerc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/stere.hpp>
#include <boost/geometry/extensions/gis/projections/proj/sterea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/sts.hpp>
#include <boost/geometry/extensions/gis/projections/proj/tcc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/tcea.hpp>
#include <boost/geometry/extensions/gis/projections/proj/tmerc.hpp>
#include <boost/geometry/extensions/gis/projections/proj/tpeqd.hpp>
#include <boost/geometry/extensions/gis/projections/proj/urm5.hpp>
#include <boost/geometry/extensions/gis/projections/proj/urmfps.hpp>
#include <boost/geometry/extensions/gis/projections/proj/vandg.hpp>
#include <boost/geometry/extensions/gis/projections/proj/vandg2.hpp>
#include <boost/geometry/extensions/gis/projections/proj/vandg4.hpp>
#include <boost/geometry/extensions/gis/projections/proj/wag2.hpp>
#include <boost/geometry/extensions/gis/projections/proj/wag3.hpp>
#include <boost/geometry/extensions/gis/projections/proj/wag7.hpp>
#include <boost/geometry/extensions/gis/projections/proj/wink1.hpp>
#include <boost/geometry/extensions/gis/projections/proj/wink2.hpp>

namespace boost { namespace geometry { namespace projections
{

template <typename LatLong, typename Cartesian, typename Parameters = parameters>
class factory : public detail::base_factory<LatLong, Cartesian, Parameters>
{
private:

    typedef std::map
        <
            std::string,
            boost::shared_ptr
                <
                    detail::factory_entry
                        <
                            LatLong,
                            Cartesian,
                            Parameters
                        >
                >
        > prj_registry;
    prj_registry m_registry;

public:

    factory()
    {
        detail::aea_init(*this);
        detail::aeqd_init(*this);
        detail::airy_init(*this);
        detail::aitoff_init(*this);
        detail::august_init(*this);
        detail::bacon_init(*this);
        detail::bipc_init(*this);
        detail::boggs_init(*this);
        detail::bonne_init(*this);
        detail::cass_init(*this);
        detail::cc_init(*this);
        detail::cea_init(*this);
        detail::chamb_init(*this);
        detail::collg_init(*this);
        detail::crast_init(*this);
        detail::denoy_init(*this);
        detail::eck1_init(*this);
        detail::eck2_init(*this);
        detail::eck3_init(*this);
        detail::eck4_init(*this);
        detail::eck5_init(*this);
        detail::eqc_init(*this);
        detail::eqdc_init(*this);
        detail::etmerc_init(*this);
        detail::fahey_init(*this);
        detail::fouc_s_init(*this);
        detail::gall_init(*this);
        detail::geocent_init(*this);
        detail::geos_init(*this);
        detail::gins8_init(*this);
        detail::gn_sinu_init(*this);
        detail::gnom_init(*this);
        detail::goode_init(*this);
        detail::gstmerc_init(*this);
        detail::hammer_init(*this);
        detail::hatano_init(*this);
        detail::healpix_init(*this);
        detail::krovak_init(*this);
        detail::igh_init(*this);
        detail::imw_p_init(*this);
        detail::isea_init(*this);
        detail::labrd_init(*this);
        detail::laea_init(*this);
        detail::lagrng_init(*this);
        detail::larr_init(*this);
        detail::lask_init(*this);
        detail::latlong_init(*this);
        detail::lcc_init(*this);
        detail::lcca_init(*this);
        detail::loxim_init(*this);
        detail::lsat_init(*this);
        detail::mbtfpp_init(*this);
        detail::mbtfpq_init(*this);
        detail::mbt_fps_init(*this);
        detail::merc_init(*this);
        detail::mill_init(*this);
        detail::mod_ster_init(*this);
        detail::moll_init(*this);
        detail::natearth_init(*this);
        detail::nell_init(*this);
        detail::nell_h_init(*this);
        detail::nocol_init(*this);
        detail::nsper_init(*this);
        detail::nzmg_init(*this);
        detail::ob_tran_init(*this);
        detail::ocea_init(*this);
        detail::oea_init(*this);
        detail::omerc_init(*this);
        detail::ortho_init(*this);
        detail::qsc_init(*this);
        detail::poly_init(*this);
        detail::putp2_init(*this);
        detail::putp3_init(*this);
        detail::putp4p_init(*this);
        detail::putp5_init(*this);
        detail::putp6_init(*this);
        detail::robin_init(*this);
        detail::rouss_init(*this);
        detail::rpoly_init(*this);
        detail::sconics_init(*this);
        detail::somerc_init(*this);
        detail::stere_init(*this);
        detail::sterea_init(*this);
        detail::sts_init(*this);
        detail::tcc_init(*this);
        detail::tcea_init(*this);
        detail::tmerc_init(*this);
        detail::tpeqd_init(*this);
        detail::urm5_init(*this);
        detail::urmfps_init(*this);
        detail::vandg_init(*this);
        detail::vandg2_init(*this);
        detail::vandg4_init(*this);
        detail::wag2_init(*this);
        detail::wag3_init(*this);
        detail::wag7_init(*this);
        detail::wink1_init(*this);
        detail::wink2_init(*this);
    }

    virtual ~factory() {}

    virtual void add_to_factory(std::string const& name,
                    detail::factory_entry<LatLong, Cartesian, Parameters>* sub)
    {
        m_registry[name].reset(sub);
    }

    inline projection<LatLong, Cartesian>* create_new(Parameters const& parameters)
    {
        typename prj_registry::iterator it = m_registry.find(parameters.name);
        if (it != m_registry.end())
        {
            return it->second->create_new(parameters);
        }

        return 0;
    }
};

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_FACTORY_HPP


#include <string>
#include <vector>
#include <sstream>
#include <iostream>
#include <random>
#include <boost/ref.hpp>
#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <glog/logging.h>
#include <hdf5_hl.h>
#include "streamer/streamer.h"
namespace py = boost::python;
namespace np = boost::python::numpy;

namespace {
    using std::istringstream;
    using std::ostringstream;
    using std::string;
    using std::runtime_error;
    using std::cerr;
    using std::endl;
    using std::vector;

    typedef boost::geometry::model::d2::point_xy<float> point_xy;
    typedef boost::geometry::model::polygon<point_xy> Polygon;

    static int PARAMS = 8;
    int random_seed = 2019;
    static float constexpr PIx2 = M_PI * 2;

    // sanity check np::ndarray to make sure they are dense and continuous
    template <typename T=float>
    void check_dense (np::ndarray array, int nd = 0) {
        CHECK(array.get_dtype() == np::dtype::get_builtin<T>());
        if (nd > 0) CHECK(array.get_nd() == nd);
        else nd = array.get_nd();
        int stride = sizeof(T);
        for (int i = 0, off=nd-1; i < nd; ++i, --off) {
            CHECK(array.strides(off) == stride);
            stride *= array.shape(off);
        }
    }

    // allocate an ndarray of given shape, return the pointer to firt element
    template <typename T=float>
    T *alloc_ndarray (py::tuple shape, np::ndarray **ptr) {
        np::ndarray *array = new np::ndarray(np::zeros(shape, np::dtype::get_builtin<T>()));
        check_dense<T>(*array);
        *ptr = array;
        return reinterpret_cast<T *>(array->get_data());
    }

    // Lidar point, xyz and reflectance
    struct __attribute__((__packed__)) Point {
        static int constexpr DIM = 4;
        union {
            float data[4];
            struct {
                float x, y, z, r;
            };
        };
    };

    struct __attribute__((__packed__)) Prior {
        static int constexpr DIM = 5;
        union {
            float data[5];
            struct {
                float l, w, h, z, qt; // length is along x, width is along y
            };
        };

        float t () const {
            return qt * M_PI / 2;
        }
        // qt = 0 1 2 -1
    };

    static float intersect (float min1, float max1, float min2, float max2) {
        // intersection length of range [min1, max1] and [min2, max2]
        float a = std::max(min1, min2);
        float b = std::min(max1, max2);
        if (a <= b) return b-a;
        return 0;
    }

    float norm_angle (float d) {
        if (d >= PIx2) d -= PIx2;
        else if (d <= -PIx2) d += PIx2;
        float ad = d >= 0 ? (d - PIx2) : (d + PIx2);
        if (abs(ad) < abs(d)) return ad;
        return d;
    }

    // 3D rotated box
    struct __attribute__((__packed__)) Box {
        static int constexpr DIM = 8;
        union {
            float data[8];
            struct {
                float x, y, z, h, w, l, t, s;
            };
        };
    public:
        bool sanity_check (Prior const &p, float ax, float ay) const {
            do {
                if (h <= 0) break;
                if (w <= 0) break;
                if (l <= 0) break;
                if (h > 2) break;
                if (w > 2) break;
                if (l > 5) break;
                float d = sqrt(w * w + l * l);
                float dist = sqrt((x-ax)*(x-ax) + (y-ay)*(y-ay));
                if (d < dist) break;
                return true;
            } while (false);
            std::cerr << "BAD " << ax << ' ' << ay << ' ' << h << ' ' << w << ' ' << l << std::endl;
            return false;
        }

        void from_prior (Prior const &p, float ax, float ay) {
            x = ax;
            y = ay;
            z = p.z;
            h = p.h;
            w = p.w;
            l = p.l;
            t = p.t();
            s = 0;
        }

        void augment (float sc, float dx, float dy, float dz, float dt, float s, float c) {
            // s: sin(dt)
            // c: cos(dt)
            float tx = x * sc + dx;
            float ty = y * sc + dy;
            x = c * tx - s * ty;
            y = s * tx + c * ty;
            z = z * sc + dz;
            h *= sc;
            w *= sc;
            l *= sc;
            t += dt;
        }

        void load (float const *params) {
            std::copy(params, params + DIM, data);
        }

        void store (float *params) {
            std::copy(data, data + DIM, params);
        }

        py::tuple make_tuple () const {
            return py::make_tuple(x, y, z, h, w, l, t, s);
        }

        void from_tuple (py::tuple box) {
            x = py::extract<float>(box[0]);
            y = py::extract<float>(box[1]);
            z = py::extract<float>(box[2]);
            h = py::extract<float>(box[3]);
            w = py::extract<float>(box[4]);
            l = py::extract<float>(box[5]);
            t = py::extract<float>(box[6]);
            s = py::extract<float>(box[7]);
        }

        void to_residual (float ax, float ay, Prior const &prior, float *params) const {
            // residual is the regression target
            float d = sqrt(prior.l * prior.l + prior.w * prior.w);

            params[0] = (x - ax);
            params[1] = (y - ay);
            CHECK(abs(params[1]) < 8);
            params[2] = (z - prior.z) / prior.z;
            params[3] = (h - prior.h) / prior.h;
            params[4] = (w - prior.w) / prior.w;
            params[5] = (l - prior.l) / prior.l;
            params[6] = norm_angle(t - prior.t()) / M_PI;
            params[7] = 0;
        }

        void from_residual (float ax, float ay, Prior const &prior, float const *params) {
            float d = sqrt(prior.l * prior.l + prior.w * prior.w);
            CHECK(d < 10);

            x = params[0] + ax;
            y = params[1] + ay;
            z = params[2] * prior.z + prior.z;
            h = params[3] * prior.h + prior.h;
            w = params[4] * prior.w + prior.w;
            l = params[5] * prior.l + prior.l;
            t = params[6] * M_PI + prior.t();
        }

#if 0
        float score_anchor (float ax, float ay, Prior const &prior) const {
            // approximate
            float a = l * l + w * w;
            float d = sqrt(a)/2;
            float a2 = prior.l * prior.l + prior.w * prior.w;
            float d2 = sqrt(a2)/2;
            float i = intersect(x-d, x+d, ax-d2, ax+d2)   // intersection area
                     * intersect(y-d, y+d, ay-d2, ay+d2);
            return i / (a + a2 - i + 0.00001);
        }
#endif

		void polygon (Polygon *poly) const {
			namespace bl = boost::numeric::ublas;
			using namespace boost::geometry;
			bl::matrix<float> mref(2, 2);
            // we are using -t to calculate the matrix
            // this is determined by visualization
			mref(0, 0) = cos(t); mref(0, 1) = -sin(t);
			mref(1, 0) = sin(t); mref(1, 1) = cos(t);

			bl::matrix<float> corners(2, 4);
			float data[] = {w / 2, w / 2, -w / 2, -w / 2,
							l / 2, -l / 2, -l / 2, l / 2};
			std::copy(data, data + 8, corners.data().begin());
			bl::matrix<float> gc = prod(mref, corners);
			for (int i = 0; i < 4; ++i) {
				gc(0, i) += x;
				gc(1, i) += y;
			}

			float points[][2] = {{gc(0, 0), gc(1, 0)},{gc(0, 1), gc(1, 1)},{gc(0, 2), gc(1, 2)},{gc(0, 3), gc(1, 3)},{gc(0, 0), gc(1, 0)}};
			boost::geometry::append(*poly, points);
		}
    };

#if 0
    float iou (Box const &a, Box const &b) {
        // approximate
        float aa = a.x * a.x + a.y * a.y;
        float ab = b.x * b.x + b.y * b.y;   // area
        float ra = sqrt(aa)/2;
        float rb = sqrt(ab)/2;
        float i = intersect(a.x-ra, a.x+ra, b.x-rb, b.x+rb)
                * intersect(a.y-ra, a.y+ra, b.y-rb, b.y+rb);
        return i / (aa + ab - i + 0.00001);
    }
#endif

    float iou (Polygon const &p1, Polygon const &p2) {
        vector<Polygon> in, un;
        boost::geometry::intersection(p1, p2, in);
        boost::geometry::union_(p1, p2, un);
        float inter_area = in.empty() ? 0 : boost::geometry::area(in.front());
        float union_area = boost::geometry::area(un.front());
        return inter_area / union_area;
    }

    // Give unmanaged memory an array-like accessment interface.
    template <typename T>
    class View {
        T *data;
        size_t sz;
    public:
        View (void *p, size_t s): data(reinterpret_cast<T *>(p)), sz(s) {
        }

        View (np::ndarray array) {
			check_dense<float>(array, 2);
			CHECK(array.shape(1) * sizeof(float) == sizeof(T));
			data = reinterpret_cast<T *>(array.get_data());
			sz = array.shape(0);
        }

        View (vector<T> &v) {
            data = &v[0];
            sz = v.size();
        }

        T &operator [] (size_t i) {
            return data[i];
        }

        T const &operator [] (size_t i) const {
            return data[i];
        }
        size_t size () const { return sz; }

        bool empty () const { return sz == 0; }

        T const *begin () const { return data; }
        T const *end () const { return data + sz; }
        T *begin () { return data; }
        T *end () { return data + sz; }
    };

    class H5File {
        hid_t hid;
    public:
        H5File (string const &path) {
            hid = H5Fopen(path.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);
            CHECK(hid >= 0);
        }

        template <typename T>
        void load (char const *name, vector<T> *buffer) {
            herr_t e = H5LTfind_dataset(hid, name);
            int rank;
            e = H5LTget_dataset_ndims(hid, name, &rank);
            CHECK(e >= 0);
            CHECK(rank == 2);
            vector<hsize_t> dims(rank);
            H5T_class_t class_id;
            size_t type_size;
            e = H5LTget_dataset_info(hid, name, &dims[0], &class_id, &type_size);
            CHECK(class_id == H5T_FLOAT);
            CHECK(type_size == sizeof(float));
            CHECK(dims[1] * sizeof(float) == sizeof(T));
            buffer->resize(dims[0]);
            if (buffer->size() > 0) {
                e = H5LTread_dataset_float(hid, name, buffer->at(0).data);
            }
            CHECK(e >= 0);
        }

        ~H5File () {
            herr_t e = H5Fclose(hid);
            CHECK(e >= 0);
        }
    };

    class Voxelizer {
        float x_min, x_max, x_factor;
        float y_min, y_max, y_factor;
        float z_min, z_max, z_factor;
        int nx, ny, nz;
    protected:
        std::default_random_engine rng;
    public:
        Voxelizer (np::ndarray ranges, np::ndarray shape): rng(random_seed) {
            // ranges is a 3 * 2 arrange
            {
                check_dense<float>(ranges, 2);
                CHECK(ranges.shape(0) == 3);
                CHECK(ranges.shape(1) == 2);
                float const *ptr = (float const *)ranges.get_data();
                x_min = *ptr++; x_max = *ptr++;
                y_min = *ptr++; y_max = *ptr++;
                z_min = *ptr++; z_max = *ptr++;
            }
            {
                check_dense<int>(shape, 1);
                CHECK(shape.shape(0) == 3);
                int32_t const *ptr = (int32_t const *)shape.get_data();
                nx = *ptr++; ny = *ptr++; nz = *ptr++;
            }

            float x_range = x_max - x_min;
            float y_range = y_max - y_min;
            float z_range = z_max - z_min;

            // quantization method
            // [min, max], range   -->   [0, n-1]
            // q(x) = round(((x - min)/range) * (n-1))
            //      = round((x - min) * factor)
            x_factor = (nx - 1) / x_range;
            y_factor = (ny - 1) / y_range;
            z_factor = (nz - 1) / z_range;
        }

        void augment_helper (vector<View<Point>> &points_batch, vector<View<Box>> &boxes_batch, int seed) {
            // TODO: per-box rotation
            CHECK(points_batch.size() == boxes_batch.size());
            std::default_random_engine rng1(seed);
            std::uniform_real_distribution<float> shiftxy(-0.5, 0.5);
            std::uniform_real_distribution<float> shiftz(-0.2, 0.2);
            std::uniform_real_distribution<float> shift2(-0.05, 0.05);
            std::uniform_real_distribution<float> scale(0.95, 1.05);
            std::uniform_real_distribution<float> rotate(-M_PI/4, M_PI/4);
            //vector<float> r;
            for (unsigned i = 0; i < points_batch.size(); ++i) {
                float sc = scale(rng);
                float dx = shiftxy(rng), dy = shiftxy(rng), dz = shiftz(rng);
                float dt = rotate(rng);

                View<Point> &points = points_batch[i];
                View<Box> &boxes = boxes_batch[i];
                //r.resize(points.size());
                float s = sin(dt), c = cos(dt);

                for (unsigned j = 0; j < points.size(); ++j) {
                    Point &point = points[j];
                    float x = point.x * sc + dx + shift2(rng);
                    float y = point.y * sc + dy + shift2(rng);
                    point.x = c * x - s * y;
                    point.y = s * x + c * y;
                    point.z = point.z * sc + dz + shift2(rng);
                }

                for (Box &box: boxes) {
                    box.augment(sc, dx, dy, dz, dt, s, c);
                }
            }
        }

        void augment (py::list points_batch, py::list boxes_batch) {
            int batch = py::len(boxes_batch);
            CHECK(batch == py::len(points_batch));
            vector<View<Point>> points_views;
            vector<View<Box>> boxes_views;
            for (int i = 0; i < batch; ++i) {
                np::ndarray points = py::extract<np::ndarray>(points_batch[i]);
                check_dense<float>(points, 2);
                CHECK(points.shape(1) == 4);
                points_views.emplace_back(points);

                // foreach batch
                np::ndarray boxes = py::extract<np::ndarray>(boxes_batch[i]);
                check_dense<float>(boxes, 2);
                CHECK(boxes.shape(1) == 8);
                boxes_views.emplace_back(boxes);
            }
            augment_helper(points_views, boxes_views, rng());
        }

        static int quantize (float v, float min, float factor) {
            return int(round((v-min)*factor));
        }

        // group point by voxels in the grid nx * ny * nz
        // This is the helper function that do the real work.
        void voxelize_points_helper (vector<View<Point>> const &points_batch, int T, bool lock,
                // the return arrays, P for points, M for masks and I for voxel indexes
                np::ndarray **V, np::ndarray **M, np::ndarray **I) {
            // Input:
            //  points_batch:    a batch of point clouds; each as View<Point>.
            //  T:      maximal points in each voxel
            //  lock:   when allocating ndarray, should python thread be locked
            // Output:
            //  *V:     pointer to voxel array,  N * T * C
            //          N non-empty voxels, each voxel has T points, each point has C channels
            //  *M:     pointer to voxel mask,   N * T
            //          For each non empty voxel, T 0/1 values.  1 for those with valid points.
            //  *I:     flatten voxel index,     N
            //          We use this index to put the voxels back into a dense 3D grid

            int batch = points_batch.size();
            int Cin = 4;
            int C = Cin + 3;
            int nv = nx * ny * nz;

            vector<vector<Point const *>> voxels(batch * nv);
            for (int i = 0; i < batch; ++i) {
                // foreach batch
                auto const &points = points_batch[i];

                // we need to drop points from over-crowded voxels in randomm
                // and we do this by shuffling the points before adding them to voxels
                vector<unsigned> index(points.size());
                for (unsigned j = 0; j < index.size(); ++j) index[j] = j;
                std::random_shuffle(index.begin(), index.end());

                for (unsigned j: index) {
                    Point const &point = points[j];

                    // TODO:
                    //      some points may be around the voxel boundary
                    //      we might want to added in those close to boundary as well
                    int ix = quantize(point.x, x_min, x_factor);
                    if (ix < 0 || ix >= nx) continue;
                    int iy = quantize(point.y, y_min, y_factor);
                    if (iy < 0 || iy >= ny) continue;
                    int iz = quantize(point.z, z_min, z_factor);
                    if (iz < 0 || iz >= nz) continue;

                    int cell = i * nv + (ix * ny + iy) * nz + iz;
                    auto &voxel = voxels[cell];
                    if (int(voxel.size()) < T) {
                        voxel.push_back(&point);
                    }
                }
            }

            int N = 0; // non-empty entries
            for (auto const &v: voxels) if (v.size() > 0) ++N;

            float *V_buf, *M_buf;
            int32_t *I_buf;

            {   // allocate ndarrays for return values
                streamer::ScopedGState _(lock);
                V_buf = alloc_ndarray<>(py::make_tuple(N, T, C), V);
                M_buf = alloc_ndarray<>(py::make_tuple(N, T, 1), M);
                I_buf = alloc_ndarray<int32_t>(py::make_tuple(N, 1), I);
            }
            // fill points into voxels
            for (unsigned j = 0; j < voxels.size(); ++j) {
                auto const &voxel = voxels[j];
                if (voxel.empty()) continue;
                *I_buf++ = j;

                float *p = V_buf, *m = M_buf;
                V_buf += T * C; M_buf += T;

                float cx = 0, cy = 0, cz = 0;   // centroid of points in voxel
                for (Point const *point: voxel) {
                    cx += point->x;
                    cy += point->y;
                    cz += point->z;
                }
                cx /= voxel.size();     // we only handle non-empty voxels here
                cy /= voxel.size();     // so we can directly divide
                cz /= voxel.size();
                for (Point const *point: voxel) {
                    *p++ = point->x;    *p++ = point->y;    *p++ = point->z;
                    *p++ = point->r;
                    *p++ = point->x-cx;   *p++ = point->y-cy;   *p++ = point->z-cz;
                    *m++ = 1.0;
                }
            }
        }

        // group point by voxels
        py::tuple voxelize_points (py::list points_batch, int T) {
            int batch = py::len(points_batch);
            CHECK(batch > 0);
            vector<View<Point>> views;
            for (int i = 0; i < batch; ++i) {
                np::ndarray points = py::extract<np::ndarray>(points_batch[i]);
                check_dense<float>(points, 2);
                CHECK(points.shape(1) == 4);
                views.emplace_back(points);
            }
            np::ndarray *V, *M, *I;
            voxelize_points_helper(views, T, false, &V, &M, &I);
            py::tuple tuple = py::make_tuple(*V, *M, *I);
            delete V; delete M; delete I;
            return tuple;
        }

        // Generate one batch of label arrays
        // This is the helper function that do the real work.
        void voxelize_labels_helper (vector<View<Box>> const &boxes_batch, View<Prior> priors,
                int downsize, float lower_th, float upper_th, bool lock,
                np::ndarray **A, np::ndarray **AW, np::ndarray **P, np::ndarray **PW) {

            int batch = boxes_batch.size();

            CHECK(nz % downsize == 0);
            CHECK(ny % downsize == 0);
            CHECK(nx % downsize == 0);
            int lx = nx / downsize;
            int ly = ny / downsize;

            // anchors and masks
            float *pa, *paw, *pp, *ppw;
            {   // allocate returned ndarrays
                streamer::ScopedGState _(lock);
                pa = alloc_ndarray<>(py::make_tuple(batch, lx, ly, priors.size()), A);
                paw = alloc_ndarray<>(py::make_tuple(batch, lx, ly, priors.size()), AW);
                **AW += 1.0;
                pp = alloc_ndarray(py::make_tuple(batch, lx, ly, priors.size() * PARAMS), P);
                ppw = alloc_ndarray(py::make_tuple(batch, lx, ly, priors.size()), PW);
            }

            int count = 0;
            for (auto const &boxes: boxes_batch) {
                vector<Polygon> box_polygons(boxes.size());
                for (unsigned i = 0; i < boxes.size(); ++i) {
                    boxes[i].polygon(&box_polygons[i]);
                }
                for (int x = 0; x < lx; ++x) { for (int y = 0; y < ly; ++y) {
                    float ax(x*downsize/x_factor+x_min), ay(y*downsize/y_factor+y_min);
                    for (auto const &prior: priors) {
                        Box prior_box;
                        Polygon prior_polygon;
                        prior_box.from_prior(prior, ax, ay);
                        prior_box.polygon(&prior_polygon);
                        Box const *best_box = nullptr;
                        float best_d = 0;
                        for (unsigned j = 0; j < boxes.size(); ++j) {
                            float d = iou(prior_polygon, box_polygons[j]);
                            float dt = abs(norm_angle(boxes[j].t - prior.t()));
                            if (dt > 3 * M_PI/8) d = 0;
                            if (d > best_d) {   // find best circle
                                best_d = d;
                                best_box = &boxes[j];
                            }
                        }
                        if (best_box && best_d >= lower_th) {
                            best_box->to_residual(ax, ay, prior, pp);
                            ppw[0] = 1.0; //best_c->weight;
                            ++count;
                            if (best_d < upper_th) {
                                paw[0] = 0;
                            }
                            else {
                                pa[0] = 1;      // to class label
                            }
                        }
                        pa += 1, paw +=1, pp += PARAMS, ppw += 1;
                }}} // prior, y, x
            } // batch
        }

        py::tuple voxelize_labels (py::list boxes_batch, np::ndarray priors, int downsize, float lower_th, float upper_th) {
            vector<View<Box>> views;
            int batch = py::len(boxes_batch);
            CHECK(batch > 0);
            for (int i = 0; i < batch; ++i) {
                // foreach batch
                np::ndarray boxes = py::extract<np::ndarray>(boxes_batch[i]);
                check_dense<float>(boxes, 2);
                CHECK(boxes.shape(1) == 8);
                views.emplace_back(boxes);
            }
            View<Prior> priors_view(priors);
            np::ndarray *A, *AW, *P, *PW;
            voxelize_labels_helper(views, priors_view, downsize, lower_th, upper_th, false, &A, &AW, &P, &PW);
            py::tuple tuple = make_tuple(*A, *AW, *P, *PW);
            delete A; delete AW; delete P; delete PW;
            return tuple;
        }

        py::list generate_boxes (np::ndarray probs, np::ndarray params, np::ndarray priors, float anchor_th) {
            check_dense<float>(probs, 4);
            check_dense<float>(params, 4);
            check_dense<float>(priors, 2);
            int batch = probs.shape(0);
            int lx = probs.shape(1);
            int ly = probs.shape(2);
            CHECK(nx % lx == 0);
            CHECK(ny % ly == 0);
            int downsize = nx / lx;
            CHECK(ny / ly == downsize);
            CHECK(params.shape(1) == lx);
            CHECK(params.shape(2) == ly);
            CHECK(params.shape(3) % probs.shape(3) == 0);
            CHECK(probs.shape(3) == priors.shape(0));
            CHECK(params.shape(3) / probs.shape(3) == PARAMS);

            View<Prior> priors_view(priors);

            py::list list;
            float *pa = (float *)(probs.get_data());
            float *pp = (float *)(params.get_data());
            for (int i = 0; i < batch; ++i) {
                py::list boxes;
                for (int x = 0; x < lx; ++x) { for (int y = 0; y < ly; ++y) {
                    float ax(x*downsize/x_factor+x_min);    // anchor location
                    float ay(y*downsize/y_factor+y_min);
                    // check all the priors
                    for (auto const &prior: priors_view) {
                        float prob = pa[0];
                        if (prob >= anchor_th) {
                            Box box;
                            box.from_residual(ax, ay, prior, pp);
                            box.s = prob;
                            if (box.sanity_check(prior, ax, ay)) {
                                boxes.append(box.make_tuple());
                            }
                        }
                        pa += 1, pp += PARAMS;
                    } // prior
                }} // x, y
                list.append(boxes);
            } // batch
            return list;
        }

        py::list box2polygon (py::tuple b) {
            Box box;
            box.from_tuple(b);
            Polygon poly;
            box.polygon(&poly);
            py::list list;
            for (auto const &p: poly.outer()) {
                float x = (p.x() - x_min) * x_factor;
                float y = (p.y() - y_min) * y_factor;
                list.append(py::make_tuple(x, y));
            }
            return list;
        }
    };

    py::list nms (py::list inputs, float nms_th) {
        py::list outputs;
        for (int i = 0; i < len(inputs); ++i) {
            vector<Box> boxes;
            {
                py::list list = py::extract<py::list>(inputs[i]);
                for (int j = 0; j < len(list); ++j) {
                    Box box;
                    box.from_tuple(py::extract<py::tuple>(list[j]));
                    boxes.push_back(box);
                }
            }
            std::sort(boxes.begin(), boxes.end(), [](Box const &a, Box const &b) { return a.s > b.s; });

            vector<Box> keep;
            vector<Polygon> polygons;
            for (auto const &box: boxes) {
                Polygon polygon;
                box.polygon(&polygon);
                bool good = true;
                for (auto const &polygon2: polygons) {

                    if (iou(polygon, polygon2) >= nms_th) {
                        good = false;
                        break;
                    }
                }
                if (good) {
                    keep.push_back(box);
                    polygons.push_back(polygon);
                }
            }

            {
                py::list list;
                for (auto const &box: keep) {
                    list.append(box.make_tuple());
                }
                outputs.append(list);
            }
        }
        return outputs;
    }

    struct Task {
        py::object *inputs;
        vector<string> paths;
        int seed;
    };

    class Streamer: public streamer::Streamer<Task>, Voxelizer {

        View<Prior> priors;
        int downsize, T;
        float lower_th, upper_th;

        Task *stage1 (py::object *obj) {
            Task *task = new Task;
            task->seed = rng();
            task->inputs = obj;
            {
                streamer::ScopedGState _;
                int len = py::len(*obj);
                for (int i = 0; i < len; ++i) {
                    task->paths.push_back(py::extract<string>((*obj)[i]));
                }
            }
            return task;
        }

        py::object *stage2 (Task *task) {
            // load from H5
            vector<vector<Point>> points;
            vector<vector<Box>> boxes;
            vector<View<Point>> points_views;
            vector<View<Box>> boxes_views;
            // load data from H5 files
            for (string const &path: task->paths) {
                H5File file(path);
                points.emplace_back();
                boxes.emplace_back();
                file.load("points", &points.back());
                file.load("boxes", &boxes.back());
                points_views.emplace_back(points.back());
                boxes_views.emplace_back(boxes.back());
            }
            augment_helper(points_views, boxes_views, task->seed);
            /*
                if (boxes_views.back().size()) {
                float min_z = boxes_views.back()[0].z;
                float max_z = min_z;
                for (Box const &b: boxes_views.back()) {
                    min_z = std::min(min_z, b.z);
                    max_z = std::max(max_z, b.z);
                }
                std::cerr << "MINMAX " << min_z << " " << max_z << std::endl;
                }
            */

            np::ndarray *V, *M, *I;
            np::ndarray *A, *AW, *P, *PW;
            // voxelelize
            voxelize_points_helper(points_views, T, true, &V, &M, &I);
            voxelize_labels_helper(boxes_views, priors, downsize, lower_th, upper_th, true, &A, &AW, &P, &PW);
            py::object *tuple;
            {
                streamer::ScopedGState _;
                tuple = new py::tuple(py::make_tuple(*task->inputs, *V, *M, *I, *A, *AW, *P, *PW));
                delete task->inputs;
                delete V; delete M; delete I; delete A; delete AW; delete P; delete PW;
            }
            delete task;
            return tuple;
        }
    public:
        Streamer (py::object gen,  np::ndarray ranges, np::ndarray shape, np::ndarray priors_, int downsize_, int T_, float lower_th_, float upper_th_, int seed)
            : streamer::Streamer<Task>(gen, 6),
            Voxelizer(ranges, shape),
            priors(priors_),
            downsize(downsize_),
            T(T_),
            lower_th(lower_th_),
            upper_th(upper_th_)
        {
            if (seed > 0) {
                rng = std::default_random_engine(seed);
            }
        }
    };
}

BOOST_PYTHON_MODULE(cpp)
{
    np::initialize();
    py::class_<Voxelizer, boost::noncopyable>("Voxelizer", py::init<np::ndarray, np::ndarray>())
        .def("voxelize_points", &Voxelizer::voxelize_points)
        .def("voxelize_labels", &Voxelizer::voxelize_labels)
        .def("generate_boxes", &Voxelizer::generate_boxes)
        .def("box2polygon", &Voxelizer::box2polygon)
        .def("augment", &Voxelizer::augment)
    ;

    py::class_<Streamer, boost::noncopyable>("Streamer",
                py::init<py::object, np::ndarray, np::ndarray, np::ndarray, int, int, float, float, int>())
        .def("next", &Streamer::next)
    ;

    def("nms", ::nms);
}