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

/*
 Copyright (C) 2009, 2014 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.
*/

#ifndef quantlib_recursive_loss_model_hpp
#define quantlib_recursive_loss_model_hpp

#include <ql/experimental/credit/constantlosslatentmodel.hpp>
#include <ql/experimental/credit/defaultlossmodel.hpp>

#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/bind.hpp>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic pop
#endif
#include <map>
#include <algorithm>

namespace QuantLib {

    /*! Recursive STCDO default loss model for a heterogeneous pool of names.
    The pool names are heterogeneous in their default probabilities, notionals
    and recovery rates. Correlations are given by the latent model.
    The recursive pricing algorithm used here is described in Andersen, Sidenius
    and Basu; "All your hedges in one basket", Risk, November 2003, pages 67-72

        Notice that using copulas other than Gaussian it is only an
        approximation (see remark on p.68).

        \todo Make the loss unit equal to some small fraction depending on the
        portfolio loss weights (notionals and recoveries). As it is now this
        is ok for pricing but not for risk metrics. See the discussion in O'Kane
        18.3.2
        \todo Intengrands should all use the inverted probabilities for
        performance instead of calling the copula inversion with the same vals.
    */
    template<class copulaPolicy>
    class RecursiveLossModel : public DefaultLossModel {
    public:
        RecursiveLossModel(
            const ext::shared_ptr<ConstantLossLatentmodel<copulaPolicy> >& m,
// nope! use max common divisor. See O'Kane. Or give both options at least.
            Size nbuckets  = 1)
        : copula_(m), nBuckets_(nbuckets), wk_() { }
      private:
          /*!
          @param pDefDate Vector of unconditional default probabilities for each
          live name (at the current evaluation date). This is passed instead of
          the date for performance reasons (if in the future other magnitudes
          -e.g. lgd- are contingent on the date they shouldd be passed too).
          */
        Disposable<std::map<Real, Probability> > conditionalLossDistrib(
            const std::vector<Probability>& pDefDate,
            const std::vector<Real>& mktFactor) const;
        Real expectedConditionalLoss(const std::vector<Probability>& pDefDate, //<< never used!!
            const std::vector<Real>& mktFactor) const;
        Disposable<std::vector<Real> > conditionalLossProb(
            const std::vector<Probability>& pDefDate,
            //const Date& date,
            const std::vector<Real>& mktFactor) const;
        //versions using the P-inverse, deprecate the former
        Disposable<std::map<Real, Probability> > conditionalLossDistribInvP(
            const std::vector<Real>& pDefDate,
            //const Date& date,
            const std::vector<Real>& mktFactor) const;
        Real expectedConditionalLossInvP(const std::vector<Real>& pDefDate,
            //const Date& date,
            const std::vector<Real>& mktFactor) const;
    protected:
        void resetModel();
    public:
        /*  Expected tranche Loss calculation.
            This is computed from the first equation on page 70 (not numbered)
            Notice that while we want to compute:
            \f[
            EL(t) = \sum_{l_k}l_k P(l;t) =
              \sum_{l_k}l_k \int P(l_k;t|\omega) d\omega q(\omega)
            \f]
            One can invert the sumation and the integral order to:
            \f[
            EL(t) = \int\,q(\omega)\,d\omega\,\sum_{l_k}\,l_k\,P(l_k;t|\omega) =
              \int\,q(\omega)\,d\omega\,EL(t|\omega)
            \f]
            and this is the way it is integrated here. The recursion formula
            makes it easier this way.
        */
       Real expectedTrancheLoss(const Date& date) const;
       Disposable<std::vector<Real> > lossProbability(const Date& date) const;
       // REMEBER THIS HAS TO BE MOVED TO A DISTRIBUTION OBJECT.............
       Disposable<std::map<Real, Probability> > lossDistribution(
           const Date& d) const;
       // INTEGRATE THEN SEARCH RATHER THAN SEARCH AND THEN INTEGRATE:
       // Here I am not using a search because the point might not be attainable
       //  (loss distrib is not continuous)
       Real percentile(const Date& d, Real percentile) const;
       Real expectedShortfall(const Date& d, Real perctl) const;
    protected:
        const ext::shared_ptr<ConstantLossLatentmodel<copulaPolicy> > copula_;
    private:
        // loss model descriptor members
        const Size nBuckets_;
        mutable std::vector<Real> wk_;
        mutable Real lossUnit_;
        //! name to name factor. In the single factor copula:
        //    correl = beta * beta
        // When constructing through a single correlation number the factor is
        //   taken to be the positive swuare root of this number in the copula.
        ////////in the latent model now: mutable std::vector<Real> oneFactorCorrels_;
        // cached remaining basket magnitudes:
        mutable Real attachAmount_,
            detachAmount_,
            notional_;
        mutable Size remainingBsktSize_;
        mutable std::vector<Real> notionals_;
    };

    typedef RecursiveLossModel<GaussianCopulaPolicy> RecursiveGaussLossModel;

    // Inlines ------------------------------------------------

    template<class CP>
    inline Real RecursiveLossModel<CP>::expectedTrancheLoss(
        const Date& date) const
    {
/*
        std::map<Real, Probability> dist = lossDistribution(date);

        Real expLoss = 0.;
        std::map<Real, Probability>::iterator distIt = dist.begin();

        while(distIt != dist.end()) {
            Real loss = distIt->first * lossUnit_;
            loss = std::max(std::min(loss, detachAmount_)-attachAmount_, 0.);
            // MIN MAX BUGS ....??
            expLoss += loss * distIt->second;
            distIt++;
        }
        return expLoss ;

    ///////////////////////////////////////////////////////////////////////

        // calculate inverted unconditional Ps first so we save the inversion:
        // TO DO : turn to STL algorithm code
        std::vector<Probability> uncDefProb =
            basket_->remainingProbabilities(date);

        return copula_->integratedExpectedValue(
            boost::function<Real (const std::vector<Real>& v1)>(
                boost::bind(
                    &RecursiveLossModel::expectedConditionalLoss,
                    this,
                    boost::cref(uncDefProb),
                    _1)
                )
            );
            */
/**/
        std::vector<Probability> uncDefProb =
            basket_->remainingProbabilities(date);
        std::vector<Real> invProb;
        for(Size i=0; i<uncDefProb.size(); ++i)
           invProb.push_back(copula_->inverseCumulativeY(uncDefProb[i], i));
           ///  invProb.push_back(CP::inverseCumulativeY(uncDefProb[i], i));//<-static call
        return copula_->integratedExpectedValue(
            boost::function<Real (const std::vector<Real>& v1)>(
                boost::bind(
                    &RecursiveLossModel::expectedConditionalLossInvP,
                    this,
                    boost::cref(invProb),
                    _1)
                )
            );

    }

    template<class CP>
    inline Disposable<std::vector<Real> >
        RecursiveLossModel<CP>::lossProbability(const Date& date) const {

        std::vector<Probability> uncDefProb =
            basket_->remainingProbabilities(date);
        return copula_->integratedExpectedValue(
            boost::function<Disposable<std::vector<Real> > (const std::vector<Real>& v1)>(
                boost::bind(
                    &RecursiveLossModel::conditionalLossProb,
                    this,
                    boost::cref(uncDefProb),
                    _1)
                )
            );
    }

    // -------------------------------------------------------------------

    template<class CP>
    void RecursiveLossModel<CP>::resetModel() {
        // basket update:
        notionals_ = basket_->remainingNotionals();
        notional_  = basket_->remainingNotional();
        attachAmount_ = basket_->remainingAttachmentAmount();
        detachAmount_ = basket_->remainingDetachmentAmount();
        // model parameters:
        remainingBsktSize_ = notionals_.size();

        copula_->resetBasket(basket_.currentLink());

        std::vector<Real> lgdsTmp, lgds;
        for(Size i=0; i<remainingBsktSize_; ++i)
            lgds.push_back(notionals_[i]*(1.-copula_->recoveries()[i]));
        lgdsTmp = lgds;
        ///////////////std::remove(lgds.begin(), lgds.end(), 0.);
        lgds.erase(std::remove(lgds.begin(), lgds.end(), 0.), lgds.end());
        lossUnit_ = *(std::min_element(lgds.begin(), lgds.end()))
            / nBuckets_;
        for(Size i=0; i<remainingBsktSize_; ++i)
            wk_.push_back(std::floor(lgdsTmp[i]/lossUnit_ + .5));
    }

    // make it return a distribution object?
    template<class CP>
    Disposable<std::map<Real, Probability> >
        RecursiveLossModel<CP>::lossDistribution(const Date& d) const
    {
        std::map<Real, Probability> distrib;
        std::vector<Real> values  = lossProbability(d);
        Real sum = 0.;
        for(Size i=0; i<values.size(); ++i) {
            distrib.insert(std::make_pair<Real, Probability>(i * lossUnit_,
                sum + values[i]));
            sum += values[i];
        }
        return distrib;
    }

    // Integrate then search rather than search and then integrate?
    // Here I am not using a search because the point might be not attainable
    //   (loss distrib is not continuous)
    template<class CP>
    Real RecursiveLossModel<CP>::percentile(const Date& d,
        Real percentile) const
    {
        std::map<Real, Probability> dist = lossDistribution(d);

        if(dist.begin()->second >=1.) return dist.begin()->first;

        // deterministic case (e.g. date requested is todays date)
        if(dist.size() == 1) return dist.begin()->first;

        if(percentile == 1.) return dist.rbegin()->second;
        if(percentile == 0.) return dist.begin()->second;
        std::map<Real, Probability>::const_iterator itdist = dist.begin();
        while (itdist->second <= percentile) ++itdist;
        Real valPlus = itdist->second;
        Real xPlus   = itdist->first;
        --itdist;  //we're never 1st or last, because of tests above
        Real valMin  = itdist->second;
        Real xMin    = itdist->first;

        // return xPlus-(xPlus-xMin)*(valPlus-percentile)/(valPlus-valMin);
        Real portfLoss =  xPlus-(xPlus-xMin)*(valPlus-percentile)
            /(valPlus-valMin);
        return //remainingNotional_ *
            std::min(std::max(portfLoss - attachAmount_, 0.),
                detachAmount_ - attachAmount_);/////(detach_ - attach_);
    }

    template<class CP>
    Real RecursiveLossModel<CP>::expectedShortfall(const Date& d,
        Real perctl) const
    {
        if(d == Settings::instance().evaluationDate()) return 0.;
        std::map<Real, Probability> distrib = lossDistribution(d);

        std::map<Real, Probability>::iterator itNxt, itDist =
            distrib.begin();
        for(; itDist != distrib.end(); ++itDist)
            if(itDist->second >= perctl) break;
        itNxt = itDist;
        --itDist; // what if we are on the first one?!!!

        // One could linearly triangulate the exact point and get extra
        // precission on the first(broken) period.
        if(itNxt != distrib.end()) {
            Real lossNxt = std::min(std::max(itNxt->first - attachAmount_,
                0.), detachAmount_ - attachAmount_);
            Real lossHere = std::min(std::max(itDist->first - attachAmount_,
                0.), detachAmount_ - attachAmount_);

            Real val =  lossNxt - (itNxt->second - perctl) *
                (lossNxt - lossHere) / (itNxt->second - itDist->second);
            Real suma = (itNxt->second - perctl) * (lossNxt + val) * .5;
            ++itDist; ++itNxt;
            do{
                lossNxt = std::min(std::max(itNxt->first - attachAmount_,
                    0.), detachAmount_ - attachAmount_);
                lossHere = std::min(std::max(itDist->first - attachAmount_,
                    0.), detachAmount_ - attachAmount_);
                suma += .5 * (lossHere + lossNxt) * (itNxt->second -
                    itDist->second);
                ++itDist; ++itNxt;
            }while(itNxt != distrib.end());
            return suma / (1.-perctl);
        }
        return 0.;// well, we are in error....  fix: FAIL
    }

    template<class CP>
    Disposable<std::map<Real, Probability> >
        RecursiveLossModel<CP>::conditionalLossDistrib(
            const std::vector<Probability>& pDefDate,
            //const Date& date,
            const std::vector<Real>& mktFactor) const
    {
        //eq. 10 p.68
        //attainable losses distribution, recursive algorithm
        const std::vector<Probability>& uncDefProb = pDefDate;// alias, remove

        std::map<Real, Probability> pIndepDistrib;
        ////////  K=0
        pIndepDistrib.insert(std::make_pair(0., 1.));
        for(Size iName=0; iName<remainingBsktSize_; ++iName) {
            Probability pDef =
                copula_->conditionalDefaultProbability(uncDefProb[iName], iName,
                                                mktFactor);
            ////// iterate on all possible losses in the distribution:
            std::map<Real, Probability> pDistTemp;
            std::map<Real, Probability>::iterator distIt =
                pIndepDistrib.begin();
            while(distIt != pIndepDistrib.end()) {
              ///   update prob if this name does not default
                std::map<Real, Probability>::iterator matchIt
                    = pDistTemp.find(distIt->first);
                if(matchIt != pDistTemp.end()) {
                    matchIt->second += distIt->second * (1.-pDef);
                }else{
                    pDistTemp.insert(std::make_pair(distIt->first,
                        distIt->second * (1.-pDef)));
                }
              ////   and if it does
                matchIt = pDistTemp.find(distIt->first + wk_[iName]);
                if(matchIt != pDistTemp.end()) {
                    matchIt->second += distIt->second * pDef;
                }else{
                    pDistTemp.insert(std::make_pair(
                        distIt->first+wk_[iName], distIt->second * pDef));
                }
                ++distIt;
            }
           /////  copy back
            pIndepDistrib = pDistTemp;
        }
        /* Apply tranche limits now .... mind you this could be done outside*/
        ////  to be done....
        return pIndepDistrib;
    }

    template<class CP>
    Disposable<std::map<Real, Probability> >
        // twice?! rewrite one in terms of the other, this is a duplicate!
        RecursiveLossModel<CP>::conditionalLossDistribInvP(
            const std::vector<Real>& invpDefDate,
            //const Date& date,
            const std::vector<Real>& mktFactor) const
    {
        // eq. 10 p.68
        // attainable losses distribution, recursive algorithm

        std::map<Real, Probability> pIndepDistrib;
        // K=0
        pIndepDistrib.insert(std::make_pair(0., 1.));
        for(Size iName=0; iName<remainingBsktSize_; ++iName) {
            Probability pDef =
                copula_->conditionalDefaultProbabilityInvP(invpDefDate[iName],
                    iName, mktFactor);

            // iterate on all possible losses in the distribution:
            std::map<Real, Probability> pDistTemp;
            std::map<Real, Probability>::iterator distIt =
                pIndepDistrib.begin();
            while(distIt != pIndepDistrib.end()) {
                // update prob if this name does not default
                std::map<Real, Probability>::iterator matchIt
                    = pDistTemp.find(distIt->first);
                if(matchIt != pDistTemp.end()) {
                    matchIt->second += distIt->second * (1.-pDef);
                }else{
                    pDistTemp.insert(std::make_pair(distIt->first,
                        distIt->second * (1.-pDef)));
                }
                // and if it does
                matchIt = pDistTemp.find(distIt->first + wk_[iName]);
                if(matchIt != pDistTemp.end()) {
                    matchIt->second += distIt->second * pDef;
                }else{
                    pDistTemp.insert(std::make_pair(
                        distIt->first+wk_[iName], distIt->second * pDef));
                }
                ++distIt;
            }
            // copy back
            pIndepDistrib = pDistTemp;
        }
        /* Apply tranche limits now .... mind you this could be done outside*/
        return pIndepDistrib;
    }

    /*
    Bugs here???. The max min on the tranche looks
    wrong. It is better to have a tranche function since that way we can avoid
    adding up losses over all the posible losses rather than just over the
    tranche limits.
    */
    //! Portfolio loss conditional to the market factor value
    template<class CP>
    Real RecursiveLossModel<CP>::expectedConditionalLoss(
        const std::vector<Probability>& pDefDate,
        //const Date& date,
        const std::vector<Real>& mktFactor) const
    {
        std::map<Real, Probability> pIndepDistrib =
            conditionalLossDistrib(pDefDate, mktFactor);

        // get the expected value subject to the value of the market
        //   factor.
        Real expLoss = 0.;
        //---------------------------------------------------------------
        /* This is the original (easy to read) loop which I have partially
             unroll below to take profit of the fact that once we go over
             the tranche top the loss amount is fixed:
        */
        std::map<Real, Probability>::iterator distIt =
            pIndepDistrib.begin();

        while(distIt != pIndepDistrib.end()) {
            Real loss = distIt->first * lossUnit_;
     //       loss = std::max(std::min(loss, detachAmount_)-attachAmount_, 0.);
            loss = std::min(std::max(loss - attachAmount_, 0.),
                detachAmount_ - attachAmount_);
            // MIN MAX BUGS ....??
            expLoss += loss * distIt->second;
            ++distIt;
        }
        return expLoss ;
    }

    template<class CP>
    // again, I am duplicating code.
    Real RecursiveLossModel<CP>::expectedConditionalLossInvP(
                                 const std::vector<Real>& invPDefDate,
                                 //const Date& date,
                                 const std::vector<Real>& mktFactor) const
    {
        std::map<Real, Probability> pIndepDistrib =
            conditionalLossDistribInvP(invPDefDate, mktFactor);

        // get the expected value subject to the value of the market
        //   factor.
        Real expLoss = 0.;
        //---------------------------------------------------------------
        /* This is the original (easy to read) loop which I have partially
             unroll below to take profit of the fact that once we go over
             the tranche top the loss amount is fixed:
        */
        std::map<Real, Probability>::iterator distIt =
            pIndepDistrib.begin();

        while(distIt != pIndepDistrib.end()) {
            Real loss = distIt->first * lossUnit_;
   //         loss = std::max(std::min(loss, detachAmount_)-attachAmount_, 0.);
            loss = std::min(std::max(loss - attachAmount_, 0.),
                detachAmount_ - attachAmount_);
            // MIN MAX BUGS ....???
            expLoss += loss * distIt->second;
            ++distIt;
        }
        return expLoss ;
    }

    template<class CP>
    Disposable<std::vector<Real> > RecursiveLossModel<CP>::conditionalLossProb(
        const std::vector<Probability>& pDefDate,
        //const Date& date,
        const std::vector<Real>& mktFactor) const
    {
        std::map<Real, Probability> pIndepDistrib =
            conditionalLossDistrib(pDefDate, mktFactor);

        std::vector<Real> results;
        std::map<Real, Probability>::iterator distIt = pIndepDistrib.begin();
        while(distIt != pIndepDistrib.end()) {
            //Real loss = distIt->first * loss_unit_
            //                    ;
            //loss = std::max(std::min(loss,
            //    results_.xMax)-results_.xMin, 0.);
            //expLoss += loss * distIt->second;

            results.push_back(distIt->second);
             ++distIt;
        }
        return results;
    }

}

#endif


// ---------------------------------------------------------------------
//
// Copyright (c) 2019 - 2020 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

// Config files

#include <SAMRAI_config.h>

// Headers for basic libMesh objects
#include <libmesh/mesh.h>
#include <libmesh/mesh_generation.h>

// Headers for application-specific algorithm/data structure objects
#include <ibtk/AppInitializer.h>
#include <ibtk/IBTKInit.h>
#include <ibtk/libmesh_utilities.h>

// Set up application namespace declarations
#include <ibamr/app_namespaces.h>

#include <boost/multi_array.hpp>

// Verify that the new function, IBTK::get_max_edge_length, prints out the
// same result as the old get_elem_hmax function (that was internal to
// FEDataManager). Test with several 3D meshes.

void
log_max_edge_length(const ReplicatedMesh& mesh)
{
    const unsigned int dim = mesh.mesh_dimension();
    boost::multi_array<double, 2> X_node;
    for (auto elem_iter = mesh.active_local_elements_begin(); elem_iter != mesh.active_local_elements_end();
         ++elem_iter)
    {
        auto elem = *elem_iter;
        // Don't bother with deformation: just use the material coordinates
        boost::multi_array<double, 2>::extent_gen extent;
        const unsigned int n_nodes = elem->n_nodes();
        X_node.resize(extent[n_nodes][dim]);

        const Node* const* nodes = elem->get_nodes();
        for (unsigned int node_n = 0; node_n < n_nodes; ++node_n)
            for (unsigned int d = 0; d < dim; ++d) X_node[node_n][d] = (*nodes[node_n])(d);

        plog << std::setprecision(12) << get_max_edge_length(elem, X_node) << std::endl;
    }
}

int
main(int argc, char** argv)
{
    // Initialize IBAMR and libraries. Deinitialization is handled by this object as well.
    IBTKInit ibtk_init(argc, argv, MPI_COMM_WORLD);
    const LibMeshInit& init = ibtk_init.getLibMeshInit();

    {
        Pointer<AppInitializer> app_initializer = new AppInitializer(argc, argv, "IB.log");

        Pointer<Database> input_db = app_initializer->getInputDatabase();
        const double radius = 2.0;
        const unsigned int n_refinements = 1;

        {
            plog << "Test 1: tet4" << std::endl;
            ReplicatedMesh mesh(init.comm(), 3);
            MeshTools::Generation::build_cube(mesh, 2, 3, 3, 0.0, 0.7, 0.1, 0.9, 1.0, 2.0, TET4);
            log_max_edge_length(mesh);
        }

        {
            plog << std::endl << "Test 1: tet10" << std::endl;
            ReplicatedMesh mesh(init.comm(), 3);
            MeshTools::Generation::build_cube(mesh, 2, 3, 3, 0.0, 0.7, 0.1, 0.9, 1.0, 2.0, TET10);
            log_max_edge_length(mesh);
        }

        // like the last test, but wobble the triangulation a bit so that the
        // edges are not all straight lines
        {
            plog << std::endl << "Test 2: tet10" << std::endl;
            ReplicatedMesh mesh(init.comm(), 3);
            MeshTools::Generation::build_cube(mesh, 2, 3, 3, 0.0, 0.7, 0.1, 0.9, 1.0, 2.0, TET10);
            for (std::size_t node_n = 0; node_n < mesh.n_nodes(); ++node_n)
            {
                const double y = (*mesh.node_ptr(node_n))(1);
                (*mesh.node_ptr(node_n))(0) += 2 * y * (1 - y);
            }
            log_max_edge_length(mesh);
        }

        {
            plog << std::endl << "Test 3: pyramid5" << std::endl;
            ReplicatedMesh mesh(init.comm(), 3);
            MeshTools::Generation::build_cube(mesh, 2, 3, 3, 0.0, 0.7, 0.1, 0.9, 1.0, 2.0, PYRAMID5);
            log_max_edge_length(mesh);
        }

        {
            plog << std::endl << "Test 4: hex8" << std::endl;
            ReplicatedMesh mesh(init.comm(), 3);
            MeshTools::Generation::build_sphere(mesh, radius, n_refinements, HEX8);
            log_max_edge_length(mesh);
        }

        {
            plog << std::endl << "Test 5: hex27" << std::endl;
            ReplicatedMesh mesh(init.comm(), 3);
            MeshTools::Generation::build_sphere(mesh, radius, n_refinements, HEX27);
            log_max_edge_length(mesh);
        }
    }
} // main


/// HEADER
#include <cslibs_vectormaps/maps/oriented_grid_vector_map.h>

/// COMPONENT
#include <cslibs_boost_geometry/algorithms.h>
#include <cslibs_vectormaps/utility/tools.hpp>
#include <boost/geometry.hpp>

#include <utility>
#include <iostream>

using namespace cslibs_vectormaps;
using namespace cslibs_boost_geometry;

OrientedGridVectorMap::OrientedGridVectorMap(const BoundingBox &bounding,
                                             const double       range,
                                             const double       resolution,
                                             const double       angular_resolution,
                                             const bool         debug)
    : GridVectorMap(bounding, range, resolution, debug),
      angular_resolution_(angular_resolution)
{
    theta_bins_      = std::ceil(2 * M_PI / angular_resolution);
    theta_bins_inv_  = 1.0 / theta_bins_;
    grid_dimensions_ = {rows_, cols_, theta_bins_};
    grid_.resize(grid_dimensions_.globalSize());

    fovs_.resize(theta_bins_);

    // calculate all possible fovs
    for(std::size_t bin = 0; bin < theta_bins_; ++bin) {
        // allow for some leeway
        double low = index2lowerAngle(bin) - theta_bins_inv_ * 0.5 * M_PI;
        double up  = index2upperAngle(bin) + theta_bins_inv_ * 0.5 * M_PI;

        // need to check the four corners to guarantee seeing every vector
        double o = resolution / 2;
        Point corner[4];
        corner[0] = Point(-o, -o);
        corner[1] = Point(+o, -o);
        corner[2] = Point(+o, +o);
        corner[3] = Point(-o, +o);

        double r = 10 * range;
        Point ray_up(r * std::cos(up), r * std::sin(up));
        Point ray_low(r * std::cos(low), r * std::sin(low));

        Polygon polygon;
        for(unsigned int c = 0; c < 4; ++c) {
            boost::geometry::append(polygon.outer(),
                                    corner[c]);
            boost::geometry::append(polygon.outer(),
                                    Point(corner[c].x() + ray_up.x(),
                                          corner[c].y() + ray_up.y()));
            boost::geometry::append(polygon.outer(),
                                    Point(corner[c].x() + ray_low.x(),
                                          corner[c].y() + ray_low.y()));
        }

        boost::geometry::convex_hull(polygon, fovs_[bin]);
    }
}

OrientedGridVectorMap::OrientedGridVectorMap() :
    GridVectorMap()
{
}

unsigned int OrientedGridVectorMap::handleInsertion()
{
    unsigned int assigned = 0;

    const std::size_t rows = grid_dimensions_.size<0>();
    const std::size_t cols = grid_dimensions_.size<1>();

    std::cout << "generating index\n";

    if(valid_area_.outer().empty()) {
#pragma omp parallel for reduction(+:assigned)
        for(unsigned int i = 0 ; i < rows ; ++i) {
            Point min(min_corner_.x() - padding_,
                      min_corner_.y() - padding_ + i * resolution_);
            Point max(min_corner_.x() + padding_ + resolution_,
                      min_corner_.y() + padding_ + (i+1) * resolution_);

            for(unsigned int j = 0 ; j < cols ; ++j) {
                std::cout << "generating " << (i+1) << "\t/ " << (j+1);

                BoundingBox cell_bounding(min, max);
                Point center(min_corner_.x() + (j + 0.5) * resolution_,
                             min_corner_.y() + (i + 0.5) * resolution_);

                VectorPtrs  possible_lines;
                findPossibleLines(center, cell_bounding, possible_lines);
                int dropped = 0;

                dropped += removeHiddenLines(center, cell_bounding, possible_lines);

                // need to check the four corners to guarantee seeing every vector
                std::set<const Vector*> visible_lines;
                double sample_resolution = 1.0;
                unsigned int sampling_steps = std::ceil(resolution_ / sample_resolution);
                double sample_width_step = resolution_ / (double) sampling_steps;

                for(unsigned int si = 0 ; si < sampling_steps ; ++si) {
                    for(unsigned int sj = 0 ; sj < sampling_steps ; ++sj) {

                        Point sample(min_corner_.x() + j * resolution_ + sj * sample_width_step,
                                     min_corner_.y() + i * resolution_ + si * sample_width_step);

                        findVisibleLinesByRaycasting(sample, cell_bounding, possible_lines, visible_lines);
                    }
                }

                dropped += (possible_lines.size() - visible_lines.size());
                possible_lines.assign(visible_lines.begin(), visible_lines.end());

                std::cout << "\t" << possible_lines.size() << " visible lines (" << dropped << " dropped)\n";

                for(unsigned int t = 0 ; t < theta_bins_ ; ++t) {
                    VectorPtrs cell;

                    for(const Vector* line : possible_lines) {
                        /*Point current_center((min.x() + max.x()) * 0.5,
                                             (min.y() + max.y()) * 0.5);*/
                        if(isInView(*line, center, t)) {
                            cell.push_back(line);
                        }
                    }

                    grid_[grid_dimensions_.index(i,j,t)] = cell;

                    ++assigned;
                }

                min.x(min.x() + resolution_);
                max.x(max.x() + resolution_);
            }
        }
    } else {
#pragma omp parallel for reduction(+:assigned)
        for(unsigned int i = 0 ; i < rows ; ++i) {
            Point min(min_corner_.x() - padding_,
                      min_corner_.y() - padding_ + i * resolution_);
            Point pmin(min_corner_.x(),
                       min_corner_.y() + i * resolution_);
            Point max(min_corner_.x()  + padding_ + resolution_,
                      min_corner_.y()  + padding_ + (i+1) * resolution_);
            Point pmax(min_corner_.x() + resolution_,
                       min_corner_.y() + (i+1) * resolution_);

            for(unsigned int j = 0 ; j < cols ; ++j) {
                Polygon     cell_bounding_polygon = algorithms::toPolygon<Point>(pmin, pmax);
                if(algorithms::covered_by<Point>(cell_bounding_polygon, valid_area_)) {
                    std::cout << "generating " << (i+1) << "\t/ " << (j+1);

                    BoundingBox cell_bounding(min, max);
                    Point center(min_corner_.x() + (j + 0.5) * resolution_,
                                 min_corner_.y() + (i + 0.5) * resolution_);

                    VectorPtrs  possible_lines;
                    findPossibleLines(center, cell_bounding, possible_lines);
                    int dropped = 0;

                    dropped += removeHiddenLines(center, cell_bounding, possible_lines);

                    // need to check the four corners too guarantee seeing every vector
                    std::set<const Vector*> visible_lines;
                    double sample_resolution = 1.0;
                    unsigned int sampling_steps = std::ceil(resolution_ / sample_resolution);
                    double sample_width_step = resolution_ / (double) sampling_steps;

                    for(unsigned int si = 0 ; si < sampling_steps ; ++si) {
                        for(unsigned int sj = 0 ; sj < sampling_steps ; ++sj) {

                            Point sample(min_corner_.x() + j * resolution_ + sj * sample_width_step,
                                         min_corner_.y() + i * resolution_ + si * sample_width_step);

                            findVisibleLinesByRaycasting(sample, cell_bounding, possible_lines, visible_lines);
                        }
                    }

                    dropped += (possible_lines.size() - visible_lines.size());
                    possible_lines.assign(visible_lines.begin(), visible_lines.end());

                    std::cout << "\t" << possible_lines.size() << " visible lines (" << dropped << " dropped)\n";

                    for(unsigned int t = 0 ; t < theta_bins_ ; ++t) {
                        VectorPtrs cell;

                        for(const Vector* line : possible_lines) {
                            /*Point current_center((min.x() + max.x()) * 0.5,
                                                 (min.y() + max.y()) * 0.5);*/
                            if(isInView(*line, center, t)) {
                                cell.push_back(line);
                            }
                        }

                        grid_[grid_dimensions_.index(i,j,t)] = cell;

                        ++assigned;
                    }
                } else {
                    if(debug_) {
                        std::cout << "Cell out of valid area!\n";
                    }
                }

                min.x(min.x() + resolution_);
                max.x(max.x() + resolution_);
                pmin.x(pmin.x() + resolution_);
                pmax.x(pmax.x() + resolution_);
            }
        }
    }

    return assigned;
}

void OrientedGridVectorMap::findPossibleLines(const Point &center, const BoundingBox &cell_bounding, VectorPtrs &necessary_lines) const
{
    for(const Vector& line : data_) {
        if(algorithms::touches<Point>(line, cell_bounding)) {
            necessary_lines.push_back(&line);
        }
    }
}

int OrientedGridVectorMap::removeHiddenLines(const Point& center,
                                             const BoundingBox& cell_bounding,
                                             VectorPtrs& possible_lines) const
{
    // need to check the four corners to guarantee seeing every vector
    double o = resolution_ * 0.5;
    Point corner[4];
    corner[0] = Point(center.x() - o, center.y() - o);
    corner[1] = Point(center.x() + o, center.y() - o);
    corner[2] = Point(center.x() + o, center.y() + o);
    corner[3] = Point(center.x() - o, center.y() + o);

    BoundingBox bb(corner[0], corner[2]);

    //VectorPtrs visible_lines = necessary_lines;
    //    std::sort(necessary_lines.begin(), necessary_lines.end(), by_length());

    // find necessary lines
    std::list<const Vector*> visible_lines;//(necessary_lines.begin(), necessary_lines.end());
    std::vector<const Vector*> necessary_lines;

    for(const Vector* line : possible_lines) {
        if(algorithms::touches<Point>(*line, bb)) {
            necessary_lines.push_back(line);
        } else {
            visible_lines.push_back(line);
        }
    }

    //    std::cerr << "necessary: " << necessary_lines.size() << "\tvisible: " << visible_lines.size() << "\n";

    int dropped = 0;
    for(auto line_it = visible_lines.begin(); line_it != visible_lines.end(); ++line_it) {
        const Vector& line = **line_it;

        // now check if *line* completely covers other lines
        for(auto other_it = visible_lines.begin(); other_it != visible_lines.end();) {
            if(other_it == line_it) {
                ++other_it;
                continue;
            }
            const Vector& other = **other_it;

            bool covered = true; // iff every line between corners and *other* crosses *line*
            for(unsigned int c = 0; c < 4; ++c) {
                //std::vector<VectorMap::Point> inter;

                Vector c1(corner[c], other.first);
                if(!boost::geometry::intersects(c1, line)) {
                    covered = false;
                    break;
                }
                Vector c2(corner[c], other.second);
                if(!boost::geometry::intersects(c2, line)) {
                    covered = false;
                    break;
                }
            }

            if(covered) {
                other_it = visible_lines.erase(other_it);
                ++dropped;
            } else {
                ++other_it;
            }
        }
    }

    possible_lines = std::move(necessary_lines);
    possible_lines.insert(possible_lines.end(), visible_lines.begin(), visible_lines.end());

    return dropped;
}

void OrientedGridVectorMap::findVisibleLinesByRaycasting(const Point& center,
                                                         const BoundingBox& cell_bounding,
                                                         const VectorPtrs& possible_lines,
                                                         std::set<const Vector*> &visible) const
{
    double max_range = 1e10;
    double angular_res = algorithms::rad(2.0);
    for(double theta = -M_PI; theta < M_PI; theta += angular_res) {
        Vector ray(center, Point(max_range * std::cos(theta), max_range * std::sin(theta)));
        double min_dist = std::numeric_limits<double>::max();
        const Vector* min_line = nullptr;

        for(const Vector* other : possible_lines) {
            std::vector<Point> intersections;
            boost::geometry::intersection(ray, *other, intersections);
            if(!intersections.empty()) {
                const Point& hit = intersections.front();
                double distance = boost::geometry::distance(center, hit);
                if(distance < min_dist) {
                    min_dist = distance;
                    min_line = other;
                }
            }
        }
        if(min_line != nullptr) {
            visible.insert(min_line);
        }
    }
}

const void* OrientedGridVectorMap::cell(const Point& pos) const
{
    return &grid_[grid_dimensions_.index(row(pos), col(pos), 0)];
}

bool OrientedGridVectorMap::isInView(const Vector& line, Point center, std::size_t t) const
{
    // take the general fov and shift it to center
    Vector linecopy = line;
    boost::geometry::subtract_point(linecopy.first, center);
    boost::geometry::subtract_point(linecopy.second, center);

    return algorithms::touches<Point>(linecopy, fovs_[t]);
}

double OrientedGridVectorMap::angularResolution() const
{
    return angular_resolution_;
}

double OrientedGridVectorMap::minSquaredDistanceNearbyStructure(const Point& pos,
                                                                const void* cell_ptr,
                                                                const double angle) const
{
    unsigned int theta = angle2index(angle);
    double min_squared_dist = std::numeric_limits<double>::max();
    const VectorPtrs& cell = static_cast<const VectorPtrs*>(cell_ptr)[theta];

    for (const Vector* line : cell) {
        double squared_dist = boost::geometry::comparable_distance(pos, *line);
        if (squared_dist < min_squared_dist)
            min_squared_dist = squared_dist;
    }

    return min_squared_dist;
}

double OrientedGridVectorMap::minDistanceNearbyStructure(const Point &pos,
                                                         const unsigned int row,
                                                         const unsigned int col,
                                                         const double angle) const
{
    unsigned int theta = angle2index(angle);
    double min_dist = std::numeric_limits<double>::max();
    auto cell = grid_.at(grid_dimensions_.index(row, col, theta));

    for(const Vector* line : cell) {
        double dist = algorithms::distance<double,Point>(pos, *line);
        if(dist < min_dist)
            min_dist = dist;
    }

    return min_dist;
}

double OrientedGridVectorMap::minSquaredDistanceNearbyStructure(const Point &pos,
                                                                const unsigned int row,
                                                                const unsigned int col,
                                                                const double angle) const
{
    unsigned int theta = angle2index(angle);
    double min_squared_dist = std::numeric_limits<double>::max();
    auto cell = grid_.at(grid_dimensions_.index(row, col, theta));

    for(const Vector* line : cell) {
        double squared_dist = boost::geometry::comparable_distance(pos, *line);
        if(squared_dist < min_squared_dist)
            min_squared_dist = squared_dist;
    }

    return min_squared_dist;
}

unsigned int OrientedGridVectorMap::thetaBins() const
{
    return theta_bins_;
}

double OrientedGridVectorMap::minDistanceNearbyStructure(const Point &pos) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    double min_dist = std::numeric_limits<double>::max();

    for(unsigned int theta = 0 ; theta < grid_dimensions_.size<2>() ; ++theta) {
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));
        for(const Vector* line : cell) {
            double dist = algorithms::distance<double,Point>(pos, *line);
            if(dist < min_dist)
                min_dist = dist;
        }
    }

    if(min_dist == std::numeric_limits<double>::max())
        return -1.0;
    else
        return min_dist;
}

double OrientedGridVectorMap::minSquaredDistanceNearbyStructure(const Point &pos) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    double min_squared_dist = std::numeric_limits<double>::max();

    for(unsigned int theta = 0 ; theta < grid_dimensions_.size<2>() ; ++theta) {
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));
        for(const Vector* line : cell) {
            double squared_dist = boost::geometry::comparable_distance(pos, *line);
            if(squared_dist < min_squared_dist)
                min_squared_dist = squared_dist;
        }
    }

    if(min_squared_dist == std::numeric_limits<double>::max())
        return -1.0;
    else
        return min_squared_dist;
}

bool OrientedGridVectorMap::structureNearby(const Point &pos,
                                            const double thresh) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    for(unsigned int theta = 0 ; theta < grid_dimensions_.size<2>() ; ++theta) {
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));
        for(const Vector* line : cell) {
            double dist = algorithms::distance<double,Point>(pos, *line);
            if(dist > 0.0 && dist < thresh)
                return true;
        }
    }

    return false;
}

bool OrientedGridVectorMap::retrieveFiltered(const Point &pos,
                                             const double orientation,
                                             Vectors &lines) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);
    unsigned int theta = angle2index(orientation);

    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

    // compute the exact bound box for pos
    //  (if the resolution is large, there might be many unnecessary lines)
    Point min(pos.x() - range_, pos.y() - range_);
    Point max(pos.x() + range_, pos.y() + range_);
    BoundingBox bound(min, max);

    for(const Vector* line : cell) {
        // filter out unnecessary lines
        if(algorithms::touches<Point>(*line, bound)) {
            lines.push_back(*line);
        }
    }

    return lines.size() > 0;
}

bool OrientedGridVectorMap::retrieve(const Point &pos,
                                     const double orientation,
                                     Vectors     &lines) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);
    unsigned int theta = angle2index(orientation);

    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

    for(const Vector* line : cell) {
        lines.push_back(*line);
    }

    return lines.size() > 0;
}

bool OrientedGridVectorMap::retrieve(const double x,
                                     const double y,
                                     const double orientation,
                                     Vectors &lines) const
{
    if(tools::coordinatesOutsideMap(x, y, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(y);
    unsigned int col = GridVectorMap::col(x);
    unsigned int theta = angle2index(orientation);

    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

    for(const Vector* line : cell) {
        lines.push_back(*line);
    }

    return lines.size() > 0;
}

bool OrientedGridVectorMap::retrieve(const unsigned int row,
                                     const unsigned int col,
                                     const double angle,
                                     Vectors &lines) const
{
    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col,  angle2index(angle)));

    for(const Vector* line : cell) {
        lines.push_back(*line);
    }

    return lines.size() > 0;
}

bool OrientedGridVectorMap::retrieve(const unsigned int row,
                                     const unsigned int col,
                                     const double min_angle,
                                     const double max_angle,
                                     Vectors &lines) const
{
    std::set<const Vector*> bucket;
    unsigned int index_min = angle2index(min_angle);
    unsigned int index_max = angle2index(max_angle);
    for(unsigned int i = index_min ; i < index_max ; ++i) {
        if(i == theta_bins_)
            i = 0;
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, i));
        bucket.insert(cell.begin(), cell.end());
    }

    for(const Vector* line : bucket) {
        lines.push_back(*line);
    }

    return lines.size() > 0;
}

double OrientedGridVectorMap::intersectScanRay(const Vector &ray,
                                               const void* cell_ptr,
                                               const double angle,
                                               const double max_range) const
{
    const VectorPtrs& cell = static_cast<const VectorPtrs*>(cell_ptr)[angle2index(angle)];
    return algorithms::nearestIntersectionDistance<double, types::Point2d>(ray, cell, max_range);
}

double OrientedGridVectorMap::intersectScanRay(const Vector &ray,
                                               const unsigned int row,
                                               const unsigned int col,
                                               const double angle,
                                               const double max_range) const
{
    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, angle2index(angle)));
    return algorithms::nearestIntersectionDistance<double, types::Point2d>(ray, cell, max_range);
}

double OrientedGridVectorMap::intersectScanRay(const Vector &ray,
                                               const unsigned int row,
                                               const unsigned int col,
                                               const double angle,
                                               Point &p,
                                               const double max_range) const
{
    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, angle2index(angle)));
    types::PointSet2d points;
    algorithms::nearestIntersection<types::Point2d>(ray, cell, points);

    if(points.size() == 0)
        return max_range;

    p = points.front();

    return boost::geometry::distance(ray.first, p);
}

void OrientedGridVectorMap::intersectScanRay(const Vector &ray,
                                             const unsigned int row,
                                             const unsigned int col,
                                             const double ray_angle,
                                             double &distance,
                                             double &angle,
                                             const double max_range,
                                             const double default_angle) const
{
    const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, angle2index(ray_angle)));
    algorithms::nearestIntersectionDistance<double, types::Point2d>(ray, cell,
                                                                distance,
                                                                angle,
                                                                max_range,
                                                                default_angle);
}

bool OrientedGridVectorMap::retrieveFiltered(const Point &pos,
                                             Vectors &lines) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    for(unsigned int theta = 0 ; theta < grid_dimensions_.size<2>() ; ++theta) {
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

        // compute the exact bound box for pos
        //  (if the resolution is large, there might be many unnecessary lines)
        Point min(pos.x() - range_,
                  pos.y() - range_);
        Point max(pos.x() + range_,
                  pos.y() + range_);
        BoundingBox bound(min, max);

        for(const Vector* line : cell) {
            // filter out unnecessary lines
            if(algorithms::touches<Point>(*line, bound)) {
                lines.push_back(*line);
            }
        }
    }

    return lines.size() > 0;
}

bool OrientedGridVectorMap::retrieve(const Point &pos,
                                     Vectors     &lines) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    for(unsigned int theta = 0 ; theta < grid_dimensions_.size<2>() ; ++theta) {
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

        for(const Vector* line : cell) {
            lines.push_back(*line);
        }
    }

    return lines.size() > 0;
}

bool OrientedGridVectorMap::retrieve(const Point &pos,
                                     const double min_angle,
                                     const double max_angle,
                                     Vectors     &lines) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return false;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    const unsigned int min_angle_index = angle2index(min_angle);
    const unsigned int max_angle_index = angle2index(max_angle);

    for(unsigned int theta = min_angle_index ; theta <= max_angle_index ; ++theta) {
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

        for(const Vector* line : cell) {
            lines.push_back(*line);
        }
    }

    return lines.size() > 0;
}

int OrientedGridVectorMap::intersectScanPattern (
        const Point& pos,
        const Vectors &pattern,
        IntersectionSet &intersections) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedGridVectorMap] : Position "
                         "(" << pos.x() << "|" << pos.y() << ")"
                         " to test not within grid structured area!\n";
        }
        return -1;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    int intersection_count = 0;
    ValidPoints result;

    for(const Vector& line : pattern) {
        double dx = line.second.x() - line.first.x();
        double dy = line.second.y() - line.first.y();
        double angle = std::atan2(dy, dx);
        unsigned int theta = angle2index(angle);
        auto &cell = grid_.at(grid_dimensions_.index(row, col, theta));

        result.result.clear();
        result.valid = algorithms::nearestIntersection<Point>(line,
                                                              cell,
                                                              result.result);
        intersection_count += cell.size();
        intersections.push_back(result);
    }

    return intersection_count;
}

int OrientedGridVectorMap::intersectScanPattern (
        const Point& pos,
        const Vectors &pattern,
        std::vector<double> &angles,
        IntersectionSet &intersections) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedGridVectorMap] : Position "
                         "(" << pos.x() << "|" << pos.y() << ")"
                         " to test not within grid structured area!\n";
        }
        return -1;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    int intersection_count = 0;
    ValidPoints result;

    auto lines_ptr = pattern.data();
    auto angles_ptr = angles.data();

    for(unsigned int i = 0 ; i < pattern.size(); ++i) {
        auto &line = *(lines_ptr + i);
        auto angle = *(angles_ptr + i);
        unsigned int theta = angle2index(angle);
        auto &cell = grid_.at(grid_dimensions_.index(row, col, theta));

        result.result.clear();
        result.valid = algorithms::nearestIntersection<Point>(line,
                                                              cell,
                                                              result.result);
        intersection_count += cell.size();
        intersections.push_back(result);
    }

    return intersection_count;
}

void OrientedGridVectorMap::intersectScanPattern(const Point   &pos,
                                                 const Vectors &pattern,
                                                 std::vector<float> &ranges,
                                                 const float default_measurement) const
{
    if(tools::pointOutsideMap(pos, min_corner_, max_corner_)) {
        if(debug_) {
            std::cerr << "[OrientedGridVectorMap] : Position to test "
                         "not within grid structured area!\n";
        }
        return;
    }

    // find the cell of point pos
    unsigned int row = GridVectorMap::row(pos);
    unsigned int col = GridVectorMap::col(pos);

    ranges.resize(pattern.size());

    for(unsigned int i = 0 ; i < pattern.size() ; ++i) {
        const Vector& line = pattern[i];
        double dx = line.second.x() - line.first.x();
        double dy = line.second.y() - line.first.y();
        double angle = std::atan2(dy, dx);
        unsigned int theta = angle2index(angle);
        const VectorPtrs &cell = grid_.at(grid_dimensions_.index(row, col, theta));

        ranges[i] = algorithms::nearestIntersectionDistance<float, types::Point2d>(line, cell, default_measurement);
    }
}

unsigned int OrientedGridVectorMap::sizeAccessStructures() const
{
    unsigned int size = 0;
    for(const VectorPtrs &cell : grid_) {
        size += cell.size() * sizeof(Vector*);
    }
    return size;
}

void OrientedGridVectorMap::doLoad(const YAML::Node &node)
{
    GridVectorMap::doLoad(node);
    angular_resolution_ = node["angular_resolution"].as<double>();
    theta_bins_         = node["theta_bins"].as<std::size_t>();
    theta_bins_inv_     = node["theta_bins_inv"].as<double>();

    grid_dimensions_ = {rows_, cols_, theta_bins_};
}

void OrientedGridVectorMap::doSave(YAML::Node &node) const
{
    GridVectorMap::doSave(node);
    node["map_type"]           = "oriented_grid";
    node["angular_resolution"] = angular_resolution_;
    node["theta_bins"]         = theta_bins_;
    node["theta_bins_inv"]     = theta_bins_inv_;

    assert(node.IsMap());
}


// C++ standard library headers
#include <cmath> // for std::isfinite
#include <cstdlib> // for std::exit
#include <cstring> // for std::strlen
#include <ctime> // for std::clock
#include <iostream> // for std::cout
#include <limits> // for std::numeric_limits

// Boost library headers
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/eigen.hpp>

// RKTK headers
#include "OptimizerDriver.hpp"

template <unsigned Digits>
using boost_float = boost::multiprecision::number<
    boost::multiprecision::cpp_bin_float<Digits,
        boost::multiprecision::digit_base_2>,
    boost::multiprecision::et_off>;

long long int get_positive_integer_argument(char *str) {
    char *end;
    const long long int result = std::strtoll(str, &end, 10);
    const bool read_whole_arg = (
            std::strlen(str) == static_cast<std::size_t>(end - str));
    const int is_positive = (result > 0);
    if (!read_whole_arg || !is_positive) {
        std::cerr << "ERROR: Expected command-line argument '" << str
                  << "' to be a positive integer." << std::endl;
        std::exit(EXIT_FAILURE);
    }
    return result;
}

#define RETURN_PRECISION(T)                                                    \
        return std::numeric_limits<T>::digits;                                 \
    } else if (precision == std::numeric_limits<T>::digits) {                  \
        return std::numeric_limits<T>::digits;                                 \
    }

#define HANDLE_SMALLEST_PRECISION(T)                                           \
    if (precision < std::numeric_limits<T>::digits) {                          \
        std::cout << "Rounding up to " << std::numeric_limits<T>::digits       \
                  << "-bit precision, which is the smallest "                  \
                        "available machine precision." << std::endl;           \
        RETURN_PRECISION(T)

#define HANDLE_MACHINE_PRECISION(T)                                            \
    else if (precision < std::numeric_limits<T>::digits) {                     \
        std::cout << "Rounding up to " << std::numeric_limits<T>::digits       \
                  << "-bit precision, which is the next "                      \
                        "available machine precision." << std::endl;           \
        RETURN_PRECISION(T)

#define HANDLE_EXTENDED_PRECISION(T)                                           \
    else if (precision < std::numeric_limits<T>::digits) {                     \
        std::cout << "Rounding up to " << std::numeric_limits<T>::digits       \
                  << "-bit precision, which is the next "                      \
                        "available extended precision." << std::endl;          \
        RETURN_PRECISION(T)

#define HANDLE_LARGEST_PRECISION(T)                                            \
    HANDLE_EXTENDED_PRECISION(T) else {                                        \
        std::cout << "WARNING: Requested precision exceeds "                   \
                     "available precision. Rounding down to "                  \
                  << std::numeric_limits<T>::digits                            \
                  << "-bit precision, which is the highest "                   \
                     "available extended precision." << std::endl;             \
        return std::numeric_limits<T>::digits;                                 \
    }

int get_precision(int argc, char **argv, int index) {
    if (argc > index) {
        char *end;
        const long long int precision = std::strtoll(argv[index], &end, 10);
        const bool read_whole_arg = (std::strlen(argv[index]) ==
                                     static_cast<std::size_t>(
                                             end - argv[index]));
        const bool is_positive = (precision > 0);
        if (read_whole_arg && is_positive) {
            std::cout << "Requested " << precision << "-bit precision."
                      << std::endl;
            HANDLE_SMALLEST_PRECISION(float)
            HANDLE_MACHINE_PRECISION(double)
            HANDLE_MACHINE_PRECISION(long double)
            HANDLE_EXTENDED_PRECISION(boost_float<128>)
            HANDLE_EXTENDED_PRECISION(boost_float<256>)
            HANDLE_EXTENDED_PRECISION(boost_float<384>)
            HANDLE_EXTENDED_PRECISION(boost_float<512>)
            HANDLE_EXTENDED_PRECISION(boost_float<640>)
            HANDLE_EXTENDED_PRECISION(boost_float<768>)
            HANDLE_EXTENDED_PRECISION(boost_float<896>)
            HANDLE_LARGEST_PRECISION(boost_float<1024>)
        } else {
            std::cout << "WARNING: Could not interpret command-line argument "
                      << argv[index] << " as a positive integer."
                                        " Defaulting to double ("
                      << std::numeric_limits<double>::digits
                      << "-bit) precision." << std::endl;
            return std::numeric_limits<double>::digits;
        }
    } else {
        std::cout << "Defaulting to double ("
                  << std::numeric_limits<double>::digits
                  << "-bit) precision." << std::endl;
        return std::numeric_limits<double>::digits;
    }
}

double get_print_period(int argc, char **argv, int index) {
    if (argc > index) {
        char *end;
        const double print_period = std::strtod(argv[index], &end);
        const bool read_whole_arg = (
                std::strlen(argv[index]) ==
                static_cast<std::size_t>(end - argv[index]));
        const bool is_finite = std::isfinite(print_period);
        const bool is_positive = (print_period >= 0.0);
        if (read_whole_arg && is_finite && is_positive) {
            return print_period;
        }
    }
    return 0.5;
}

int get_print_precision(int argc, char **argv, int index) {
    if (argc > index) {
        char *end;
        const long long print_precision = std::strtoll(argv[index], &end, 10);
        const bool read_whole_arg = (
                std::strlen(argv[index]) ==
                static_cast<std::size_t>(end - argv[index]));
        const bool is_non_negative = (print_precision >= 0);
        const bool in_range = (print_precision <=
                               std::numeric_limits<int>::max());
        if (read_whole_arg && is_non_negative && in_range) {
            return static_cast<int>(print_precision);
        }
    }
    return 0;
}

enum class SearchMode { EXPLORE, REFINE };

template <typename T>
void run_main_loop(int order, std::size_t num_steps,
                   char *filename, SearchMode mode, int print_prec,
                   std::clock_t clocks_between_prints) {
    rktk::OptimizerDriver<T> optimizer(order, num_steps);
    if (mode == SearchMode::REFINE) {
        optimizer.initialize_from_file(std::string(filename));
    }
    std::clock_t last_print_clock;
    reset_d:
    if (mode == SearchMode::EXPLORE) { optimizer.initialize_random(); }
    optimizer.print(print_prec);
    optimizer.write_to_file();
    last_print_clock = std::clock();
    optimizer.set_step_size(65536 * std::numeric_limits<T>::epsilon());
    while (true) {
        if (!optimizer.step()) {
            optimizer.print(print_prec);
            std::cout << "Located candidate local minimum." << std::endl;
            optimizer.write_to_file();
            if (mode == SearchMode::EXPLORE) { goto reset_d; }
            std::exit(EXIT_SUCCESS);
        }
        if (optimizer.get_iteration_count() % 1000 == 0) {
            optimizer.write_to_file();
        }
        if (mode == SearchMode::EXPLORE &&
            optimizer.get_iteration_count() >= 100000000) {
            std::cout << "NOTICE: Exceeded maximum number "
                         "of BFGS iterations. Restarting from "
                         "new random point." << std::endl;
            goto reset_d;
        }
        const std::clock_t current_clock = std::clock();
        if (current_clock - last_print_clock >= clocks_between_prints) {
            optimizer.print(print_prec);
            last_print_clock = current_clock;
        }
    }
}

#define RUN_MAIN_LOOP(T)                                                       \
    if (prec == std::numeric_limits<T>::digits) {                              \
        run_main_loop<T>(order, num_stages, argv[6], mode,                     \
                         print_prec, clocks_between_prints);                   \
    }

int main(int argc, char **argv) {
    if (argc < 3) {
        std::cerr << "Usage: " << argv[0] << " order num-stages num-bits "
                  << "print-period print-precision [input-filename]"
                  << std::endl;
        return EXIT_FAILURE;
    }
    const unsigned long long int order = static_cast<unsigned long long int>(
            get_positive_integer_argument(argv[1]));
    const std::size_t num_stages = static_cast<std::size_t>(
            get_positive_integer_argument(argv[2]));
    if (order > 15) {
        std::cerr << "ERROR: Runge-Kutta methods of order greater than 15 "
                  << "are not yet supported." << std::endl;
        std::exit(EXIT_FAILURE);
    }
    if (order > num_stages) {
        std::cerr << "ERROR: The order of a Runge-Kutta method cannot exceed "
                  << "its number of stages." << std::endl;
        std::exit(EXIT_FAILURE);
    }
    const int prec = get_precision(argc, argv, 3);
    const auto clocks_between_prints = static_cast<std::clock_t>(
            get_print_period(argc, argv, 4) * CLOCKS_PER_SEC);
    const int print_prec = get_print_precision(argc, argv, 5);
    const SearchMode mode = (argc > 6)
                            ? SearchMode::REFINE
                            : SearchMode::EXPLORE;
    RUN_MAIN_LOOP(float)
    else RUN_MAIN_LOOP(double)
    else RUN_MAIN_LOOP(long double)
    else RUN_MAIN_LOOP(boost_float<128>)
    else RUN_MAIN_LOOP(boost_float<256>)
    else RUN_MAIN_LOOP(boost_float<384>)
    else RUN_MAIN_LOOP(boost_float<512>)
    else RUN_MAIN_LOOP(boost_float<640>)
    else RUN_MAIN_LOOP(boost_float<768>)
    else RUN_MAIN_LOOP(boost_float<896>)
    else RUN_MAIN_LOOP(boost_float<1024>)
}


/*
 * Copyright (c) DTAI - KU Leuven – All rights reserved.
 * Proprietary, do not copy or distribute without permission.
 * Written by Pieter Robberechts, 2019
 */

#ifndef DECISIONTREE_CALCULATIONS_HPP
#define DECISIONTREE_CALCULATIONS_HPP

#include <tuple>
#include <vector>
#include <string>
#include <unordered_map>
#include <boost/timer/timer.hpp>
#include "Question.hpp"
#include "Utils.hpp"

using ClassCounter = std::unordered_map<std::string, int>;

namespace Calculations {

std::tuple<const std::vector<size_t>, const std::vector<size_t>> partition(const Data &data, const Question &q, const std::vector<size_t>& indexes); // changed so that it partitions indexes instead the data

const double gini(const ClassCounter& counts, double N);

std::tuple<const double, const Question> find_best_split(const Data &rows, const MetaData &meta, const std::vector<size_t>& indexes);

std::tuple<std::string, double> determine_best_threshold(const Data &data, int col, const std::vector<size_t>& indexes, const ClassCounter& counter);

const ClassCounter copy(const ClassCounter &counter); //used to make a copy of class counter

const ClassCounter empty(const ClassCounter &counter); // used to make an empty copy of class counter

const ClassCounter classCounts(const Data &data, const std::vector<size_t>& indexes);

} // namespace Calculations

#endif //DECISIONTREE_CALCULATIONS_HPP


// Copyright 2018-2019 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/core/lightweight_test.hpp>
#include <boost/histogram/detail/large_int.hpp>
#include <cstdint>
#include <iosfwd>
#include <limits>
#include "utility_meta.hpp"

using namespace boost::histogram;

using large_int = detail::large_int<std::allocator<std::uint64_t>>;

std::ostream& operator<<(std::ostream& os, const large_int& x) {
  os << "large_int" << x.data;
  return os;
}

template <class... Ts>
auto make_mp_int(Ts... ts) {
  large_int r;
  r.data = {static_cast<uint64_t>(ts)...};
  return r;
}

int main() {
  // low-level tools
  {
    uint8_t c = 0;
    BOOST_TEST_EQ(detail::safe_increment(c), true);
    BOOST_TEST_EQ(c, 1);
    c = 255;
    BOOST_TEST_EQ(detail::safe_increment(c), false);
    BOOST_TEST_EQ(c, 255);
    c = 0;
    BOOST_TEST_EQ(detail::safe_radd(c, 255u), true);
    BOOST_TEST_EQ(c, 255);
    c = 1;
    BOOST_TEST_EQ(detail::safe_radd(c, 255u), false);
    BOOST_TEST_EQ(c, 1);
    c = 255;
    BOOST_TEST_EQ(detail::safe_radd(c, 1u), false);
    BOOST_TEST_EQ(c, 255);
  }

  const auto vmax = std::numeric_limits<std::uint64_t>::max();

  BOOST_TEST_EQ(large_int(), 0u);
  BOOST_TEST_EQ(large_int(1u), 1u);
  BOOST_TEST_EQ(large_int(1u), 1.0);
  BOOST_TEST_EQ(large_int(1u), large_int(1u));
  BOOST_TEST_NE(large_int(1u), 2u);
  BOOST_TEST_NE(large_int(1u), 2.0);
  BOOST_TEST_NE(large_int(1u), large_int(2u));
  BOOST_TEST_LT(large_int(1u), 2u);
  BOOST_TEST_LT(large_int(1u), 2.0);
  BOOST_TEST_LT(large_int(1u), large_int(2u));
  BOOST_TEST_LE(large_int(1u), 2u);
  BOOST_TEST_LE(large_int(1u), 2.0);
  BOOST_TEST_LE(large_int(1u), large_int(2u));
  BOOST_TEST_LE(large_int(1u), 1u);
  BOOST_TEST_GT(large_int(1u), 0u);
  BOOST_TEST_GT(large_int(1u), 0.0);
  BOOST_TEST_GT(large_int(1u), large_int(0u));
  BOOST_TEST_GE(large_int(1u), 0u);
  BOOST_TEST_GE(large_int(1u), 0.0);
  BOOST_TEST_GE(large_int(1u), 1u);
  BOOST_TEST_GE(large_int(1u), large_int(0u));
  BOOST_TEST_NOT(large_int(1u) < large_int(1u));
  BOOST_TEST_NOT(large_int(1u) > large_int(1u));
  BOOST_TEST_GT(1, large_int());
  BOOST_TEST_LT(-1, large_int());
  BOOST_TEST_GE(1, large_int());
  BOOST_TEST_LE(-1, large_int());
  BOOST_TEST_NE(1, large_int());

  auto a = large_int();
  ++a;
  BOOST_TEST_EQ(a.data.size(), 1);
  BOOST_TEST_EQ(a.data[0], 1);
  ++a;
  BOOST_TEST_EQ(a.data[0], 2);
  a = vmax;
  BOOST_TEST_EQ(a, vmax);
  BOOST_TEST_EQ(a, static_cast<double>(vmax));
  ++a;
  BOOST_TEST_EQ(a, make_mp_int(0, 1));
  ++a;
  BOOST_TEST_EQ(a, make_mp_int(1, 1));
  a += a;
  BOOST_TEST_EQ(a, make_mp_int(2, 2));
  BOOST_TEST_EQ(a, 2 * static_cast<double>(vmax) + 2);

  // carry once A
  a.data[0] = vmax;
  a.data[1] = 1;
  ++a;
  BOOST_TEST_EQ(a, make_mp_int(0, 2));
  // carry once B
  a.data[0] = vmax;
  a.data[1] = 1;
  a += 1;
  BOOST_TEST_EQ(a, make_mp_int(0, 2));
  // carry once C
  a.data[0] = vmax;
  a.data[1] = 1;
  a += make_mp_int(1, 1);
  BOOST_TEST_EQ(a, make_mp_int(0, 3));

  a.data[0] = vmax - 1;
  a.data[1] = vmax;
  ++a;
  BOOST_TEST_EQ(a, make_mp_int(vmax, vmax));

  // carry two times A
  ++a;
  BOOST_TEST_EQ(a, make_mp_int(0, 0, 1));
  // carry two times B
  a = make_mp_int(vmax, vmax);
  a += 1;
  BOOST_TEST_EQ(a, make_mp_int(0, 0, 1));
  // carry two times C
  a = make_mp_int(vmax, vmax);
  a += large_int(1);
  BOOST_TEST_EQ(a, make_mp_int(0, 0, 1));

  // carry and enlarge
  a = make_mp_int(vmax, vmax);
  a += a;
  BOOST_TEST_EQ(a, make_mp_int(vmax - 1, vmax, 1));

  // add smaller to larger
  a = make_mp_int(1, 1, 1);
  a += make_mp_int(1, 1);
  BOOST_TEST_EQ(a, make_mp_int(2, 2, 1));

  // add larger to smaller
  a = make_mp_int(1, 1);
  a += make_mp_int(1, 1, 1);
  BOOST_TEST_EQ(a, make_mp_int(2, 2, 1));

  a = large_int(1);
  auto b = 1.0;
  BOOST_TEST_EQ(a, b);
  for (unsigned i = 0; i < 80; ++i) {
    b += b;
    BOOST_TEST_NE(a, b);
    a += a;
    BOOST_TEST_EQ(a, b);
  }
  BOOST_TEST_GT(a.data.size(), 1u);

  return boost::report_errors();
}


//   Copyright 2018 Dhruvesh Nikhilkumar Patel
//
//   Licensed under the Apache License, Version 2.0 (the "License");
//   you may not use this file except in compliance with the License.
//   You may obtain a copy of the License at
//
//       http://www.apache.org/licenses/LICENSE-2.0
//
//   Unless required by applicable law or agreed to in writing, software
//   distributed under the License is distributed on an "AS IS" BASIS,
//   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//   See the License for the specific language governing permissions and
//   limitations under the License.
//
/** \file Matrix.hpp
 * \brief Contains typedefs and convinience operators which are wrappers around boost::numeric::ublas::matrix class
 * \todo Provide better way of creating a matrix than zero initialization and then assignment to each entry.
 */
#ifndef _MATRIX_HPP_
#define _MATRIX_HPP_
#include <boost/numeric/ublas/matrix.hpp>

template<typename EntryT>
using Matrix = boost::numeric::ublas::matrix<EntryT>;///< Typedef for the main matrix class
template <typename EntryT>
using Identity_Matrix = boost::numeric::ublas::identity_matrix<EntryT>;

//template <typename MatrixT>
//using productFunctionT = typename boost::numeric::ublas::matrix_matrix_binary_traits<typename MatrixT::value_type, MatrixT, typename MatrixT::value_type, MatrixT>::result_type(*)(const boost::numeric::ublas::matrix_expression<MatrixT>& e1, const boost::numeric::ublas::matrix_expression<MatrixT>& e2);
//
/** \todo Create an operator* as a wrapper so that the requirements from Matrix type can be stated cleanly in terms of operators rather than relying on exact function names
 */
using boost::numeric::ublas::prod; ///< exposing the prod() along with all its instantiations and overloads to global namespace
using boost::numeric::ublas::trans;
using boost::numeric::ublas::element_prod;
namespace boost { namespace numeric { namespace ublas {
   /*! \brief Finds trace of a square matrix
    *
    *  Detailed description of the function
    * \except{strong}{Throws std::invalid_argument if the supplied matrix is now square}
    * \return trace Trace of a square matrix
    */
   template <typename EntryT>
   EntryT trace(const Matrix<EntryT>& M);

   /** \brief Convinience function to check if a matrix is square or not
    */
   template <typename MatrixT>
   bool isSquare(const MatrixT& M);

                                       }
               }

}

//template <typename FieldT>
//class Matrix
//{
//   public:
//       /******* Constructors ********/
//      /*! \brief Takes in no arguments and creates a 0x0 matrix
//       *
//       */
//       Matrix();
//
//       /*! \brief Create a matrix with dimensions num_rows x num_cols with default
//        * initialized entries.
//        * \param num_rows
//        * \param num_cols
//        */
//       Matrix(size_t num_rows,size_t num_cols);
//
//       /*! \brief Use 2d initializer list to construct the matrix
//        *
//        *
//        * \param entires 2d initializer list for example, for FieldT=int, {{23,56},{1,100}}
//        */
//       Matrix(std::initializer_list<std::initializer_list<FieldT>> entries);
//
//   private:
//      std::vector<FieldT> m_entries;
//};

#endif // _MATRIX_HPP_


#include <iostream>
#include <vector>
#include <algorithm>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_2.h>
#include <boost/graph/adjacency_list.hpp>
#include <boost/pending/disjoint_sets.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef int                                            Index;
typedef CGAL::Triangulation_vertex_base_with_info_2<Index,K>   Vb;
typedef CGAL::Triangulation_face_base_2<K>                     Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>            Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds>                  DT;
typedef std::pair<K::Point_2,Index> IPoint;

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
  boost::no_property, boost::property<boost::edge_weight_t, long> >      weighted_graph;
typedef boost::property_map<weighted_graph, boost::edge_weight_t>::type weight_map;
typedef boost::graph_traits<weighted_graph>::edge_descriptor            edge_desc;
typedef boost::graph_traits<weighted_graph>::vertex_descriptor          vertex_desc;

struct Edge {
  int u, v;
  long w;

  bool operator<(Edge e) const {
    return w < e.w;
  }
};

typedef std::vector<Edge> EdgeV;

using namespace std;

int maxFamilies(vector<int> &componentsOfSize, int k) {
  int f = componentsOfSize[k];
  if (k == 4) {
    if (componentsOfSize[3] <= componentsOfSize[1]) {
      f += componentsOfSize[3];
      f += ((componentsOfSize[1] - componentsOfSize[3]) / 2 + componentsOfSize[2]) / 2;
    }
    else {
      f += componentsOfSize[1];
      f += ((componentsOfSize[3] - componentsOfSize[1]) + componentsOfSize[2]) / 2;
    }
  }
  else if (k == 3) {
    if (componentsOfSize[2] <= componentsOfSize[1]) {
      f += componentsOfSize[2];
      f += (componentsOfSize[1] - componentsOfSize[2]) / 3;
    }
    else {
      f += componentsOfSize[1];
      f += (componentsOfSize[2] - componentsOfSize[1]) / 2;
    }
  }
  else if (k == 2) {
    f += componentsOfSize[1] / 2;
  }
  return f;
}

void solve() {
  int n, k, f0;
  long s0;
  cin >> n >> k >> f0 >> s0;

  vector<IPoint> pts(n);
  for (std::size_t i = 0; i < n; ++i) {
    int x, y;
    cin >> x >> y;
    pts[i] = {{x, y}, i};
  }

  DT dt;
  dt.insert(pts.begin(), pts.end());

  EdgeV edges;
  edges.reserve(3*n);
  for (auto e = dt.finite_edges_begin(); e != dt.finite_edges_end(); ++e) {
    int u = e->first->vertex((e->second+1)%3)->info();
    int v = e->first->vertex((e->second+2)%3)->info();
    if (u > v) swap(u, v);
    edges.push_back({u, v, long(dt.segment(e).squared_length())});
  }

  std::sort(edges.begin(), edges.end());

  boost::disjoint_sets_with_storage<> uf(n);
  Index n_components = n;
  vector<int> sizeOfComponent(n, 1);
  vector<int> componentsOfSize(k + 1, 0);
  long s = -1;
  int f = -1;
  componentsOfSize[1] = n;
  vector<long> squaredDistances;
  for (auto e : edges) {
    Index c1 = uf.find_set(e.u);
    Index c2 = uf.find_set(e.v);
    if (c1 != c2) {

      int fMax = maxFamilies(componentsOfSize, k);

      if (fMax >= f0) {
        s = e.w;
      }
      if (e.w >= s0) {
        f = max(f, fMax);
      }

      squaredDistances.push_back(e.w);

      uf.link(c1, c2);
      int c = uf.find_set(c1);

      componentsOfSize[sizeOfComponent[c1]] -= 1;
      componentsOfSize[sizeOfComponent[c2]] -= 1;
      sizeOfComponent[c] = min(k, sizeOfComponent[c1] + sizeOfComponent[c2]);
      componentsOfSize[sizeOfComponent[c]] += 1;

      if (--n_components == 1) break;
    }
  }
  int fMax = maxFamilies(componentsOfSize, k);
  f = max(f, fMax);

  cout << s << " " << f << endl;
}

int main()
{
  ios_base::sync_with_stdio(false);
  int t; cin >> t;
  while (t--) {
    solve();
  }
  return 0;
}


#include <boost/graph/transitive_closure.hpp>


#ifndef RADIUMENGINE_POINT_CLOUD_HPP_
#define RADIUMENGINE_POINT_CLOUD_HPP_

#include <Core/RaCore.hpp>

#include <Eigen/Eigenvalues>

#include <Core/Container/VectorArray.hpp>
#include <Core/Math/LinearAlgebra.hpp>
#include <Core/Math/Math.hpp>
#include <Core/Math/Obb.hpp>

namespace Ra {
namespace Core {
/// This file contains functions operating on any unstructured set of points.
/// If not stated otherwise the functions behaviour is undefined if the set
/// of points is empty.
namespace Geometry {
/// Compute the mean point of a set of points, i.e. the barycenter.
RA_CORE_API inline Math::Vector3 meanPoint( const Container::Vector3Array& pts );

/// Returns a transform computed by PCA of the given set of points.
/// The rotation gives you the principal directions in increasing
/// order of importance (Z = principal direction)
/// The translation is the barycenter of the point set.
RA_CORE_API inline Math::Transform principalAxis( const Container::Vector3Array& pts );

/// Returns the axis-aligned bounding box of a set of points.
/// This function returns an empty AABB if the set of points is
/// empty.
RA_CORE_API inline Math::Aabb aabb( const Container::Vector3Array& pts );

/// Computes an oriented bounding box based on PCA of the points coordinates.
RA_CORE_API inline Math::Obb pcaObb( const Container::Vector3Array& pts );

} // namespace Geometry
} // namespace Core
} // namespace Ra

#include <Core/Geometry/PointCloud.inl>

#endif // RADIUMENGINE_POINT_CLOUD_HPP_


#include <catch2/catch.hpp>

#include <Eigen/Eigenvalues>
#include <solvers/newton_solver.hpp>
#include <utils/eigen_ext.hpp>
#include <utils/not_implemented_error.hpp>

using namespace ipc;
using namespace ipc::rigid;

TEST_CASE("Simple tests of Newton's Method", "[opt][newtons_method]")
{
    int num_vars = GENERATE(1, 10, 100);

    // Setup problem
    // -----------------------------------------------------------------
    class AdHocProblem : public virtual OptimizationProblem {
    public:
        AdHocProblem(int num_vars)
        {
            num_vars_ = num_vars;
            x0.resize(num_vars);
            x0.setRandom();
            is_dof_fixed_ = VectorXb::Zero(num_vars);
        }

        double compute_objective(
            const Eigen::VectorXd& x,
            Eigen::VectorXd& grad_fx,
            Eigen::SparseMatrix<double>& hess_fx,
            bool compute_grad = true,
            bool compute_hess = true) override
        {
            if (compute_grad) {
                grad_fx = x;
            }
            if (compute_hess) {
                hess_fx =
                    Eigen::MatrixXd::Identity(x.rows(), x.rows()).sparseView();
            }
            return x.squaredNorm() / 2.0;
        }

        bool
        has_collisions(const Eigen::VectorXd&, const Eigen::VectorXd&) override
        {
            return false;
        }
        double compute_earliest_toi(
            const Eigen::VectorXd& xi, const Eigen::VectorXd& xj) override
        {
            return std::numeric_limits<double>::infinity();
        }
        bool is_ccd_aligned_with_newton_update() override { return true; }

        const Eigen::VectorXd& starting_point() const { return x0; }
        int num_vars() const override { return num_vars_; }
        const VectorXb& is_dof_fixed() const override { return is_dof_fixed_; }

        double compute_min_distance(const Eigen::VectorXd& x) const override
        {
            return -1;
        }

        /// Get the world coordinates of the vertices
        Eigen::MatrixXd world_vertices(const Eigen::VectorXd& x) const override
        {
            throw NotImplementedError("no vertices");
        }

        /// Get the length of the diagonal of the worlds bounding box
        double world_bbox_diagonal() const override
        {
            throw NotImplementedError("no world bbox diagonal");
        }

        DiagonalMatrixXd mass_matrix() const override
        {
            DiagonalMatrixXd I(num_vars_);
            I.setIdentity();
            return I;
        }
        double average_mass() const override { return 1; }

        double timestep() const override { return 1; }

        int num_vars_;
        VectorXb is_dof_fixed_;
        Eigen::VectorXd x0;
    };

    AdHocProblem problem(num_vars);

    NewtonSolver solver;
    solver.set_problem(problem);
    solver.init_solve(problem.starting_point());
    OptimizationResults results = solver.solve(problem.starting_point());
    REQUIRE(results.success);
    CHECK(results.x.squaredNorm() == Approx(0).margin(1e-6));
    CHECK(results.minf == Approx(0).margin(1e-6));
}

TEST_CASE("Test Newton direction solve", "[opt][newtons_method][newton_dir]")
{
    int num_vars = 1000;
    Eigen::VectorXd x(num_vars);
    x.setRandom();
    // f = x^2
    Eigen::VectorXd gradient = 2 * x;
    Eigen::SparseMatrix<double> hessian =
        SparseDiagonal<double>(2 * Eigen::VectorXd::Ones(num_vars));
    Eigen::VectorXd delta_x;
    ipc::rigid::NewtonSolver solver;
    solver.compute_direction(gradient, hessian, delta_x);
    CHECK((x + delta_x).squaredNorm() == Approx(0.0));
}

TEST_CASE("Test making a matrix SPD", "[opt][make_spd]")
{
    Eigen::SparseMatrix<double> A =
        Eigen::MatrixXd::Random(100, 100).sparseView();
    double mu = ipc::rigid::make_matrix_positive_definite(A);
    CAPTURE(mu);
    auto eig_vals = Eigen::MatrixXd(A).eigenvalues();
    for (int i = 0; i < eig_vals.size(); i++) {
        CHECK(eig_vals(i).real() >= Approx(0.0).margin(1e-12));
    }
}


// The Command Line Interface
// Created by haoming on 10/4/17.
//

#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <numeric>
#include <chrono>

#include <fmlbase/utils.h>
#include <fmlbase/SolverBase.h>
#include <fmlbase/PIS2TASQRTLassoSolver.h>
#include <fmlbase/PISTALassoSolver.h>
#include <fmlbase/PIS2TACMRSolver.h>

#include <Eigen/Dense>

using namespace Eigen;
using namespace std;
using namespace std::chrono;

void runCLITask(const std::string &task_path)
{
    fmlbase::utils::FmlParam param(task_path);

    vector<double> times;

    if(param.getStrArg("algorithm") == "sqrtlasso") {
        fmlbase::PIS2TASQRTLassoSolver solver(param);
        solver.initialize();
        MatrixXd* testx;
        fmlbase::utils::readCsvMat(testx, param.getStrArg("rootpath") + "/"+param.getStrArg("testdata"));
        VectorXd* testy;
        fmlbase::utils::readCsvVec(testy, param.getStrArg("rootpath") + "/"+param.getStrArg("testlabel"));

        for (int i = 0; i < param.getIntArg("nexp"); ++i) {
            solver.reinitialize();
            auto begin = std::chrono::steady_clock::now();
            solver.train();
            auto end = std::chrono::steady_clock::now();
            auto diff = 1. * (end - begin).count() * nanoseconds::period::num / nanoseconds::period::den;
            times.emplace_back(diff);
            cout << i << "th trail, training time (/s): " << diff << endl;
            cout << i << "train error: " << solver.eval() << endl;
            cout << i << "test error: " << solver.eval(*testx, *testy) << endl;
        }
    }
    if(param.getStrArg("algorithm") == "lasso") {
        fmlbase::PISTALassoSolver solver(param);
        solver.initialize();
        MatrixXd* testx;
        fmlbase::utils::readCsvMat(testx, param.getStrArg("rootpath") + "/"+param.getStrArg("testdata"));
        VectorXd* testy;
        fmlbase::utils::readCsvVec(testy, param.getStrArg("rootpath") + "/"+param.getStrArg("testlabel"));

        for (int i = 0; i < param.getIntArg("nexp"); ++i) {
            solver.reinitialize();
            auto begin = std::chrono::steady_clock::now();
            solver.train();
            auto end = std::chrono::steady_clock::now();
            auto diff = 1. * (end - begin).count() * nanoseconds::period::num / nanoseconds::period::den;
            times.emplace_back(diff);
            cout << i << "th trail, training time (/s): " << diff << endl;
            cout << i << "train error: " << solver.eval() << endl;
            cout << i << "test error: " << solver.eval(*testx, *testy) << endl;
        }
    }
    if(param.getStrArg("algorithm") == "CMR") {
        fmlbase::PIS2TACMRSolver solver(param);
        solver.initialize();
        MatrixXd* testx;
        fmlbase::utils::readCsvMat(testx, param.getStrArg("rootpath") + "/"+param.getStrArg("testdata"));
        MatrixXd* testy;
        fmlbase::utils::readCsvMat(testy, param.getStrArg("rootpath") + "/"+param.getStrArg("testlabel"));

        for (int i = 0; i < param.getIntArg("nexp"); ++i) {
            solver.reinitialize();
            auto begin = std::chrono::steady_clock::now();
            solver.train();
            auto end = std::chrono::steady_clock::now();
            auto diff = 1. * (end - begin).count() * nanoseconds::period::num / nanoseconds::period::den;
            times.emplace_back(diff);
            cout << i << "th trail, training time (/s): " << diff << endl;
            cout << i << "train error: " << solver.eval() << endl;
            cout << i << "test error: " << solver.eval(*testx, *testy) << endl;
        }
    }
    if(param.getStrArg("algorithm") == "SPME") {
        MatrixXd* S;
        fmlbase::utils::readCsvMat(S, param.getStrArg("rootpath") + "/"+param.getStrArg("data"));
        auto nfeature = S->cols();
        vector<fmlbase::PIS2TASQRTLassoSolver*> solver_vec;
        double errors = 0;
        for (int j = 0; j < nfeature; ++j) {
            auto temp_col = S->col(0);
            S->col(0) = S->col(j);
            S->col(j) = temp_col;
            fmlbase::PIS2TASQRTLassoSolver *new_solver = new fmlbase::PIS2TASQRTLassoSolver(param, S->rightCols(nfeature-1), S->col(0));
            new_solver->initialize();
            solver_vec.push_back(new_solver);
        }
        cout << "Construction Completed \n";
        for (int i = 0; i < param.getIntArg("nexp"); ++i) {
            for (int j = 0; j < nfeature; ++j)
                solver_vec[j]->reinitialize();
            auto begin = std::chrono::steady_clock::now();
            for (int j = 0; j < nfeature; ++j)
            {
                solver_vec[j]->train();
                //cout << "trained"<<j<<"th solver \n";
            }
            auto end = std::chrono::steady_clock::now();
            auto diff = 1. * (end - begin).count() * nanoseconds::period::num / nanoseconds::period::den;
            times.emplace_back(diff);
            cout << i << "th trail, training time (/s): " << diff << endl;
            errors = 0;
            for (int j = 0; j < nfeature; ++j)
                errors+=solver_vec[j]->eval();
        }
        int num_zeros = 0;
        for (int j = 0; j < nfeature; ++j)
        {
            num_zeros += ((*(solver_vec[j]->theta)).array().abs()<=0.00001).count();
        }
        cout << "Sparsity: " << 1-1.*num_zeros/nfeature/(nfeature-1) << endl;
        cout << "Errors: " << errors << endl;

    }

    double sumT = 0;
    for(double t : times)
        sumT += t;

    cout<<"mean training time (/s): "<<1.0*sumT/param.getIntArg("nexp")<<endl;

}

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

    if(argc < 2)
        runCLITask("./Tasks/Arabidopsis");
    else
        runCLITask(argv[1]);

    return 0;
}


#include "mex.h"
#include "drake/drakeUtil.h"
#include <Eigen/Dense>

using namespace Eigen;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    if (nrhs != 4 || nlhs != 2) {
        mexErrMsgIdAndTxt("Drake:lqrmex:InvalidUsage","Usage: [K, S] = lqrmex(A, B, Q, R)");
    }

    const size_t A_rows = mxGetM(prhs[0]);
    const size_t A_cols = mxGetN(prhs[0]);

    const size_t B_rows = mxGetM(prhs[1]);
    const size_t B_cols = mxGetN(prhs[1]);

    const size_t Q_rows = mxGetM(prhs[2]);
    const size_t Q_cols = mxGetN(prhs[2]);

    const size_t R_rows = mxGetM(prhs[3]);
    const size_t R_cols = mxGetN(prhs[3]);

    assert(A_rows == A_cols);
    assert(Q_rows == Q_cols);
    assert(Q_rows == A_rows);
    assert(R_rows == R_cols);
    assert(R_rows == B_cols);

    Map<MatrixXd> A(mxGetPr(prhs[0]), A_rows, A_cols);
    Map<MatrixXd> B(mxGetPr(prhs[1]), B_rows, B_cols);
    Map<MatrixXd> Q(mxGetPr(prhs[2]), Q_rows, Q_cols);
    Map<MatrixXd> R(mxGetPr(prhs[3]), R_rows, R_cols);

    plhs[0] = mxCreateDoubleMatrix(R_rows, A_cols, mxREAL);
    plhs[1] = mxCreateDoubleMatrix(A_rows, A_cols, mxREAL);

    Map<MatrixXd> K(mxGetPr(plhs[0]), R_rows, A_cols);
    Map<MatrixXd> S(mxGetPr(plhs[1]), A_rows, A_cols);

    lqr(A, B, Q, R, K, S);
}