/*
 * self_similarity.cpp
 *
 *  Created on: Jan 25, 2012
 *      Author: lbossard
 */

#include "self_similarity.hpp"

#include <boost/math/constants/constants.hpp>
#include <boost/foreach.hpp>

namespace vision
{
namespace features
{

SelfSimilarityExtractor::SelfSimilarityExtractor(
        bool color_ssd,
        unsigned int patch_size,
        unsigned int window_radius,
        unsigned int radius_bin_count,
        unsigned int angle_bin_count,
        float var_noise,
        unsigned int auto_var_radius)
{
    color_ssd_ = color_ssd;
    this->setSelfSimilarityParameters(
            patch_size,
            window_radius,
            radius_bin_count,
            angle_bin_count,
            var_noise,
            auto_var_radius);
}

void SelfSimilarityExtractor::setSelfSimilarityParameters(
        unsigned int patch_size,
        unsigned int window_radius,
        unsigned int radius_bin_count,
        unsigned int angle_bin_count,
        float var_noise,
        unsigned int auto_var_radius)
{

    self_similarity_.setParameters(patch_size,
            window_radius,
            radius_bin_count,
            angle_bin_count,
            var_noise,
            auto_var_radius);

}

/*virtual*/ SelfSimilarityExtractor::~SelfSimilarityExtractor()
{

}

/*virtual*/ cv::Mat_<float> SelfSimilarityExtractor::denseExtract(
        const cv::Mat& input_image,
        std::vector<cv::Point>& locations,
        const cv::Mat_<uchar>& mask
) const
{
    const unsigned int radius = 5;

    // generate locations
    locations.clear();
    LowLevelFeatureExtractor::generateGridLocations(
            input_image.size(),
            cv::Size(radius, radius),
            radius,
            radius,
            locations,
            mask);
    if (locations.size() == 0)
    {
        return cv::Mat_<float>();
    }

    // convert to gray if necessary
    cv::Mat image = input_image;
    if (!color_ssd_ && image.type() != CV_8U)
    {
        cv::cvtColor(image, image, CV_BGR2GRAY);
    }

    // allocate memroy
    cv::Mat_<float> features(locations.size(), descriptorLength());

    // loop over locations and compute descriptors
    std::size_t point_idx = 0;
    BOOST_FOREACH(const cv::Point& p, locations)
    {
        cv::Mat_<float> descriptor = features.row(point_idx);
        if (self_similarity_.extract(image, p, descriptor))
        {
            ++point_idx;
        }
    }
    if (point_idx == 0)
    {
        return cv::Mat_<float>();
    }
    features.resize(point_idx);
    return features;
}

/*virtual*/ unsigned int SelfSimilarityExtractor::descriptorLength() const
{
    return self_similarity_.descriptorLength();
}
////////////////////////////////////////////////////////////////////////////////
/**
 *
 * @param patch_size       odd
 * @param window_radius    even
 * @param radius_bin_count
 * @param angle_bin_count
 * @param var_noise
 * @param auto_var_radius
 */
SelfSimilarity::SelfSimilarity(
        unsigned int patch_size,
        unsigned int window_radius,
        unsigned int radius_bin_count,
        unsigned int angle_bin_count,
        float var_noise,
        unsigned int auto_var_radius)
{
    setParameters(patch_size,
            window_radius,
            radius_bin_count,
            angle_bin_count,
            var_noise,
            auto_var_radius);
}
/*virtual*/ SelfSimilarity::~SelfSimilarity()
{

}

void SelfSimilarity::setParameters(unsigned int patch_size,
        unsigned int window_radius,
        unsigned int radius_bin_count,
        unsigned int angle_bin_count,
        float var_noise,
        unsigned int auto_var_radius)
{
    // set parameters
    patch_size_ = patch_size;
    window_radius_ = window_radius;
    radius_bin_count_ = radius_bin_count;
    angle_bin_count_ = angle_bin_count;
    var_noise_ = var_noise;

    // precompute binning and autovar indices
    const int window_size = 2 * window_radius_ + 1;
    bin_map_= -cv::Mat_<int>::ones(window_size, window_size);  // init to -1
    autovar_indices_.clear();
    autovar_indices_.reserve((2*auto_var_radius + 1)*(2*auto_var_radius + 1)); // upper bound
    double angle = 0.f;
    double radius = 0.f;
    int angle_bin = 0;
    int radius_bin = 0;
    int bin_id = 0;
    for (int y = -window_radius_; y < window_size; ++y)
    {
        for (int x = -window_radius_; x < window_size; ++x)
        {
            /*
             * scale log(y) to (bin_count - 1)...0
             */
            radius = std::sqrt(static_cast<float>(x * x + y * y));
            if (radius > window_radius_ || radius == 0.)
            {
                continue;
            }
            radius_bin = static_cast<int>(
                    ( std::log(radius)) / std::log(window_radius_+.5)
                    * radius_bin_count_);

            /*
             * \alpha \in [ -\pi ... +\pi ]
             * bin_id \in [0 ... (bin_count - 1) ]
             * bin_id = floor( (\alpha + \pi) / ( 2 \pi) * bin_count )
             *        = floor( \alpha bin_count / ( 2 \pi) + bin_count/2 )
             */
            angle = std::atan2(y, x); // [-pi ... pi]
            angle_bin = static_cast<int>(
                   ((angle * angle_bin_count_)
                    / ( boost::math::constants::two_pi<double>()))
                    + angle_bin_count_/2.) % angle_bin_count; // atan2 gives [-pi ... pi] and not (-pi ... pi] that's why we mod

            bin_id = angle_bin * radius_bin_count + radius_bin;
            bin_map_(y + window_radius_, x + window_radius_) = bin_id;

            if (radius <= auto_var_radius && radius > 0)
            {
                autovar_indices_.push_back(cv::Point(y + window_radius_, x + window_radius_));
            }

        }
    }
}

bool SelfSimilarity::extract(const cv::Mat& image, const cv::Point& location, cv::Mat_<float>& descriptor) const
{
    // compute the distance surface
    cv::Mat_<float> distance_surface;
    compute_distance_surface(image, location, distance_surface);

    if (distance_surface.rows == 0 || distance_surface.cols == 0)
    {
        return false;
    }

    // put it into the logpolar descriptor
    compute_descriptor(distance_surface, descriptor);
    return true;
}

//void showmat(const std::string titile, const cv::Mat& mat, int size)
//{
//    cv::Mat scaled;
//    cv::resize(mat, scaled, cv::Size(mat.rows * size, mat.cols * size), 0, 0, cv::INTER_AREA);
//    cv::imshow(titile, scaled);
//}

void SelfSimilarity::compute_distance_surface(const cv::Mat& image, const cv::Point& loaction, cv::Mat_<float>& distance_surface) const
{
    const int patch_radius = patch_size_ / 2;
    const int window_size  = window_radius_ * 2 + 1;

    // check boundaries
    if (	   (loaction.x - (int)window_radius_ - patch_radius) < 0
            || (loaction.y - (int)window_radius_ - patch_radius) < 0
            || (loaction.x + (int)window_radius_ + patch_size_) >= image.cols
            || (loaction.y + (int)window_radius_ + patch_size_) >= image.rows)
    {
        distance_surface = cv::Mat_<float>();
        return;
    }

    // allocate memory and precompute some structures
    distance_surface.create(window_size, window_size);

    const cv::Mat inner_patch = image(
            cv::Range(loaction.y - patch_radius, loaction.y + patch_radius + 1),
            cv::Range(loaction.x - patch_radius, loaction.x + patch_radius + 1));

//    assert(distance_surface.rows == window_size);
//    assert(distance_surface.cols == window_size);
    for (unsigned int r = 0; r < window_size; ++r)
    {
        const int y_window_patch_start = loaction.y - window_radius_ + r - patch_radius ;
        const cv::Range y_window_range(
                y_window_patch_start,
                y_window_patch_start + patch_size_);
        for (unsigned int c = 0; c < window_size; ++c)
        {
//            const int bin_idx = bin_map_(r,c);
//            if (bin_idx < 0)
//            {
//                continue;
//            }
            const int x_window_patch_start = loaction.x - window_radius_  + c - patch_radius;
            const cv::Range x_window_range(
                    x_window_patch_start,
                    x_window_patch_start + patch_size_);

            const cv::Mat window_patch = image(y_window_range, x_window_range);
            if (image.channels() == 3)
            {
                distance_surface(r, c) = ssd3(inner_patch, window_patch);
            }
            else
            {
                distance_surface(r, c) = ssd1(inner_patch, window_patch);
            }

//            showmat("inner", inner_patch, 100);
//            showmat("window_patch", window_patch, 100);
//            cv::Mat d = image.clone();
//            // window_patch
//            cv::rectangle(d,
//                    cv::Point(x_window_range.start, y_window_range.start),
//                    cv::Point(x_window_range.end-1,   y_window_range.end-1),
//                    CV_RGB(0,255,255)
//                    );
//            // center_patch
//            cv::rectangle(d,
//                    cv::Point(loaction.x - patch_radius, loaction.y - patch_radius),
//                    cv::Point(loaction.x + patch_radius, loaction.y + patch_radius),
//                    CV_RGB(255,0,255)
//                    );
//            // whole window
//            cv::rectangle(d,
//                    cv::Point(
//                            loaction.x - window_radius_,
//                            loaction.y - window_radius_),
//                    cv::Point(
//                            loaction.x + window_radius_,
//                            loaction.y + window_radius_),
//                    CV_RGB(255,0,255)
//                    );
//            showmat("all", d, 5);
//            cv::waitKey();
        }
    }
}

void SelfSimilarity::compute_descriptor(const cv::Mat_<float>& distance_surface, cv::Mat_<float>& descriptor) const
{
    // allocate log-polar descriptor. we init it with -1 as the e^-x is
    // always > 0 for x < \inf
    const int descriptor_length = radius_bin_count_*angle_bin_count_;
    descriptor.create(1, descriptor_length);
    descriptor = -1;

    // compute auto noise (like original paper matlab implementation):
    // find max ssd at the autovar_indices (<- places with within radius 1)
    float variance = var_noise_;
    for (unsigned int i = 0; i < autovar_indices_.size(); ++i)
    {
        const cv::Point& p = autovar_indices_[i];
        variance = std::max(variance, distance_surface(p.y, p.x));
    }

    // compute finally the descriptor
    float min_correlation = std::numeric_limits<float>::max();
    float max_correlation = std::numeric_limits<float>::min();
    for (int r = 0; r < distance_surface.rows; ++r)
    {
        for (int c = 0; c < distance_surface.cols; ++c)
        {
            const int bin_idx = bin_map_(r,c);
            if (bin_idx < 0)
            {
                continue;
            }
            // compute S_q(x,y)
            float correlation = std::exp(-distance_surface(r, c) / variance);
            // we store only the max ssd
            const float current_value = descriptor(bin_idx);
            if (current_value < correlation)
            {
                descriptor(bin_idx) = correlation;
                min_correlation = std::min(correlation, min_correlation);
                max_correlation = std::max(correlation, max_correlation);
            }

        }
    }
    // finally: normalize to [0 ... 1]
    //XXX: Note: in the original implementation, there would be only stretching to max without substracting min
    // however, the paper states "normalized by linearly streticht its values to the range [0...1]"
//    descriptor = (descriptor - min_correlation) / (max_correlation - min_correlation);
    //XXX: strange enough: "correct" normalization gives a segfault while clustering
    descriptor = descriptor / max_correlation;  // normalisation from the paper implementation

}

} /* namespace features */
} /* namespace vision */


#include "kernel_divergent_viscosity.hpp"

#include "kernel_test.hpp"

#include <boost/test/unit_test.hpp>

using namespace GooBalls;
using namespace d2;
using namespace Physics;

namespace utf = boost::unit_test;

using KKernel = DivergentViscosity;

/*
/// Don't try this, the kernel diverges by definition
BOOST_AUTO_TEST_CASE(divergent_viscosity_normalization_1d, *utf::tolerance(0.0001)){
    KKernel k;
    testNormalization1d(k, 100000);
}
BOOST_AUTO_TEST_CASE(divergent_viscosity_normalization, *utf::tolerance(0.00001)){
    KKernel k;
    testNormalization(k, 100000);
}

*/

BOOST_AUTO_TEST_CASE(divergent_viscosity_zero_border_1d, *utf::tolerance(0.0001)){
    KKernel k;
    testZeroBorder1d(k);
}

BOOST_AUTO_TEST_CASE(divergent_viscosity_zero_border_gradient_1d, *utf::tolerance(0.001)){
    KKernel k;
    testZeroBorderGradient1d(k);
}

BOOST_AUTO_TEST_CASE(divergent_viscosity_grad_finite_diff_1d, *utf::tolerance(0.02)){
    KKernel k;
    testGradientFiniteDifference1d(k, 100);
}
/*
BOOST_AUTO_TEST_CASE(divergent_viscosity_lap_grad_finite_diff_1d, *utf::tolerance(0.0001)){
    KKernel k;
    testLaplacianFromGradientFiniteDifferences1d(k, 100);
}

BOOST_AUTO_TEST_CASE(divergent_viscosity_lap_finite_diff_1d, *utf::tolerance(0.00001)){
    KKernel k;
    testLaplacianFiniteDifference1d(k, 100);
}
*/

BOOST_AUTO_TEST_CASE(divergent_viscosity_zero_border, *utf::tolerance(0.0001)){
    KKernel k;
    testZeroBorder(k, 100);
}
/*
BOOST_AUTO_TEST_CASE(divergent_viscosity_zero_border_gradient, *utf::tolerance(0.001)){
    KKernel k;
    testZeroBorderGradient(k, 100);
}
*/

BOOST_AUTO_TEST_CASE(divergent_viscosity_monotonicity, *utf::tolerance(0.0001)){
    KKernel k;
    testMonotonicity(k, 10);
}

BOOST_AUTO_TEST_CASE(divergent_viscosity_radial_symmetric){
    KKernel k;
    testRadialSymmetry(k, 100);
}

/*
BOOST_AUTO_TEST_CASE(divergent_viscosity_grad_finite_diff, *utf::tolerance(0.0005)){
    KKernel k;
    testGradientFiniteDifference(k, 100);
}

BOOST_AUTO_TEST_CASE(divergent_viscosity_lap_grad_finite_diff, *utf::tolerance(0.0001)){
    KKernel k;
    testLaplacianFromGradientFiniteDifferences(k, 100);
}

BOOST_AUTO_TEST_CASE(divergent_viscosity_lap_finite_diff, *utf::tolerance(0.00001)){
    KKernel k;
    testLaplacianFiniteDifference(k, 100);
}
*/


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

#include <costmap_converter/costmap_to_polygons.h>
#include <costmap_converter/misc.h>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <pluginlib/class_list_macros.h>

PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::BaseCostmapToPolygons)

namespace
{

/**
 * @brief Douglas-Peucker Algorithm for fitting lines into ordered set of points
 *
 * Douglas-Peucker Algorithm, see https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm
 *
 * @param begin iterator pointing to the begin of the range of points
 * @param end interator pointing to the end of the range of points
 * @param epsilon distance criteria for removing points if it is closer to the line segment than this
 * @param result the simplified polygon
 */
std::vector<geometry_msgs::Point32> douglasPeucker(std::vector<geometry_msgs::Point32>::iterator begin,
  std::vector<geometry_msgs::Point32>::iterator end, double epsilon)
{
  if (std::distance(begin, end) <= 2)
  {
    return std::vector<geometry_msgs::Point32>(begin, end);
  }

  // Find the point with the maximum distance from the line [begin, end)
  double dmax = std::numeric_limits<double>::lowest();
  std::vector<geometry_msgs::Point32>::iterator max_dist_it;
  std::vector<geometry_msgs::Point32>::iterator last = std::prev(end);
  for (auto it = std::next(begin); it != last; ++it)
  {
    double d = costmap_converter::computeSquaredDistanceToLineSegment(*it, *begin, *last);
    if (d > dmax)
    {
      max_dist_it = it;
      dmax = d;
    }
  }

  if (dmax < epsilon * epsilon)
  { // termination criterion reached, line is good enough
    std::vector<geometry_msgs::Point32> result;
    result.push_back(*begin);
    result.push_back(*last);
    return result;
  }

  // Recursive calls for the two splitted parts
  auto firstLineSimplified = douglasPeucker(begin, std::next(max_dist_it), epsilon);
  auto secondLineSimplified = douglasPeucker(max_dist_it, end, epsilon);

  // Combine the two lines into one line and return the merged line.
  // Note that we have to skip the first point of the second line, as it is duplicated above.
  firstLineSimplified.insert(firstLineSimplified.end(),
    std::make_move_iterator(std::next(secondLineSimplified.begin())),
    std::make_move_iterator(secondLineSimplified.end()));
  return firstLineSimplified;
}

} // end namespace

namespace costmap_converter
{

CostmapToPolygonsDBSMCCH::CostmapToPolygonsDBSMCCH() : BaseCostmapToPolygons()
{
  costmap_ = NULL;
  dynamic_recfg_ = NULL;
  neighbor_size_x_ = neighbor_size_y_ = -1;
  offset_x_ = offset_y_ = 0.;
}

CostmapToPolygonsDBSMCCH::~CostmapToPolygonsDBSMCCH()
{
  if (dynamic_recfg_ != NULL)
    delete dynamic_recfg_;
}

void CostmapToPolygonsDBSMCCH::initialize(ros::NodeHandle nh)
{
    costmap_ = NULL;

    nh.param("cluster_max_distance", parameter_.max_distance_, 0.4);
    nh.param("cluster_min_pts", parameter_.min_pts_, 2);
    nh.param("cluster_max_pts", parameter_.max_pts_, 30);
    nh.param("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, 0.1);

    parameter_buffered_ = parameter_;

    // setup dynamic reconfigure
    dynamic_recfg_ = new dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>(nh);
    dynamic_reconfigure::Server<CostmapToPolygonsDBSMCCHConfig>::CallbackType cb = boost::bind(&CostmapToPolygonsDBSMCCH::reconfigureCB, this, _1, _2);
    dynamic_recfg_->setCallback(cb);
}

void CostmapToPolygonsDBSMCCH::compute()
{
    std::vector< std::vector<KeyPoint> > clusters;
    dbScan(clusters);

    // Create new polygon container
    PolygonContainerPtr polygons(new std::vector<geometry_msgs::Polygon>());

    // add convex hulls to polygon container
    for (std::size_t i = 1; i <clusters.size(); ++i) // skip first cluster, since it is just noise
    {
      polygons->push_back( geometry_msgs::Polygon() );
      convexHull2(clusters[i], polygons->back() );
    }

    // add our non-cluster points to the polygon container (as single points)
    if (!clusters.empty())
    {
      for (std::size_t i=0; i < clusters.front().size(); ++i)
      {
        polygons->push_back( geometry_msgs::Polygon() );
        convertPointToPolygon(clusters.front()[i], polygons->back());
      }
    }

    // replace shared polygon container
    updatePolygonContainer(polygons);
}

void CostmapToPolygonsDBSMCCH::setCostmap2D(costmap_2d::Costmap2D *costmap)
{
    if (!costmap)
      return;

    costmap_ = costmap;

    updateCostmap2D();
}

void CostmapToPolygonsDBSMCCH::updateCostmap2D()
{
      occupied_cells_.clear();

      if (!costmap_->getMutex())
      {
        ROS_ERROR("Cannot update costmap since the mutex pointer is null");
        return;
      }

      { // get a copy of our parameters from dynamic reconfigure
        boost::mutex::scoped_lock lock(parameter_mutex_);
        parameter_ = parameter_buffered_;
      }

      costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());

      // allocate neighbor lookup
      int cells_x = int(costmap_->getSizeInMetersX() / parameter_.max_distance_) + 1;
      int cells_y = int(costmap_->getSizeInMetersY() / parameter_.max_distance_) + 1;

      if (cells_x != neighbor_size_x_ || cells_y != neighbor_size_y_) {
        neighbor_size_x_ = cells_x;
        neighbor_size_y_ = cells_y;
        neighbor_lookup_.resize(neighbor_size_x_ * neighbor_size_y_);
      }
      offset_x_ = costmap_->getOriginX();
      offset_y_ = costmap_->getOriginY();
      for (auto& n : neighbor_lookup_)
        n.clear();

      // get indices of obstacle cells
      for(std::size_t i = 0; i < costmap_->getSizeInCellsX(); i++)
      {
        for(std::size_t j = 0; j < costmap_->getSizeInCellsY(); j++)
        {
          int value = costmap_->getCost(i,j);
          if(value >= costmap_2d::LETHAL_OBSTACLE)
          {
            double x, y;
            costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y);
            addPoint(x, y);
          }
        }
      }
}

void CostmapToPolygonsDBSMCCH::dbScan(std::vector< std::vector<KeyPoint> >& clusters)
{
  std::vector<bool> visited(occupied_cells_.size(), false);

  clusters.clear();

  //DB Scan Algorithm
  int cluster_id = 0; // current cluster_id
  clusters.push_back(std::vector<KeyPoint>());
  for(int i = 0; i< (int)occupied_cells_.size(); i++)
  {
    if(!visited[i]) //keypoint has not been visited before
    {
      visited[i] = true; // mark as visited
      std::vector<int> neighbors;
      regionQuery(i, neighbors); //Find neighbors around the keypoint
      if((int)neighbors.size() < parameter_.min_pts_) //If not enough neighbors are found, mark as noise
      {
        clusters[0].push_back(occupied_cells_[i]);
      }
      else
      {
        ++cluster_id; // increment current cluster_id
        clusters.push_back(std::vector<KeyPoint>());

        // Expand the cluster
        clusters[cluster_id].push_back(occupied_cells_[i]);
        for(int j = 0; j<(int)neighbors.size(); j++)
        {
          if ((int)clusters[cluster_id].size() == parameter_.max_pts_)
            break;

          if(!visited[neighbors[j]]) //keypoint has not been visited before
          {
            visited[neighbors[j]] = true;  // mark as visited
            std::vector<int> further_neighbors;
            regionQuery(neighbors[j], further_neighbors); //Find more neighbors around the new keypoint
//             if(further_neighbors.size() < min_pts_)
//             {
//               clusters[0].push_back(occupied_cells[neighbors[j]]);
//             }
//             else
            if ((int)further_neighbors.size() >= parameter_.min_pts_)
            {
              // neighbors found
              neighbors.insert(neighbors.end(), further_neighbors.begin(), further_neighbors.end());  //Add these newfound P' neighbour to P neighbour vector "nb_indeces"
              clusters[cluster_id].push_back(occupied_cells_[neighbors[j]]);
            }
          }
        }
      }
    }
  }
}

void CostmapToPolygonsDBSMCCH::regionQuery(int curr_index, std::vector<int>& neighbors)
{
    neighbors.clear();

    double dist_sqr_threshold = parameter_.max_distance_ * parameter_.max_distance_;
    const KeyPoint& kp = occupied_cells_[curr_index];
    int cx, cy;
    pointToNeighborCells(kp, cx,cy);

    // loop over the neighboring cells for looking up the points
    const int offsets[9][2] = {{-1, -1}, {0, -1}, {1, -1},
                               {-1,  0}, {0,  0}, {1,  0},
                               {-1,  1}, {0,  1}, {1,  1}};
    for (int i = 0; i < 9; ++i)
    {
      int idx = neighborCellsToIndex(cx + offsets[i][0], cy + offsets[i][1]);
      if (idx < 0 || idx >= int(neighbor_lookup_.size()))
        continue;
      const std::vector<int>& pointIndicesToCheck = neighbor_lookup_[idx];
      for (int point_idx : pointIndicesToCheck) {
        if (point_idx == curr_index) // point is not a neighbor to itself
          continue;
        const KeyPoint& other = occupied_cells_[point_idx];
        double dx = other.x - kp.x;
        double dy = other.y - kp.y;
        double dist_sqr = dx*dx + dy*dy;
        if (dist_sqr <= dist_sqr_threshold)
          neighbors.push_back(point_idx);
      }
    }
}

bool isXCoordinateSmaller(const CostmapToPolygonsDBSMCCH::KeyPoint& p1, const CostmapToPolygonsDBSMCCH::KeyPoint& p2)
{
  return p1.x < p2.x || (p1.x == p2.x && p1.y < p2.y);
}

void CostmapToPolygonsDBSMCCH::convexHull(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon)
{
    //Monotone Chain ConvexHull Algorithm source from http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull

    int k = 0;
    int n = cluster.size();

    // sort points according to x coordinate (TODO. is it already sorted due to the map representation?)
    std::sort(cluster.begin(), cluster.end(), isXCoordinateSmaller);

    polygon.points.resize(2*n);

    // lower hull
    for (int i = 0; i < n; ++i)
    {
      while (k >= 2 && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0)
      {
        --k;
      }
      cluster[i].toPointMsg(polygon.points[k]);
      ++k;
    }

    // upper hull
    for (int i = n-2, t = k+1; i >= 0; --i)
    {
      while (k >= t && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0)
      {
        --k;
      }
      cluster[i].toPointMsg(polygon.points[k]);
      ++k;
    }

    polygon.points.resize(k); // original
    // TEST we skip the last point, since in our definition the polygon vertices do not contain the start/end vertex twice.
//     polygon.points.resize(k-1); // TODO remove last point from the algorithm above to reduce computational cost

    simplifyPolygon(polygon);
}

void CostmapToPolygonsDBSMCCH::convexHull2(std::vector<KeyPoint>& cluster, geometry_msgs::Polygon& polygon)
{
    std::vector<KeyPoint>& P = cluster;
    std::vector<geometry_msgs::Point32>& points = polygon.points;

    // Sort P by x and y
    std::sort(P.begin(), P.end(), isXCoordinateSmaller);

    // the output array H[] will be used as the stack
    int i;                 // array scan index

    // Get the indices of points with min x-coord and min|max y-coord
    int minmin = 0, minmax;
    double xmin = P[0].x;
    for (i = 1; i < (int)P.size(); i++)
        if (P[i].x != xmin) break;
    minmax = i - 1;
    if (minmax == (int)P.size() - 1)
    {   // degenerate case: all x-coords == xmin
        points.push_back(geometry_msgs::Point32());
        P[minmin].toPointMsg(points.back());
        if (P[minmax].y != P[minmin].y) // a  nontrivial segment
        {
            points.push_back(geometry_msgs::Point32());
            P[minmax].toPointMsg(points.back());
        }
        // add polygon endpoint
        points.push_back(geometry_msgs::Point32());
        P[minmin].toPointMsg(points.back());
        return;
    }

    // Get the indices of points with max x-coord and min|max y-coord
    int maxmin, maxmax = (int)P.size() - 1;
    double xmax = P.back().x;
    for (i = P.size() - 2; i >= 0; i--)
        if (P[i].x != xmax) break;
    maxmin = i+1;

    // Compute the lower hull on the stack H
    // push  minmin point onto stack
    points.push_back(geometry_msgs::Point32());
    P[minmin].toPointMsg(points.back());
    i = minmax;
    while (++i <= maxmin)
    {
        // the lower line joins P[minmin]  with P[maxmin]
        if (cross(P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin)
            continue;           // ignore P[i] above or on the lower line

        while (points.size() > 1)         // there are at least 2 points on the stack
        {
            // test if  P[i] is left of the line at the stack top
            if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
                break;         // P[i] is a new hull  vertex
            points.pop_back();         // pop top point off  stack
        }
        // push P[i] onto stack
        points.push_back(geometry_msgs::Point32());
        P[i].toPointMsg(points.back());
    }

    // Next, compute the upper hull on the stack H above  the bottom hull
    if (maxmax != maxmin)      // if  distinct xmax points
    {
         // push maxmax point onto stack
         points.push_back(geometry_msgs::Point32());
         P[maxmax].toPointMsg(points.back());
    }
    int bot = (int)points.size();                  // the bottom point of the upper hull stack
    i = maxmin;
    while (--i >= minmax)
    {
        // the upper line joins P[maxmax]  with P[minmax]
        if (cross( P[maxmax], P[minmax], P[i])  >= 0 && i > minmax)
            continue;           // ignore P[i] below or on the upper line

        while ((int)points.size() > bot)     // at least 2 points on the upper stack
        {
            // test if  P[i] is left of the line at the stack top
            if (cross(points[points.size() - 2], points.back(), P[i]) > 0)
                break;         // P[i] is a new hull  vertex
            points.pop_back();         // pop top point off stack
        }
        // push P[i] onto stack
        points.push_back(geometry_msgs::Point32());
        P[i].toPointMsg(points.back());
    }
    if (minmax != minmin)
    {
        // push  joining endpoint onto stack
        points.push_back(geometry_msgs::Point32());
        P[minmin].toPointMsg(points.back());
    }

    simplifyPolygon(polygon);
}

void CostmapToPolygonsDBSMCCH::simplifyPolygon(geometry_msgs::Polygon& polygon)
{
  size_t triangleThreshold = 3;
  // check if first and last point are the same. If yes, a triangle has 4 points
  if (polygon.points.size() > 1
      && std::abs(polygon.points.front().x - polygon.points.back().x) < 1e-5
      && std::abs(polygon.points.front().y - polygon.points.back().y) < 1e-5)
  {
    triangleThreshold = 4;
  }
  if (polygon.points.size() <= triangleThreshold) // nothing to do for triangles or lines
    return;
  // TODO Reason about better start conditions for splitting lines, e.g., by
  // https://en.wikipedia.org/wiki/Rotating_calipers
  polygon.points = douglasPeucker(polygon.points.begin(), polygon.points.end(), parameter_.min_keypoint_separation_);;
}

void CostmapToPolygonsDBSMCCH::updatePolygonContainer(PolygonContainerPtr polygons)
{
  boost::mutex::scoped_lock lock(mutex_);
  polygons_ = polygons;
}

PolygonContainerConstPtr CostmapToPolygonsDBSMCCH::getPolygons()
{
  boost::mutex::scoped_lock lock(mutex_);
  PolygonContainerConstPtr polygons = polygons_;
  return polygons;
}

void CostmapToPolygonsDBSMCCH::reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level)
{
  boost::mutex::scoped_lock lock(parameter_mutex_);
  parameter_buffered_.max_distance_ = config.cluster_max_distance;
  parameter_buffered_.min_pts_ = config.cluster_min_pts;
  parameter_buffered_.max_pts_ = config.cluster_max_pts;
  parameter_buffered_.min_keypoint_separation_ = config.convex_hull_min_pt_separation;
}

}//end namespace costmap_converter


// Copyright (c) 2021 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#include "pyinterp/geohash/string.hpp"

#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include <Eigen/Core>

#include "pyinterp/geohash/int64.hpp"

namespace py = pybind11;
namespace geohash = pyinterp::geohash;
namespace geodetic = pyinterp::geodetic;

// Parsing of the string defining a GeoHash.
static inline const auto parse_str = [](const py::str& hash) -> auto {
  auto result = std::string(hash);
  if (result.length() < 1 || result.length() > 12) {
    throw std::invalid_argument("Geohash length must be within [1, 12]");
  }
  return result;
};

// Checking the value defining the precision of a geohash.
static inline auto check_range(uint32_t precision) -> void {
  if (precision < 1 || precision > 12) {
    throw std::invalid_argument("precision must be within [1, 12]");
  }
}

void init_geohash_string(py::module& m) {
  m.def(
       "error",
       [](const uint32_t& precision) -> py::tuple {
         check_range(precision);
         auto lon_lat_err = geohash::int64::error_with_precision(precision * 5);
         return py::make_tuple(std::get<1>(lon_lat_err),
                               std::get<0>(lon_lat_err));
       },
       py::arg("precision"),
       R"__doc__(
Returns the accuracy in longitude/latitude and degrees for the given precision.

Args:
    precision (int): Number of bits used to encode the geohash code.
Returns:
    tuple: Accuracy in longitude/latitude and degrees.
Raises:
    ValueError: If the given precision is not within [1, 12].
)__doc__")
      .def(
          "encode",
          [](const geodetic::Point& point,
             const uint32_t precision) -> py::handle {
            auto result = std::array<char, 12>();
            check_range(precision);
            geohash::string::encode(point, result.data(), precision);
            return PyBytes_FromStringAndSize(result.data(), precision);
          },
          py::arg("point"), py::arg("precision") = 12,
          R"__doc__(
Encode a point into geohash code with the given precision.

Args:
  point (pyinterp.geodetic.Point) Point to encode.
  precision (int, optional) Number of bits used to encode the geohash code.
Returns:
  int: geohash code.
Raises:
  ValueError: If the given precision is not within [1, 12].
)__doc__")
      .def(
          "encode",
          [](const Eigen::Ref<const Eigen::VectorXd>& lon,
             const Eigen::Ref<const Eigen::VectorXd>& lat,
             const uint32_t precision) -> pybind11::array {
            check_range(precision);
            return geohash::string::encode(lon, lat, precision);
          },
          py::arg("lon"), py::arg("lat"), py::arg("precision") = 12,
          R"__doc__(
Encode coordinates into geohash with the given precision.

Args:
  lon (numpy.ndarray) Longitudes in degrees.
  lat (numpy.ndarray) Latitudes in degrees.
  precision (int, optional) Number of bits used to encode the geohash code.
Returns:
  numpy.ndarray: geohash codes.
Raises:
  ValueError: If the given precision is not within [1, 12].
  ValueError: If the lon and lat vectors have different sizes.
)__doc__")
      .def(
          "decode",
          [](const py::str& hash, const bool round) -> geodetic::Point {
            auto buffer = parse_str(hash);
            return geohash::string::decode(buffer.data(), buffer.length(),
                                           round);
          },
          py::arg("hash"), py::arg("round") = false,
          R"__doc__(
Decode a hash into a geographic point.

Args:
  hash (str): Geohash.
  round (optional, bool): If true, the coordinates of the point will be
    rounded to the accuracy defined by the GeoHash.
Returns:
  pyinterp.geodetic.Point: decoded geographic point.
)__doc__")
      .def(
          "decode",
          [](const pybind11::array& hash,
             const bool round) -> std::tuple<Eigen::VectorXd, Eigen::VectorXd> {
            return geohash::string::decode(hash, round);
          },
          py::arg("hash"), py::arg("round") = false,
          R"__doc__(
Decode hashes into a geographic points.

Args:
  hash (numpy.ndarray): Geohash codes.
  round (optional, bool): If true, the coordinates of the point will be
    rounded to the accuracy defined by the GeoHash."
Returns:
  tuple: longitudes/latitudes of the decoded points.
)__doc__")
      .def(
          "area",
          [](const py::str& hash,
             const std::optional<geodetic::System>& wgs) -> double {
            auto buffer = parse_str(hash);
            return geohash::string::area(buffer.data(), buffer.length(), wgs);
          },
          py::arg("hash"), py::arg("wgs") = py::none(),
          R"__doc__(
Calculate the area covered by the GeoHash.

Args:
  hash (str): Geohash.
  wgs (optional, pyinterp.geodetic.System): WGS used to calculate the area.
    Default to WGS84.
Returns:
  double: calculated area.
)__doc__")
      .def(
          "area",
          [](const pybind11::array& hash,
             const std::optional<geodetic::System>& wgs) -> Eigen::VectorXd {
            return geohash::string::area(hash, wgs);
          },
          py::arg("hash"), py::arg("wgs") = py::none(),
          R"__doc__(
Calculated the area caovered by the GeoHash codes.

Args:
  hash (numpy.ndarray): Geohash codes.
  wgs (optional, pyinterp.geodetic.System): WGS used to calculate the area.
    Default to WGS84.

Returns:
  double: calculated areas.
)__doc__")
      .def(
          "bounding_box",
          [](const py::str& hash) -> geodetic::Box {
            auto buffer = parse_str(hash);
            return geohash::string::bounding_box(buffer.data(),
                                                 buffer.length());
          },
          py::arg("hash"),
          R"__doc__(
Returns the bounding box encoded by the geohash.

Args:
  hash (str): Geohash.
Returns:
  pyinterp.geodetic.Box: Bounding box.
)__doc__")
      .def(
          "bounding_boxes",
          [](const std::optional<geodetic::Box>& box,
             const uint32_t precision) -> py::array {
            check_range(precision);
            return geohash::string::bounding_boxes(box, precision);
          },
          py::arg("box") = py::none(), py::arg("precision") = 1,
          R"__doc__(
Returns all geohash codes contained in the defined bounding box.

Args:
  box (pyinterp.geohash.Box, optional): Bounding box.
  precision (int, optional): Required accuracy.
Returns:
  numpy.ndarray: Geohash codes.
Raises:
    ValueError: If the given precision is not within [1, 12].
)__doc__")
      .def(
          "neighbors",
          [](const py::str& hash) {
            auto buffer = parse_str(hash);
            return geohash::string::neighbors(buffer.data(), buffer.length());
          },
          py::arg("hash"),
          R"__doc__(
Returns all neighbors hash clockwise from north around northwest.

Args:
  hash (str): Geohash code.
Returns:
  numpy.ndarray: Geohash codes.
)__doc__")
      .def(
          "grid_properties",
          [](const geodetic::Box& box,
             const uint32_t precision) -> std::tuple<uint64_t, size_t, size_t> {
            check_range(precision);
            return geohash::int64::grid_properties(box, precision * 5);
          },
          py::arg("box") = py::none(), py::arg("precision") = 12,
          R"__doc__(
Returns the property of the grid covering the given bounding box.

Args:
  box (pyinterp.geodetic.Box, optional): Bounding box.
  precision (int, optional): Required accuracy.
Returns:
  tuple: geohash of the minimum corner point, number of boxes in longitudes
    and latitudes.
Raises:
  ValueError: If the given precision is not within [1, 12].
)__doc__")
      .def(
          "where",
          // We want to return an associative dictionary between bytes and
          // tuples and not str and tuples.
          [](const pybind11::array& hash) -> py::dict {
            auto result = py::dict();
            for (auto&& item : geohash::string::where(hash)) {
              auto key = py::bytes(item.first);
              result[key] = py::cast(item.second);
            }
            return result;
          },
          py::arg("hash"),
          R"__doc__(
Returns the start and end indexes of the different GeoHash boxes.

Args:
  hash (numpy.ndarray): Geohash codes.
Returns:
  dict: dictionary between successive identical geohash codes and start and
    end indexes in the table provided as input.
)__doc__");
}


#ifndef STAN_MATH_REV_CORE_VAR_HPP
#define STAN_MATH_REV_CORE_VAR_HPP

#include <stan/math/rev/core/vari.hpp>
#include <stan/math/rev/core/grad.hpp>
#include <stan/math/rev/core/chainable_alloc.hpp>
#include <boost/math/tools/config.hpp>
#include <ostream>
#include <vector>

namespace stan {
  namespace math {

    // forward declare
    static void grad(vari* vi);

    /**
     * Independent (input) and dependent (output) variables for gradients.
     *
     * This class acts as a smart pointer, with resources managed by
     * an agenda-based memory manager scoped to a single gradient
     * calculation.
     *
     * An var is constructed with a double and used like any
     * other scalar.  Arithmetical functions like negation, addition,
     * and subtraction, as well as a range of mathematical functions
     * like exponentiation and powers are overridden to operate on
     * var values objects.
     */
    class var {
    public:
      // FIXME: doc what this is for
      typedef double Scalar;

      /**
       * Pointer to the implementation of this variable.
       *
       * This value should not be modified, but may be accessed in
       * var operators to construct vari
       * instances.
       */
      vari * vi_;

      /**
       * Return true if this variable has been
       * declared, but not been defined.  Any attempt to use an
       * undefined variable's value or adjoint will result in a
       * segmentation fault.
       *
       * @return true if this variable does not yet have
       * a defined variable.
       */
      bool is_uninitialized() {
        return (vi_ == static_cast<vari*>(0U));
      }

      /**
       * Construct a variable for later assignment.
       *
       * This is implemented as a no-op, leaving the underlying implementation
       * dangling.  Before an assignment, the behavior is thus undefined just
       * as for a basic double.
       */
      var() : vi_(static_cast<vari*>(0U)) { }

      /**
       * Construct a variable from a pointer to a variable implementation.
       *
       * @param vi Variable implementation.
       */
      var(vari* vi) : vi_(vi) {  }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(float x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument as
       * a value and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(double x) : vi_(new vari(x)) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(long double x) : vi_(new vari(x)) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(bool x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(char x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(short x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(int x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(long x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(unsigned char x)  // NOLINT(runtime/explicit)
      : vi_(new vari(static_cast<double>(x))) { }

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      // NOLINTNEXTLINE
      var(unsigned short x) : vi_(new vari(static_cast<double>(x))) { }

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(unsigned int x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      // NOLINTNEXTLINE
      var(unsigned long x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

#ifdef _WIN64

      // these two ctors are for Win64 to enable 64-bit signed
      // and unsigned integers, because long and unsigned long
      // are still 32-bit

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(size_t x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(ptrdiff_t x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT
#endif

#ifdef BOOST_MATH_USE_FLOAT128

      // this ctor is for later GCCs that have the __float128
      // type enabled, because it gets enabled by boost

      /**
       * Construct a variable from the specified arithmetic argument
       * by constructing a new vari with the argument
       * cast to double, and a zero adjoint.
       *
       * @param x Value of the variable.
       */
      var(__float128 x) : vi_(new vari(static_cast<double>(x))) { }  // NOLINT

#endif

      /**
       * Return the value of this variable.
       *
       * @return The value of this variable.
       */
      inline double val() const {
        return vi_->val_;
      }

      /**
       * Return the derivative of the root expression with
       * respect to this expression.  This method only works
       * after one of the grad() methods has been
       * called.
       *
       * @return Adjoint for this variable.
       */
      inline double adj() const {
        return vi_->adj_;
      }

      /**
       * Compute the gradient of this (dependent) variable with respect to
       * the specified vector of (independent) variables, assigning the
       * specified vector to the gradient.
       *
       * The grad() function does not recover memory.  In Stan
       * 2.4 and earlier, this function did recover memory.
       *
       * @param x Vector of independent variables.
       * @param g Gradient vector of partial derivatives of this
       * variable with respect to x.
       */
      void grad(std::vector<var>& x,
                std::vector<double>& g) {
        stan::math::grad(vi_);
        g.resize(x.size());
        for (size_t i = 0; i < x.size(); ++i)
          g[i] = x[i].vi_->adj_;
      }

      /**
       * Compute the gradient of this (dependent) variable with respect
       * to all (independent) variables.
       *
       * The grad() function does not recover memory.
       */
      void grad() {
        stan::math::grad(vi_);
      }

      // POINTER OVERRIDES

      /**
       * Return a reference to underlying implementation of this variable.
       *
       * If x is of type var, then applying
       * this operator, *x, has the same behavior as
       * *(x.vi_).
       *
       * Warning:  The returned reference does not track changes to
       * this variable.
       *
       * @return variable
       */
      inline vari& operator*() {
        return *vi_;
      }

      /**
       * Return a pointer to the underlying implementation of this variable.
       *
       * If x is of type var, then applying
       * this operator, x->, behaves the same way as
       * x.vi_->.
       *
       * Warning: The returned result does not track changes to
       * this variable.
       */
      inline vari* operator->() {
        return vi_;
      }

      // COMPOUND ASSIGNMENT OPERATORS

      /**
       * The compound add/assignment operator for variables (C++).
       *
       * If this variable is a and the argument is the variable b,
       * then (a += b) behaves exactly the same way as (a = a + b),
       * creating an intermediate variable representing (a + b).
       *
       * @param b The variable to add to this variable.
       * @return The result of adding the specified variable to this variable.
       */
      inline var& operator+=(const var& b);

      /**
       * The compound add/assignment operator for scalars (C++).
       *
       * If this variable is a and the argument is the scalar b, then
       * (a += b) behaves exactly the same way as (a = a + b).  Note
       * that the result is an assignable lvalue.
       *
       * @param b The scalar to add to this variable.
       * @return The result of adding the specified variable to this variable.
       */
      inline var& operator+=(double b);

      /**
       * The compound subtract/assignment operator for variables (C++).
       *
       * If this variable is a and the argument is the variable b,
       * then (a -= b) behaves exactly the same way as (a = a - b).
       * Note that the result is an assignable lvalue.
       *
       * @param b The variable to subtract from this variable.
       * @return The result of subtracting the specified variable from
       * this variable.
       */
      inline var& operator-=(const var& b);

      /**
       * The compound subtract/assignment operator for scalars (C++).
       *
       * If this variable is a and the argument is the scalar b, then
       * (a -= b) behaves exactly the same way as (a = a - b).  Note
       * that the result is an assignable lvalue.
       *
       * @param b The scalar to subtract from this variable.
       * @return The result of subtracting the specified variable from this
       * variable.
       */
      inline var& operator-=(double b);

      /**
       * The compound multiply/assignment operator for variables (C++).
       *
       * If this variable is a and the argument is the variable b,
       * then (a *= b) behaves exactly the same way as (a = a * b).
       * Note that the result is an assignable lvalue.
       *
       * @param b The variable to multiply this variable by.
       * @return The result of multiplying this variable by the
       * specified variable.
       */
      inline var& operator*=(const var& b);

      /**
       * The compound multiply/assignment operator for scalars (C++).
       *
       * If this variable is a and the argument is the scalar b, then
       * (a *= b) behaves exactly the same way as (a = a * b).  Note
       * that the result is an assignable lvalue.
       *
       * @param b The scalar to multiply this variable by.
       * @return The result of multplying this variable by the specified
       * variable.
       */
      inline var& operator*=(double b);

      /**
       * The compound divide/assignment operator for variables (C++).  If this
       * variable is a and the argument is the variable b, then (a /= b)
       * behaves exactly the same way as (a = a / b).  Note that the
       * result is an assignable lvalue.
       *
       * @param b The variable to divide this variable by.
       * @return The result of dividing this variable by the
       * specified variable.
       */
      inline var& operator/=(const var& b);

      /**
       * The compound divide/assignment operator for scalars (C++).
       *
       * If this variable is a and the argument is the scalar b, then
       * (a /= b) behaves exactly the same way as (a = a / b).  Note
       * that the result is an assignable lvalue.
       *
       * @param b The scalar to divide this variable by.
       * @return The result of dividing this variable by the specified
       * variable.
       */
      inline var& operator/=(double b);

      /**
       * Write the value of this auto-dif variable and its adjoint to
       * the specified output stream.
       *
       * @param os Output stream to which to write.
       * @param v Variable to write.
       * @return Reference to the specified output stream.
       */
      friend std::ostream& operator<<(std::ostream& os, const var& v) {
        if (v.vi_ == 0)
          return os << "uninitialized";
        return os << v.val();
      }
    };

  }
}
#endif


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

#pragma once
#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include <fbm/fbm.hpp>
namespace fbmpy {

//! Adapter function for fbm::generate_fractional_brownian_bridge
inline boost::python::numpy::ndarray fractional_brownian_bridge_3d(
    double H, int nx, const boost::python::numpy::ndarray& X) {
    Py_intptr_t shape[1] = {(nx + 1)* (nx + 1)* (nx + 1)};
    auto bridge_data = boost::python::numpy::zeros(1, shape,
            boost::python::numpy::dtype::get_builtin<double>());

    fbm::fractional_brownian_bridge_3d(reinterpret_cast<double*>
        (bridge_data.get_data()),
        H,
        nx,
        reinterpret_cast<const double*>(X.get_data()));

    return bridge_data;

}

//! Adapter function for fbm::generate_fractional_brownian_bridge
inline boost::python::numpy::ndarray fractional_brownian_bridge_2d(
    double H, int nx, const boost::python::numpy::ndarray& X) {
    Py_intptr_t shape[1] = {(nx + 1)* (nx + 1)};
    auto bridge_data = boost::python::numpy::zeros(1, shape,
            boost::python::numpy::dtype::get_builtin<double>());

    fbm::fractional_brownian_bridge_2d(reinterpret_cast<double*>
        (bridge_data.get_data()),
        H,
        nx,
        reinterpret_cast<const double*>(X.get_data()));

    return bridge_data;

}

//! Adapter function for fbm::generate_fractional_brownian_bridge
inline boost::python::numpy::ndarray fractional_brownian_bridge_1d(
    double H, int nx, const boost::python::numpy::ndarray& X) {
    Py_intptr_t shape[1] = {(nx + 1)};
    auto bridge_data = boost::python::numpy::zeros(1, shape,
            boost::python::numpy::dtype::get_builtin<double>());

    fbm::fractional_brownian_bridge_1d(reinterpret_cast<double*>
        (bridge_data.get_data()),
        H,
        nx,
        reinterpret_cast<const double*>(X.get_data()));

    return bridge_data;

}

//! Adapter function for fbm::generate_fractional_brownian_motion
inline boost::python::numpy::ndarray fractional_brownian_motion_1d(
    double H, int nx, const boost::python::numpy::ndarray& X) {
    Py_intptr_t shape[1] = {(nx + 1)};
    auto bridge_data = boost::python::numpy::zeros(1, shape,
            boost::python::numpy::dtype::get_builtin<double>());

    fbm::fractional_brownian_motion_1d(reinterpret_cast<double*>
        (bridge_data.get_data()),
        H,
        nx,
        reinterpret_cast<const double*>(X.get_data()));

    return bridge_data;

}

//! Adapter function for fbm::generate_fractional_brownian_motion
inline boost::python::numpy::ndarray fractional_brownian_motion_2d(
    double H, int nx, const boost::python::numpy::ndarray& X) {
    Py_intptr_t shape[1] = {(nx + 1)* (nx + 1)};
    auto bridge_data = boost::python::numpy::zeros(1, shape,
            boost::python::numpy::dtype::get_builtin<double>());

    fbm::fractional_brownian_bridge_2d(reinterpret_cast<double*>
        (bridge_data.get_data()),
        H,
        nx,
        reinterpret_cast<const double*>(X.get_data()),
        false);

    return bridge_data;

}

//! Adapter function for fbm::generate_fractional_brownian_motion
inline boost::python::numpy::ndarray fractional_brownian_motion_3d(
    double H, int nx, const boost::python::numpy::ndarray& X) {
    Py_intptr_t shape[1] = {(nx + 1)* (nx + 1)* (nx + 1)};
    auto bridge_data = boost::python::numpy::zeros(1, shape,
            boost::python::numpy::dtype::get_builtin<double>());

    fbm::fractional_brownian_bridge_3d(reinterpret_cast<double*>
        (bridge_data.get_data()),
        H,
        nx,
        reinterpret_cast<const double*>(X.get_data()),
        false);

    return bridge_data;

}
}


#include <boost/multi_index_container.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/composite_key.hpp>
#include <boost/multi_index/random_access_index.hpp>
#include <boost/multi_index/sequenced_index.hpp>
#include <boost/multi_index/mem_fun.hpp>

#include <algorithm>
#include <iostream>
#include <random>
#include <chrono>
#include <set>
#include <unordered_set>

class Matrix {
 public:
  /* ///////////////////////////////////////////////////////////////////
  basic types
  */ ///////////////////////////////////////////////////////////////////
  using Index = uint32_t;
  using Value = std::complex<double>;
  using Clear = uint64_t;
  /* ///////////////////////////////////////////////////////////////////
  type T to store in container
  */ ///////////////////////////////////////////////////////////////////
  class T {
    friend class Matrix;
   private:
    Index x_; Index y_; Clear c_;
   public:
    Value v_;
    T() {}
    T(const Index& x, const Index& y, const Clear& c, const Value& v) :
      x_(x), y_(y), c_(c), v_(v) {}
    std::string to_string() const {
      std::ostringstream oss;
      oss.precision(1);
      oss << std::fixed ;
      oss << "(" << x_ << "," << y_ << "," << c_ << ")";
      oss << " ";
      oss << "(" << v_.real() << "," << v_.imag() << ")";
      return oss.str();
    }
  };
  struct S{
    friend class Matrix;
   public:
   mutable T* pt_;
   mutable Index i_;
    S() {}
    S(T* pt, const Index& i) {pt_ = pt; i_ = i;}
    Index getX() const {return pt_->x_;}
    Index getY() const {return pt_->y_;}
    Clear getC() const {return pt_->c_;}
    std::string to_string() const {
      std::ostringstream oss;
      oss << i_ << " " << pt_->to_string();
      return oss.str();
    }
  };

  /* ///////////////////////////////////////////////////////////////////
  Tags
  */ ///////////////////////////////////////////////////////////////////
  struct XY {};
  struct YX {};
  struct CXY {};
  /* ///////////////////////////////////////////////////////////////////
  Type for Deque Container

1. Insertion
b. deque- All iterators and references are invalidated, unless the inserted member is at an end (front or back) of the deque (in which case all iterators are invalidated, but references to elements are unaffected).
2. Erasure
b. deque- All iterators and references are invalidated unless the erased members are at an end (front or back) of the deque (in which case only iterators and references to the erased members are invalidated)
3. Resizing
a. vector, deque, and list- As per insert/erase.

  */ ///////////////////////////////////////////////////////////////////
  using Sequence = std::deque<T>;
  Sequence sequence_;
  /* ///////////////////////////////////////////////////////////////////
  Type for Boost's Multi-Index Container
  */ ///////////////////////////////////////////////////////////////////
  using Container = boost::multi_index_container<
    S, // the data type stored
    boost::multi_index::indexed_by<
      boost::multi_index::ordered_unique<
        boost::multi_index::tag<XY>,
        boost::multi_index::composite_key<
          S,
          boost::multi_index::const_mem_fun<S, Index, &S::getX>,
          boost::multi_index::const_mem_fun<S, Index, &S::getY>
        >,
        boost::multi_index::composite_key_compare<
          std::less<Index>,
          std::less<Index>
        >
      >
      ,
      boost::multi_index::ordered_unique<
        boost::multi_index::tag<YX>,
        boost::multi_index::composite_key<
          S,
          boost::multi_index::const_mem_fun<S, Index, &S::getY>,
          boost::multi_index::const_mem_fun<S, Index, &S::getX>
        >,
        boost::multi_index::composite_key_compare<
          std::less<Index>,
          std::less<Index>
        >
      >
      ,
      boost::multi_index::ordered_unique<
        boost::multi_index::tag<CXY>,
        boost::multi_index::composite_key<
          S,
          boost::multi_index::const_mem_fun<S, Clear, &S::getC>,
          boost::multi_index::const_mem_fun<S, Index, &S::getX>,
          boost::multi_index::const_mem_fun<S, Index, &S::getY>
        >,
        boost::multi_index::composite_key_compare<
          std::less<Clear>,
          std::less<Index>,
          std::less<Index>
        >
      >
    >
  >;
  /* ///////////////////////////////////////////////////////////////////
  variables
  */ ///////////////////////////////////////////////////////////////////
  Container container_;
  Index index_max_ = UINT32_MAX;
  Clear clear_max_ = UINT64_MAX;
  Clear c_ = 1;
  Index index_last_= 0;
  /* ///////////////////////////////////////////////////////////////////
  implicit methods
  void yx_sort();
  Container::index<order_xy>::type::iterator
    xy_find(const Matrix::Index& x, const Matrix::Index& y);
  void pesAB(const Value& s, Matrix& A, Matrix& B, bool sort);
  std::pair< Container::index<random_access>::type::iterator,
    Container::index<random_access>::type::iterator
  > random_access_equal_range_xy(const Index& x);
  std::pair< Container::index<random_access>::type::iterator,
    Container::index<random_access>::type::iterator
  > random_access_equal_range_yx(const Index& y);
  void add(const Index& x, const Index& y, const Value& v, bool sort);
  Container::index<Matrix::order_xy>::type::iterator
    xy_begin();
  Container::index<Matrix::order_yx>::type::iterator
    yx_begin();
  Container::index<Matrix::random_access>::type::iterator
    random_access_begin();
  Container::index<Matrix::random_access>::type::iterator
    random_access_end();
  Value getCoeff(const Index x, const Index y);
  void reserve(Index m);
  */ ///////////////////////////////////////////////////////////////////
  void insert(const Matrix::Index& x, const Matrix::Index& y,
    const Matrix::Value& v);
  void hard_clear();
  std::string to_string() const;
  template <class ForwardIt, class Compare> void
    special_quicksort(ForwardIt first, ForwardIt last, Compare compare);
  template<class ForwardIt, class UnaryPredicate> ForwardIt
    special_partition(ForwardIt first, ForwardIt last, UnaryPredicate p);
  template<class ForwardIt1, class ForwardIt2> constexpr void
    special_iter_swap(ForwardIt1 a, ForwardIt2 b);
  void sort_xy();
};

void Matrix::sort_xy() {
  struct LessXY {
     bool operator() (const T& lhs, const T& rhs) const {
       return (lhs.x_ != rhs.x_) ? (lhs.x_ < rhs.x_) : (lhs.y_ < rhs.y_);
     }
  };
  LessXY lessxy = LessXY();
  //special_quicksort(sequence_.begin(), sequence_.end(), lessxy);
  //auto it = container_.get<XY>().find(0,0);
  //std::cout << std::distance(sequence_.begin(),*((*it).pt_)) << std::endl;
  std::sort(sequence_.begin(), sequence_.end(), lessxy);
}

template <class ForwardIt, class Compare>
void
Matrix::special_quicksort(ForwardIt first, ForwardIt last, Compare compare)
{
   if(first == last) return;
   auto pivot = *std::next(first, std::distance(first,last)/2);
   ForwardIt middle1 = std::partition(first, last,
     [pivot,compare](const auto& em){ return compare(em,pivot); });
   ForwardIt middle2 = std::partition(middle1, last,
     [pivot,compare](const auto& em){ return !compare(em,pivot); });
   special_quicksort(first, middle1, compare);
   special_quicksort(middle2, last, compare);
}

template<class ForwardIt, class UnaryPredicate>
ForwardIt
Matrix::special_partition(ForwardIt first, ForwardIt last, UnaryPredicate p)
{
  first = std::find_if_not(first, last, p);
  if (first == last) return first;
    for (ForwardIt i = std::next(first); i != last; ++i) {
      if (p(*i)) {
        special_iter_swap(i, first);
        ++first;
      }
    }
    return first;
}

template<class ForwardIt1, class ForwardIt2>
constexpr void
Matrix::special_iter_swap(ForwardIt1 a, ForwardIt2 b)
  // constexpr since C++20
{
   std::swap(*a, *b);
}

void Matrix::insert(const Matrix::Index& x, const Matrix::Index& y,
  const Matrix::Value& v) {
  sequence_.push_back(T(x,y,c_,v));
  container_.insert(S(&sequence_[sequence_.size()-1],sequence_.size()-1));
  //auto it = container_.begin();
  //(*it).pt_ = &sequence_[0];
}

void Matrix::hard_clear() {
  container_.clear();
  sequence_.clear();
  index_last_ = 0;
}

std::string Matrix::to_string() const {
  auto it_seq = sequence_.begin();
  auto it_xy = container_.get<XY>().begin();
  auto it_yx = container_.get<YX>().begin();
  auto it_cxy = container_.get<CXY>().begin();
  std::ostringstream oss;
  oss << "seq(" << sequence_.size() << ") | ";
  oss << "xy(" << container_.size() << ") | ";
  oss << "yx(" << container_.size() << ") | ";
  oss << "cxy(" << container_.size() << ")\n";
  while(it_seq != sequence_.end()) {
    oss << it_seq->to_string() << " | ";
    oss << it_xy->to_string() << " | ";
    oss << it_yx->to_string() << " | ";
    oss << it_cxy->to_string() << "\n";
    it_seq++; it_xy++; it_yx++; it_cxy++;
  }
  return oss.str();
}

/* ///////////////////////////////////////////////////////////////////
explicit methods
void Matrix::reserve(Index m) {
  container_.get<random_access>().reserve(m);
}

Matrix::Value Matrix::getCoeff(const Matrix::Index x, const Matrix::Index y) {
  auto it = xy_find(x,y);
  return it->v_;
}

void Matrix::pesAB(const Matrix::Value& s, Matrix& A, Matrix& B, bool sort) {

  if(sort) {
    A.xy_sort();
    B.yx_sort();
  }

  auto iA = A.random_access_begin();
  auto iA_old = A.random_access_begin();
  Index xA;

  auto iB = B.random_access_begin();
  Index yB;

  Value v = 0;

  while(iA != A.random_access_end()) {
    xA = iA->x_;
    iA_old = iA;
    iB = B.random_access_begin();
if(false) {
    while(iB != B.random_access_end()) {
      iA = iA_old;
      yB = iB->y_;
      v = 0;
      while(xA == iA->x_ && iA != A.random_access_end()
        && yB == iB->y_ && iB != B.random_access_end()) {
        if(iA->y_ == iB->x_) {
          v += iA->v_ * iB->v_;
          iA++; iB++;
        } else if(iA->y_ < iB->x_) { iA++; } else { iB++; }
      }
      add(xA, yB, s*v, sort);
      while(yB == iB->y_ && iB != B.random_access_end()) {iB++;}
    }
}
    while(xA == iA->x_ && iA != A.random_access_end()) {iA++;}
  }
}

void Matrix::
add(const Matrix::Index& x, const Matrix::Index& y, const Matrix::Value& v, bool sort) {
  auto it = xy_find(x,y);
  if(it != container_.get<order_xy>().end()) {
    it->v_ += v;
  } else {
    container_.insert(T(x,y,v)); // very time consuming
  }
}

std::pair<
  Matrix::Container::index<Matrix::random_access>::type::iterator,
  Matrix::Container::index<Matrix::random_access>::type::iterator
> Matrix::
random_access_equal_range_xy(const Matrix::Index& x) {
  std::pair<
    Matrix::Container::index<Matrix::random_access>::type::iterator,
    Matrix::Container::index<Matrix::random_access>::type::iterator
  > res;
  auto rangeXY = container_.get<order_xy>().equal_range(x);
  res.first = container_.project<random_access>(rangeXY.first);
  res.second = container_.project<random_access>(rangeXY.second);
  return res;
}

std::pair<
  Matrix::Container::index<Matrix::random_access>::type::iterator,
  Matrix::Container::index<Matrix::random_access>::type::iterator
> Matrix::
random_access_equal_range_yx(const Matrix::Index& y) {
  std::pair<
    Matrix::Container::index<Matrix::random_access>::type::iterator,
    Matrix::Container::index<Matrix::random_access>::type::iterator
  > res;
  auto rangeYX = container_.get<order_yx>().equal_range(y);
  res.first = container_.project<random_access>(rangeYX.first);
  res.second = container_.project<random_access>(rangeYX.second);
  return res;
}

Matrix::Container::index<Matrix::random_access>::type::iterator
Matrix::random_access_begin() {
  return container_.get<random_access>().begin();
}

Matrix::Container::index<Matrix::random_access>::type::iterator
Matrix::random_access_end() {
  return container_.get<random_access>().end();
}

Matrix::Container::index<Matrix::order_xy>::type::iterator
Matrix::xy_begin() {
  return container_.get<order_xy>().begin();
}

Matrix::Container::index<Matrix::order_yx>::type::iterator
Matrix::yx_begin() {
  return container_.get<order_yx>().begin();
}

Matrix::Container::index<Matrix::order_xy>::type::iterator
Matrix::xy_find(const Matrix::Index& x, const Matrix::Index& y) {
  return container_.get<order_xy>().find(std::make_tuple(x,y));
}

void Matrix::yx_sort() {
  container_.get<random_access>().rearrange(
    container_.get<order_yx>().begin()
  );
}

*/ ///////////////////////////////////////////////////////////////////
/*

*/


#pragma once

#include "collisions.hpp"
#include "energy.hpp"
#include <Eigen/StdVector>

class AvatarCollisionsEnergy
{
public:
    template <typename FilterFunc>
    void compute(const TriMesh& mesh,
                 const AnimatedMesh& avatar,
                 const std::vector<Collision>& cs,
                 const Eigen::VectorXd& x, double offset,
                 Eigen::VectorXd& F,
                 SparseMatrixd& dFdx,
                 SparseMatrixd& dFdv,
                 Eigen::VectorXd& dx,
                 Eigen::VectorXd& dv,
                 Eigen::VectorXd& resid,
                 FilterFunc set_filter,
                 double mu, double dt) {

        const int n_verts = x.size() / 3;

        if (was_previously_colliding_.size() != n_verts) {
            was_previously_colliding_.resize(n_verts, false);
        }

        if (spring_anchors_.size() != n_verts) {
            spring_anchors_.resize(n_verts);
        }

        #pragma omp parallel for
        for (int i=0; i<n_verts; i++) {
            if (cs[i].tri_idx != -1 && cs[i].dx < offset) {
                const Eigen::Vector3d& n = cs[i].n;

                Eigen::Vector3d avatar_vel = Eigen::Vector3d::Zero();
                for (int j=0; j<3; j++) {
                    avatar_vel.noalias() += avatar.v.segment<3>(3*avatar.f(cs[i].tri_idx,j)) * cs[i].w[j];
                }

                bool colliding = true;

                /*
                // If it was previously colliding, but is now outside
                // the offset "band" _and_ the previous step was holding it
                // back, then let this one go
                if (was_previously_colliding_[i]) {
                    if (cs[i].dx > 0.8 * offset &&
                            resid.segment<3>(3*i).dot(n) > 0.0) {
                        colliding = false;
                    }
                }
                // Outside the avatar and the difference will be made
                // up by the relative velocity
                else if (cs[i].dx > 0.0 &&
                        (offset - cs[i].dx) <
                        (mesh.v.segment<3>(3*i) * dt -
                         avatar_vel).dot(n)) {
                    colliding = false;
                }
                */

                if (colliding) {
                    F.segment<3>(3*i) += cs[i].n * ((offset - cs[i].dx) * ks_ - mesh.v.segment<3>(3*i).dot(n) * kd_);
                    for (int j=0; j<3; j++) {
                        for (int k=0; k<3; k++) {
                            dFdx.coeffRef(3*i+j, 3*i+k) -= n[j] * n[k] * ks_;
                            dFdv.coeffRef(3*i+j, 3*i+k) -= n[j] * n[k] * kd_;
                        }
                    }

                    /*
                    double d = cs[i].dx < 0.0 ? 0.05 * offset - cs[i].dx
                        : 0.5 * (offset * 0.8 - cs[i].dx);

                    set_filter(i, Eigen::Matrix3d::Identity() - n * n.transpose());

                    dx.segment<3>(3*i) = n * d;
                    if (mesh.v.segment<3>(3*i).dot(n) < avatar_vel.dot(n)) {
                        dv.segment<3>(3*i) = n.dot(avatar_vel - mesh.v.segment<3>(3*i)) * n;
                    }
                    */

                    if (mu > 1.0e-8)
                    {
                        //const double r_i = resid.segment<3>(3*i).dot(n);
                        const double r_i = (offset - cs[i].dx) * ks_ - mesh.v.segment<3>(3*i).dot(n) * kd_;

                        Vec3d vt = (mesh.v.segment<3>(3*i) - cs[i].n * cs[i].n.dot(mesh.v.segment<3>(3*i)));
                        const double vt_l = vt.norm();
                        if (vt_l > mu * r_i) {
                            vt.normalize();
                        F.segment<3>(3*i) -= mu * r_i * vt;
                        for (int j=0; j<3; j++) {
                            for (int k=0; k<3; k++) {
                                dFdv.coeffRef(3*i+j,3*i+k) -= vt[j] * vt[k] * mu * r_i;
                            }
                        }
                        }

                        /*
                        if (!was_previously_colliding_[i]) {
                            spring_anchors_[i] = x.segment<3>(3*i) + cs[i].n * (offset - cs[i].dx);
                        } else if (r_i > 0.0) {
                            Vec3d spring_vec = mesh.x.segment<3>(3*i) - spring_anchors_[i];

                            // Find the tangential portion of the spring
                            Vec3d spring_tang = spring_vec - n * n.dot(spring_vec);

                            // mu is the friction spring stiffness, so this is the force
                            //Vec3d f_fric = mu * spring_tang;

                            // If the the sliding force exceeds the normal force, then this
                            // is the dynamic friction case, and we need to update the anchor
                            //const double Ft = f_fric.norm();

                            Vec3d nt = spring_tang.normalized();

                            //Vec3d f_fric = nt * mu * mesh.v.segment<3>(3*i).dot(nt);

                            //if (Ft > Fn) {
                            //if (std::fabs(mesh.v.segment<3>(3*i).dot(nt)) > mu * std::fabs(r_i)) {
                            if (std::fabs(mesh.v.segment<3>(3*i).dot(nt)) > mu * std::fabs(r_i)) {
                                Vec3d f_fric = mu * std::fabs(r_i) * nt;
                                F.segment<3>(3*i) -= f_fric;
                                for (int j=0; j<3; j++) {
                                    for (int k=0; k<3; k++) {
                                        dFdv.coeffRef(3*i+j,3*i+k) -= nt[j] * nt[k] * mu;
                                    }
                                }

                                // The dx needed to make Fn == Ft
                                spring_anchors_[i] += nt * ((std::fabs(mesh.v.segment<3>(3*i).dot(nt)) - std::fabs(r_i)) / mu);
                            } else {
                                // Static friction. Just freeze the vertex
                                set_filter(i, Eigen::Matrix3d::Zero());
                            }
                        }
                        */

                        /*
                        if (!was_previously_colliding_[i]) {
                            // If relative velocity is low, pin it
                            Eigen::Vector3d rel_vel = mesh.v.segment<3>(3*i) - avatar_vel;
                            double vn = n.dot(rel_vel);

                            Eigen::Vector3d vt = rel_vel - n * vn;
                            double Fn = n.dot(F.segment<3>(3*i)) * dt / mesh.m[i];

                            if (vt.norm() < mu * std::fabs(Fn)) {
                                set_filter(i, Eigen::Matrix3d::Zero());
                            }
                        } else {
                            // If tangential force exceeds some fraction of normal force
                            // Allow to slide
                            double Ft = (F.segment<3>(3*i) - n * F.segment<3>(3*i).dot(n)).norm();

                            if (Ft < mu * std::fabs(r_i)) {
                                set_filter(i, Eigen::Matrix3d::Zero());
                            } else {
                                // For "high" sliding velocity, dissipative tangential force
                                // proportional to normal force
                                Eigen::Vector3d rel_vel = mesh.v.segment<3>(3*i) - avatar_vel;
                                double vn = n.dot(rel_vel);
                                Eigen::Vector3d vt = rel_vel - n * vn;

                                if (vt.norm() > mu * std::fabs(r_i)) {
                                    F.segment<3>(3*i) -= vt.normalized() * std::fabs(r_i) * mu;
                                }
                            }
                        }
                        */

                    /*
                        const double r_i = resid.segment<3>(3*i).dot(n);

                        if (r_i < 0.0 && was_previously_colliding_[i]) {
                            Eigen::Vector3d rel_vel = mesh.v.segment<3>(3*i) - avatar_vel;
                            double vn = n.dot(rel_vel);
                            Eigen::Vector3d vt = rel_vel - n * vn;
                            const double vt_mag = vt.norm();

                            if (vt_mag > 3.0 * mu * std::fabs(r_i))
                                continue;

                            const double dv_max = mu * std::fabs(r_i);
                            if (vt_mag > dv_max)
                                continue;

                            //Vec3d Ft = F_.segment<3>(3*i) - n * n.dot(F_.segment<3>(3*i));
                            //if (Ft.norm() > mu * std::fabs(Fn) * mesh.m[i] / dt)
                            //    continue;

                            set_filter(i, Eigen::Matrix3d::Zero());
                            dv.segment<3>(3*i) = -rel_vel;
                        }
                        */
                    }

                    was_previously_colliding_[i] = true;

                } else {
                    was_previously_colliding_[i] = false;
                }

            } else {
                was_previously_colliding_[i] = false;
            }
        }
    }

    size_t nbrEnergies(const TriMesh& mesh) const { return mesh.x.size() / 3; }

    void update(const TriMesh& mesh,
                const VecXd& x,
                 const AnimatedMesh& avatar,
                 const std::vector<Collision>& cs,
                 double offset, double dt,
                 VecXd& dx) {

        const int n_verts = mesh.x.size() / 3;

        if (lambda_.size() != n_verts) {
            lambda_ = VecXd::Zero(n_verts);
        }

        const double a = (1.0 / ks_) / (dt * dt);

	const double b = dt * dt * kd_;
	const double c = a * b / dt;

        #pragma omp parallel for
        for (int i=0; i<n_verts; i++) {
            if (cs[i].tri_idx != -1 && cs[i].dx < offset) {
                const Eigen::Vector3d& n = cs[i].n;

                const double C = (x.segment<3>(3*i) - avatar.x.segment<3>(3*avatar.f(cs[i].tri_idx,0))).dot(n) - offset;

                double dl = (-C - a * lambda_[i] - c * n.dot(x.segment<3>(3*i) - mesh.x.segment<3>(3*i))) / (1.0 / mesh.m[3*i] + a);
                dx.segment<3>(3*i) += n * dl / mesh.m[3*i];

                lambda_[i] += dl;
            }
        }
    }

    void reset() { lambda_.setZero(); }

    void friction(const TriMesh& mesh,
                  const std::vector<Collision>& cs,
                  double mu,
                  VecXd& x) const {
        const int n_verts = mesh.x.size() / 3;

        // Follow Coulomb's Law: dx_t <= mu * dx_n,
        #pragma omp parallel for
        for (int i=0; i<n_verts; i++) {
            if (cs[i].tri_idx != -1) {
                const Vec3d dx = x.segment<3>(3*i) - mesh.x.segment<3>(3*i);

                const double dx_n = dx.dot(cs[i].n);
                Vec3d dx_ortho = dx - cs[i].n * dx.dot(cs[i].n);

                const double dx_t = std::min(dx_ortho.norm(), mu * std::fabs(dx_n));

                x.segment<3>(3*i) -= dx_ortho.normalized() * dx_t;
            }
        }
    }

protected:
    const double ks_ = 1.0e6;
    const double kd_ = 10.0;

    std::vector<bool> was_previously_colliding_;

    std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> spring_anchors_;

    VecXd lambda_;
};


#include <iostream>
#include <algorithm>
#include <cmath>
#include <vector>
#include <map>
#include <set>
#include <tuple>
#include <stdbool.h>
#include <bitset>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>

namespace mp = boost::multiprecision;
using namespace std;

// label: a=0, b=1, c=2
void insert(map<mp::cpp_int, vector<int>>& cakes, int n, int label) {
    for(int i = 0 ; i < n ; ++i) {
        mp::cpp_int v;
        cin >> v;
        if(!cakes.count(v)) {
            cakes[v] = vector<int>(3);
            cakes[v][0] = 0;
            cakes[v][1] = 0;
            cakes[v][2] = 0;
        }
        cakes[v][label]++;
    }
}
int main(void) {
    int x,y,z;
    mp::cpp_int count;
    cin >> x;
    cin >> y;
    cin >> z;
    cin >> count;

    map<mp::cpp_int, vector<int>> cakes;
    insert(cakes, x, 0);
    insert(cakes, y, 1);
    insert(cakes, z, 2);

    mp::cpp_int now_count = 0;
    auto e = cakes.rend();
    for(auto i = cakes.rbegin() ; i != e ; ++i) {
        // select a
        while (i->second[0]) {
            mp::cpp_int sum = 0;
            i->second[0]--;
            sum += i->first;
            cout << "sel a sum:" << sum << " num:" << i->first << endl;
            // select b
            for(auto j = cakes.rbegin() ; j != e ; ++j) {
                while (j->second[1]) {
                    j->second[1]--;
                    sum += j->first;
                    cout << "sel b sum:" << sum << " num:" << j->first << endl;
                    // select c
                    for(auto k = cakes.rbegin() ; k != e ; ++k) {
                        while (k->second[2]) {
                            k->second[2]--;
                            sum += k->first;
                            cout << "sel c sum:" << sum << " num:" << k->first << endl;
                            cout << sum << endl;
                            // increment and halt
                            now_count++;
                            if(count <= now_count) {
                                return 0;
                            }
                            sum -= k->first;
                        }
                    }
                    // c
                    sum -= j->first;
                }
            }
            // b
        }
        // a
    }
    return 0;
}


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#include <cstdlib>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include "ublas_trace.hpp"
#include "ublas_elementwise.hpp"
#include "gauss_distribution.hpp"
#include "gaussian_process_normal.hpp"

namespace bayesopt
{

  namespace ublas = boost::numeric::ublas;

  GaussianProcessNormal::GaussianProcessNormal(size_t dim,
					       Parameters params,
					       const Dataset& data,
					       MeanModel& mean,
					       randEngine& eng):
    HierarchicalGaussianProcess(dim,params,data,mean,eng),
    mW0(params.mean.coef_mean.size()), mInvVarW(params.mean.coef_mean.size()),
    mD(params.mean.coef_mean.size(),params.mean.coef_mean.size())
  {
    mSigma = params.sigma_s;
    mW0 = params.mean.coef_mean;
    for (size_t ii = 0; ii < params.mean.coef_std.size(); ++ii)
      {
	double varii = params.mean.coef_std[ii] * params.mean.coef_std[ii];
	mInvVarW(ii) = 1/varii;
      }
     d_ = new GaussianDistribution(eng);
  }  // Constructor

  GaussianProcessNormal::~GaussianProcessNormal()
  {
    delete d_;
  } // Default destructor

  ProbabilityDistribution*
  GaussianProcessNormal::prediction(const vectord &query)
  {
    const double kq = computeSelfCorrelation(query);
    const vectord phi = mMean.getFeatures(query);

    vectord v = computeCrossCorrelation(query);

    inplace_solve(mL,v,ublas::lower_tag());

    vectord rq = phi - prod(v,mKF);

    vectord rho(rq);
    inplace_solve(mD,rho,ublas::lower_tag());

    double yPred = inner_prod(phi,mWMap) + inner_prod(v,mVf);
    double sPred = sqrt( mSigma * (kq - inner_prod(v,v)
			        + inner_prod(rho,rho)));

    if ((boost::math::isnan(yPred)) || (boost::math::isnan(sPred)))
      {
	throw std::runtime_error("Error in prediction. NaN found.");
      }

    d_->setMeanAndStd(yPred,sPred);
    return d_;
  }

  double GaussianProcessNormal::negativeLogLikelihood()
  {
    matrixd KK = computeCorrMatrix();
    const size_t n = KK.size1();
    const size_t p = mMean.nFeatures();

    vectord v0 = mData.mY - prod(trans(mMean.mFeatM),mW0);
    matrixd WW = zmatrixd(p,p);  //TODO: diagonal matrix
    utils::add_to_diagonal(WW,mInvVarW);
    matrixd FW = prod(trans(mMean.mFeatM),WW);
    KK += prod(FW,mMean.mFeatM);
    matrixd BB(n,n);
    utils::cholesky_decompose(KK,BB);
    inplace_solve(BB,v0,ublas::lower_tag());
    double zz = inner_prod(v0,v0);

    double lik = 1/(2*mSigma) * zz;
    lik += utils::log_trace(BB);
    return lik;
  }

  void GaussianProcessNormal::precomputePrediction()
  {
    const size_t p = mMean.nFeatures();

    mKF = trans(mMean.mFeatM);
    inplace_solve(mL,mKF,ublas::lower_tag());
    //TODO: make one line
    matrixd DD(p,p);
    DD = prod(trans(mKF),mKF);
    utils::add_to_diagonal(DD,mInvVarW);
    utils::cholesky_decompose(DD,mD);

    vectord vn = mData.mY;
    inplace_solve(mL,vn,ublas::lower_tag());
    mWMap = prod(mMean.mFeatM,vn) + utils::ublas_elementwise_prod(mInvVarW,mW0);
    utils::cholesky_solve(mD,mWMap,ublas::lower());

    mVf = mData.mY - prod(trans(mMean.mFeatM),mWMap);
    inplace_solve(mL,mVf,ublas::lower_tag());

    if (boost::math::isnan(mWMap(0)))
      {
	throw std::runtime_error("Error in precomputed prediction. NaN found.");
      }
  }

} //namespace bayesopt


#include <boost/lexical_cast.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/program_options.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread/thread.hpp>
#include <SFML/Audio.hpp>

namespace po = boost::program_options;
namespace pt = boost::posix_time;

#include <iostream>
#include <random>

#include "sounds/242501__gabrielaraujo__powerup-success.cc"
#include "sounds/253886__themusicalnomad__negative-beeps.cc"
#include "sounds/202498__xtrsounder__small-crowd-cheering-and-clapping-at-party-2.h"

class factor_pair {
public:
    int left;
    int right;
    factor_pair(int l, int r) :left(l), right(r) {}
};

class Player {
private:
    sf::SoundBuffer correct_buf;
    sf::SoundBuffer incorrect_buf;
    sf::SoundBuffer cheering_buf;
    sf::Sound correct;
    sf::Sound incorrect;
    sf::Sound cheering;
public:
    Player() {
        correct_buf.loadFromMemory(success_wav, success_wav_len);
        incorrect_buf.loadFromMemory(fail_wav, fail_wav_len);
        cheering_buf.loadFromMemory(cheering_wav, cheering_wav_len);
        correct.setBuffer(correct_buf);
        incorrect.setBuffer(incorrect_buf);
        cheering.setBuffer(cheering_buf);
    }
    void right() { correct.play(); }
    void wrong() { incorrect.play(); }
    void acclaim() { cheering.play(); }
    bool playing() {
        return correct.getStatus() == sf::SoundSource::Status::Playing
            || incorrect.getStatus() == sf::SoundSource::Status::Playing
            || cheering.getStatus() == sf::SoundSource::Status::Playing;
    }
};

int main(int argc, char *argv[])
{
    Player player{};
    int max_factor;
    double max_seconds;
    po::options_description description("allowed options");
    description.add_options()
            ("help", "print help message")
            ("max", po::value<int>(&max_factor)->default_value(12), "maximum factor")
            ("seconds", po::value<double>(&max_seconds)->default_value(5.0), "max seconds for response");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, description), vm);
    po::notify(vm);

    if (vm.count("help")) {
        std::cout << description << std::endl;
        return 0;
    }
    std::cout << "seconds: " << max_seconds << std::endl;

    std::vector<factor_pair> v;
    std::default_random_engine rng(std::random_device{}());

    for (int i = 0; i <= max_factor; ++i) {
                for (int j = 0; j <= max_factor; ++j) {
                    for (int k = 0; k < i+j; k++) {
                                v.push_back(factor_pair(i, j));
                        }
                }
    }

    auto game_begin = pt::microsec_clock::local_time();
    while (v.size() > 0) {
        int selection = std::uniform_int_distribution<int>{0, ((int)v.size())-1}(rng);
        factor_pair p = v[selection];
        int correct_answer = p.left * p.right;
        std::string response;
        int answer;
        auto before = pt::microsec_clock::local_time();
        std::cout << p.left << " x " << p.right << std::endl;
        std::cin >> response;
        try {
            answer = boost::lexical_cast<int>(response);
        } catch (boost::bad_lexical_cast &e) {
            std::cout << "Whatever." << std::endl;
            answer = correct_answer - 1;  // Make it count as wrong.
        }
        auto delay = pt::microsec_clock::local_time() - before;
        if (std::cin.eof())
            break;

        if (answer != correct_answer) {
            std::cout << "NO!  The answer is " << correct_answer << ".  ";
            v.push_back(p);
            player.wrong();
        } else {
            if (delay.total_milliseconds() <= max_seconds * 1000) {
                std::cout << "You're fast!" << std::endl;
                v.erase(
                    std::remove_if(
                        v.begin(),
                        v.end(),
                        [p](const factor_pair& f) {
                            return (p.left == f.left && p.right == f.right) ||
                                (p.left == f.right && p.right == f.left);
                        }),
                    v.end());
                player.right();
            }
            std::cout << "Correct!  ";
        }
        std::cout << v.size() << " remaining." << std::endl;
    }

    if (v.size() == 0) {
        player.acclaim();
        while (player.playing()) {
            boost::this_thread::sleep(boost::posix_time::milliseconds(100));
        }
    }
    std::cout
        << "You played for "
        << (pt::microsec_clock::local_time() - game_begin).total_seconds()
        << " seconds."
        << std::endl;
    return 0;
}