#define CGAL_PMP_REPAIR_POLYGON_SOUP_VERBOSE 1

#include "Scene_polygon_soup_item.h"
#include "Scene_surface_mesh_item.h"

#include <CGAL/Three/Viewer_interface.h>
#include <CGAL/Three/Triangle_container.h>
#include <CGAL/Three/Edge_container.h>
#include <CGAL/Three/Point_container.h>
#include <CGAL/Three/Three.h>

#include "triangulate_primitive.h"

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/array.h>
#include <CGAL/IO/OFF.h>
#include <CGAL/Polygon_mesh_processing/compute_normal.h>
#include <CGAL/Polygon_mesh_processing/polygon_soup_to_polygon_mesh.h>
#include <CGAL/Polygon_mesh_processing/polygon_mesh_to_polygon_soup.h>
#include <CGAL/Polygon_mesh_processing/orient_polygon_soup.h>
#include <CGAL/Polygon_mesh_processing/orientation.h>
#include <CGAL/Polygon_mesh_processing/repair.h>
#include <CGAL/Polygon_mesh_processing/repair_polygon_soup.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/version.h>

#include <QObject>
#include <QApplication>
#include <QtDebug>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/median.hpp>

#include <algorithm>
#include <iostream>
#include <map>
#include <queue>
#include <set>
#include <stack>
#include <streambuf>
#include <vector>

using namespace CGAL::Three;
typedef Viewer_interface Vi;
typedef Triangle_container Tc;
typedef Edge_container Ec;
typedef Point_container Pc;

struct Scene_polygon_soup_item_priv{

  typedef Polygon_soup::Polygons::const_iterator Polygons_iterator;
  typedef EPICK::Point_3 Point_3;

  Scene_polygon_soup_item_priv(Scene_polygon_soup_item* parent)
    : soup(nullptr),
      oriented(false)
  {
    item = parent;
    nb_polys = 0;
    nb_lines = 0;
    nb_nm_edges = 0;
    invalidate_stats();
  }
  ~Scene_polygon_soup_item_priv()
  {
    if(soup)
    {
      delete soup;
      soup = nullptr;
    }
  }
  void compute_normals_and_vertices(void) const;
  void triangulate_polygon(Polygons_iterator, int ) const;
  void invalidate_stats();
  void compute_stats();
  mutable QOpenGLShaderProgram *program;

  enum Face_names {
      Flat_facets=0,
      Smooth_facets,
  };
  enum Edge_names {
    Edges = 0,
    NM_edges
  };

  Polygon_soup* soup;
  bool oriented;
  mutable std::vector<float> positions_poly;
  mutable std::vector<float> positions_lines;
  mutable std::vector<float> f_colors;
  mutable std::vector<float> v_colors;
  mutable std::vector<float> normals;
  mutable std::vector<float> positions_nm_lines;
  mutable std::size_t nb_nm_edges;
  mutable std::size_t nb_polys;
  mutable std::size_t nb_lines;
  bool is_triangle, is_quad, stats_computed;
  double minl, maxl, meanl, midl, mini, maxi, ave;
  std::size_t nb_null_edges, nb_degen_faces;

  Scene_polygon_soup_item* item;

};

typedef Scene_polygon_soup_item_priv Priv;

typedef EPICK Traits;
typedef Polygon_soup::Polygon_3 Facet;

void
Scene_polygon_soup_item_priv::triangulate_polygon(Polygons_iterator pit, int polygon_id) const
{
  const CGAL::qglviewer::Vec off = static_cast<CGAL::Three::Viewer_interface*>(CGAL::QGLViewer::QGLViewerPool().first())->offset();
  EPICK::Vector_3 offset(off.x,off.y,off.z);

    //Computes the normal of the facet
    Traits::Vector_3 normal = CGAL::NULL_VECTOR;

    //Newell's method
    for (std::size_t i = 0; i < pit->size() ; ++ i){
      const Point_3& pa = soup->points[pit->at(i)];
      const Point_3& pb = soup->points[pit->at((i+1)%pit->size())];
      double x = normal.x() + (pa.y()-pb.y())*(pa.z()+pb.z());
      double y = normal.y() + (pa.z()-pb.z())*(pa.x()+pb.x());
      double z = normal.z() + (pa.x()-pb.x())*(pa.y()+pb.y());
      normal = Traits::Vector_3(x,y,z);
    }
    if (normal == CGAL::NULL_VECTOR) // No normal could be computed, return
      return;

    typedef FacetTriangulator<SMesh, EPICK, std::size_t> FT;

    std::size_t it = 0;
    std::size_t it_end =pit->size();
    std::vector<FT::PointAndId> pointIds;
    do {
      FT::PointAndId pointId;

      pointId.point = soup->points[pit->at(it)]+offset;
      pointId.id = pit->at(it);
      pointIds.push_back(pointId);
    } while( ++it != it_end );
    //detect degenerated faces
    std::vector<FT::PointAndId> pid_stack = pointIds;
    for(std::size_t i = 0; i< pointIds.size(); ++i)
    {
     FT::PointAndId pid = pid_stack.back();
     pid_stack.pop_back();
     for(FT::PointAndId poai : pid_stack)
     {
      if (pid.point== poai.point)
      {
        return;
      }
     }
    }
    FT triangulation(pointIds,normal);
    //iterates on the internal faces to add the vertices to the positions
    //and the normals to the appropriate vectors
    for(FT::CDT::Finite_faces_iterator
        ffit = triangulation.cdt->finite_faces_begin(),
        end = triangulation.cdt->finite_faces_end();
        ffit != end; ++ffit)
    {
        if(ffit->info().is_external)
            continue;

        positions_poly.push_back(ffit->vertex(0)->point().x());
        positions_poly.push_back(ffit->vertex(0)->point().y());
        positions_poly.push_back(ffit->vertex(0)->point().z());

        positions_poly.push_back(ffit->vertex(1)->point().x());
        positions_poly.push_back(ffit->vertex(1)->point().y());
        positions_poly.push_back(ffit->vertex(1)->point().z());

        positions_poly.push_back(ffit->vertex(2)->point().x());
        positions_poly.push_back(ffit->vertex(2)->point().y());
        positions_poly.push_back(ffit->vertex(2)->point().z());

        CGAL::IO::Color color;
        if(!soup->fcolors.empty())
          color = soup->fcolors[polygon_id];
        for(int i=0; i<3; i++)
        {
          normals.push_back(normal.x());
          normals.push_back(normal.y());
          normals.push_back(normal.z());
          if(!soup->fcolors.empty())
          {
            f_colors.push_back((float)color.red()/255);
            f_colors.push_back((float)color.green()/255);
            f_colors.push_back((float)color.blue()/255);
          }
          if(!soup->vcolors.empty())
          {
            CGAL::IO::Color vcolor = soup->vcolors[triangulation.v2v[ffit->vertex(i)]];
            v_colors.push_back((float)vcolor.red()/255);
            v_colors.push_back((float)vcolor.green()/255);
            v_colors.push_back((float)vcolor.blue()/255);
          }
        }
    }
}
void
Scene_polygon_soup_item_priv::compute_normals_and_vertices() const{

    //get the vertices and normals
    const CGAL::qglviewer::Vec offset = static_cast<CGAL::Three::Viewer_interface*>(CGAL::QGLViewer::QGLViewerPool().first())->offset();

    typedef Polygon_soup::Polygons::size_type size_type;
    positions_poly.resize(0);
    positions_lines.resize(0);
    normals.resize(0);
    positions_nm_lines.resize(0);
    f_colors.resize(0);
    v_colors.resize(0);
    soup->fill_edges();
    int nb = 0;
    for(Polygons_iterator it = soup->polygons.begin();
        it != soup->polygons.end(); ++it)
    {
        if(it->size()!=3)
        {
            triangulate_polygon(it, nb);
        }
        else{
            const Point_3& pa = soup->points[it->at(0)];
            const Point_3& pb = soup->points[it->at(1)];
            const Point_3& pc = soup->points[it->at(2)];

            EPICK::Vector_3 n = CGAL::cross_product(pb-pa, pc -pa);
            n = n / std::sqrt(n * n);

            normals.push_back(n.x());
            normals.push_back(n.y());
            normals.push_back(n.z());

            normals.push_back(n.x());
            normals.push_back(n.y());
            normals.push_back(n.z());

            normals.push_back(n.x());
            normals.push_back(n.y());
            normals.push_back(n.z());

            for(size_type i = 0; i < it->size(); ++i)
            {
                const Point_3& p = soup->points[it->at(i)];
                positions_poly.push_back(p.x()+offset.x);
                positions_poly.push_back(p.y()+offset.y);
                positions_poly.push_back(p.z()+offset.z);
                if(!soup->fcolors.empty())
                {
                  const CGAL::IO::Color color = soup->fcolors[nb];
                    f_colors.push_back((float)color.red()/255);
                    f_colors.push_back((float)color.green()/255);
                    f_colors.push_back((float)color.blue()/255);
                }

                if(!soup->vcolors.empty())
                {
                  const CGAL::IO::Color color = soup->vcolors[it->at(i)];
                  v_colors.push_back((float)color.red()/255);
                  v_colors.push_back((float)color.green()/255);
                  v_colors.push_back((float)color.blue()/255);
                }
            }
        }
        nb++;

        //Lines
        for(size_type i = 0; i < it->size(); ++i)
        {

            const Point_3& pa = soup->points[it->at(i)];
            const Point_3& pb = soup->points[it->at((i+1)%it->size())];
            positions_lines.push_back(pa.x()+offset.x);
            positions_lines.push_back(pa.y()+offset.y);
            positions_lines.push_back(pa.z()+offset.z);

            positions_lines.push_back(pb.x()+offset.x);
            positions_lines.push_back(pb.y()+offset.y);
            positions_lines.push_back(pb.z()+offset.z);
        }
    }

    //Non manifold edges
    for(const Polygon_soup::Edge& edge :
                    soup->non_manifold_edges)
    {

        const Point_3& a = soup->points[edge[0]];
        const Point_3& b = soup->points[edge[1]];
        positions_nm_lines.push_back(a.x()+offset.x);
        positions_nm_lines.push_back(a.y()+offset.y);
        positions_nm_lines.push_back(a.z()+offset.z);

        positions_nm_lines.push_back(b.x()+offset.x);
        positions_nm_lines.push_back(b.y()+offset.y);
        positions_nm_lines.push_back(b.z()+offset.z);
    }

}

Scene_polygon_soup_item::Scene_polygon_soup_item()
{
  d = new Scene_polygon_soup_item_priv(this);
  for(int i = 1; i>=0; --i)
  {
    setTriangleContainer(i,
                         new Tc(Vi::PROGRAM_WITH_LIGHT, false));
    setEdgeContainer(i,
                     new Ec(Vi::PROGRAM_NO_SELECTION, false));
  }
  setPointContainer(0,
                    new Pc(Vi::PROGRAM_NO_SELECTION, false));
}

Scene_polygon_soup_item::~Scene_polygon_soup_item()
{
  delete d;
}

Scene_polygon_soup_item*
Scene_polygon_soup_item::clone() const {
  Scene_polygon_soup_item* new_soup = new Scene_polygon_soup_item();
  new_soup->d->soup = d->soup->clone();
  new_soup->d->oriented = d->oriented;
  return new_soup;
}

bool
Scene_polygon_soup_item::load(std::istream& in)
{
  if (!d->soup)
    d->soup = new Polygon_soup();
  else
    d->soup->clear();

  bool result = CGAL::IO::read_OFF(in, d->soup->points, d->soup->polygons,
                                      CGAL::parameters::vertex_color_output_iterator(std::back_inserter(d->soup->vcolors))
                                                        .face_color_output_iterator(std::back_inserter(d->soup->fcolors)));

  invalidateOpenGLBuffers();
  return result;
}

void Scene_polygon_soup_item::init_polygon_soup(std::size_t nb_pts, std::size_t nb_polygons){
  if(!d->soup)
    d->soup = new Polygon_soup;
  d->soup->clear();
  d->soup->points.reserve(nb_pts);
  d->soup->polygons.reserve(nb_polygons);
  d->soup->vcolors.resize(0);
  d->soup->fcolors.resize(0);
  d->oriented = false;
}

template<class PolygonMesh>
void polygon_mesh_to_soup(PolygonMesh& mesh, Polygon_soup& soup)
{
  soup.clear();
  CGAL::Polygon_mesh_processing::polygon_mesh_to_polygon_soup(mesh, soup.points, soup.polygons);
  soup.fill_edges();
}

void Scene_polygon_soup_item::load(Scene_surface_mesh_item* sm_item) {
  if(!sm_item) return;
  if(!sm_item->face_graph()) return;

  if(!d->soup)
    d->soup = new Polygon_soup;
  polygon_mesh_to_soup(*sm_item->face_graph(), *d->soup);
  invalidateOpenGLBuffers();
}
void
Scene_polygon_soup_item::setDisplayNonManifoldEdges(const bool b)
{

  d->soup->display_non_manifold_edges = b;
}

bool
Scene_polygon_soup_item::displayNonManifoldEdges() const {

  return d->soup->display_non_manifold_edges;
}

void Scene_polygon_soup_item::shuffle_orientations()
{
  for(Polygon_soup::size_type i = 0, end = d->soup->polygons.size();
      i < end; ++i)
  {
    if(std::rand() % 2 == 0) d->soup->inverse_orientation(i);
  }
  invalidateOpenGLBuffers();
}

void Scene_polygon_soup_item::inside_out()
{
  for(Polygon_soup::size_type i = 0, end = d->soup->polygons.size();
      i < end; ++i)
  {
    d->soup->inverse_orientation(i);
  }
  invalidateOpenGLBuffers();
}

bool
Scene_polygon_soup_item::orient(std::vector<std::size_t>& non_manifold_vertices)
{
  struct Visitor : public CGAL::Polygon_mesh_processing::Default_orientation_visitor
  {
    const Polygon_soup::Polygons& polygons;
    std::set<std::size_t>& nm_vertices;
    std::set< std::pair<std::size_t, std::size_t> > nm_edges;

    Visitor(const Polygon_soup::Polygons& polygons,
            std::set<std::size_t>& nm_vertices)
      : polygons(polygons)
      , nm_vertices(nm_vertices)
    {}

    void non_manifold_edge(std::size_t v1, std::size_t v2, std::size_t)
    {
      nm_edges.insert(CGAL::make_sorted_pair(v1, v2));
    }

    void link_connected_polygons(std::size_t v, const std::vector<std::size_t>& polygons_in_current_cycle)
    {
      // check if the current components of polygon incident to the link of the vertex contains
      // a non-manifold edge. If no, then it is a "pure" non-manifold vertex in this component
      if (!nm_edges.empty())
        for(std::size_t pid : polygons_in_current_cycle)
        {
          for (std::size_t i=0; i<polygons[pid].size(); ++i)
          {
            if (polygons[pid][i]==v)
            {
              std::size_t vn1 = i==0?(polygons[pid].size()-1):(i-1);
              std::size_t vn2 = i+1;
              if (vn2==polygons[pid].size()) vn2=0;

              if (nm_edges.count(CGAL::make_sorted_pair(polygons[pid][i], polygons[pid][vn1]))==1) return;
              if (nm_edges.count(CGAL::make_sorted_pair(polygons[pid][i], polygons[pid][vn2]))==1) return;
              break;
            }
            else
              CGAL_assertion(i!=polygons[pid].size()-1); // the vertex is incident to the polygon
          }
        }
      nm_vertices.insert(v);
    }
  };
  if(isEmpty() || d->oriented)
    return true; // nothing to do
  QApplication::setOverrideCursor(Qt::WaitCursor);
  d->oriented=true;

  //first skip degenerate polygons
  Polygon_soup::Polygons valid_polygons;
  valid_polygons.reserve(d->soup->polygons.size());
  for(Polygon_soup::Polygon_3& polygon : d->soup->polygons)
  {
    std::set<std::size_t> vids;
    bool to_remove=false;
    for(std::size_t id : polygon)
    {
      if (!vids.insert(id).second){
        to_remove=true;
        break;
      }
    }
    if (!to_remove) valid_polygons.push_back(polygon);
  }
  QApplication::restoreOverrideCursor();
  if (valid_polygons.size()!=d->soup->polygons.size())
    d->soup->polygons.swap(valid_polygons);

  bool res;
  std::set<std::size_t> nm_v_set;
  Visitor visitor(valid_polygons, nm_v_set);
  QApplication::setOverrideCursor(Qt::WaitCursor);
  res =  CGAL::Polygon_mesh_processing::
    orient_polygon_soup(d->soup->points, d->soup->polygons, CGAL::parameters::visitor(visitor));
  non_manifold_vertices.assign(nm_v_set.begin(), nm_v_set.end());
  QApplication::restoreOverrideCursor();
  return res;
}

bool
Scene_polygon_soup_item::save(std::ostream& out) const
{

  typedef Polygon_soup::size_type size_type;
  CGAL::File_writer_OFF writer;
  writer.write_header(out,
                      d->soup->points.size(),
                      0,
                      d->soup->polygons.size());
  for(size_type i = 0, end = d->soup->points.size();
      i < end; ++i)
  {
    const Point_3& p = d->soup->points[i];
    writer.write_vertex( p.x(), p.y(), p.z() );
  }
  writer.write_facet_header();
  for(size_type i = 0, end = d->soup->polygons.size();
      i < end; ++i)
  {
    const Polygon_soup::Polygon_3& polygon = d->soup->polygons[i];
    const size_type size = polygon.size();
    writer.write_facet_begin(size);
    for(size_type j = 0; j < size; ++j) {
      writer.write_facet_vertex_index(polygon[j]);
    }
    writer.write_facet_end();
  }
  writer.write_footer();

  return (bool) out;
}

bool
Scene_polygon_soup_item::exportAsSurfaceMesh(SMesh *out_surface_mesh)
{
  std::vector<std::size_t> dum;
  orient(dum);
  CGAL::Polygon_mesh_processing::polygon_soup_to_polygon_mesh< CGAL::Surface_mesh<Point_3> >(
    d->soup->points, d->soup->polygons, *out_surface_mesh);
  std::size_t rv = CGAL::Polygon_mesh_processing::remove_isolated_vertices(*out_surface_mesh);
  if(rv > 0){
    std::cerr << "Ignore isolated vertices: " << rv << std::endl;
    out_surface_mesh->collect_garbage();
  }
  if(out_surface_mesh->vertices().size() > 0) {
    return true;
  }
  return false;
}
QString
Scene_polygon_soup_item::toolTip() const
{

  if(!d->soup)
    return QString();

  QString str = QObject::tr("
%1 (mode: %5, color: %6)
"
                     "Polygon soup
"
                     "
Number of vertices: %2
"
                     "Number of polygons: %3
")
    .arg(this->name())
    .arg(d->soup->points.size())
    .arg(d->soup->polygons.size())
    .arg(this->renderingModeName())
    .arg(this->color().name());
    str += QString("
Number of isolated vertices: %1
").arg(getNbIsolatedvertices());
    return str;
}

void
Scene_polygon_soup_item::draw(CGAL::Three::Viewer_interface* viewer) const {
    if(d->soup == nullptr) return;
    if(!isInit(viewer))
      initGL(viewer);
    if ( getBuffersFilled() &&
         ! getBuffersInit(viewer))
    {
      initializeBuffers(viewer);
      setBuffersInit(viewer, true);
    }
    if(!getBuffersFilled())
    {
      computeElements();
      initializeBuffers(viewer);
    }

    if(renderingMode() == Flat || renderingMode() == FlatPlusEdges)
    {

      if(d->soup->fcolors.empty())
        getTriangleContainer(Priv::Flat_facets)->setColor(this->color());
      getTriangleContainer(Priv::Flat_facets)->setAlpha(alpha());
      getTriangleContainer(Priv::Flat_facets)->draw(viewer, d->soup->fcolors.empty());
    }
    else if(renderingMode() == Gouraud)
    {
      if(d->soup->vcolors.empty())
        getTriangleContainer(Priv::Smooth_facets)->setColor(this->color());
      getTriangleContainer(Priv::Smooth_facets)->setAlpha(alpha());
      getTriangleContainer(Priv::Smooth_facets)->draw(viewer, d->soup->vcolors.empty());
    }
  }

void
Scene_polygon_soup_item::drawPoints(CGAL::Three::Viewer_interface* viewer) const {

    if(d->soup == nullptr) return;
    if(!isInit(viewer))
      initGL(viewer);
    if ( getBuffersFilled() &&
         ! getBuffersInit(viewer))
    {
      initializeBuffers(viewer);
      setBuffersInit(viewer, true);
    }
    if(!getBuffersFilled())
    {
      computeElements();
      initializeBuffers(viewer);
    }
    getPointContainer(0)->setColor(this->color());
    getPointContainer(0)->draw(viewer, true);
}

void
Scene_polygon_soup_item::drawEdges(CGAL::Three::Viewer_interface* viewer) const {
    if(d->soup == nullptr) return;
    if(!isInit(viewer))
      initGL(viewer);
    if ( getBuffersFilled() &&
         ! getBuffersInit(viewer))
    {
      initializeBuffers(viewer);
      setBuffersInit(viewer, true);
    }
    if(!getBuffersFilled())
    {
      computeElements();
      initializeBuffers(viewer);
    }
    getEdgeContainer(Priv::Edges)->setColor(QColor(Qt::black));
    getEdgeContainer(Priv::Edges)->draw(viewer, true);
    if(displayNonManifoldEdges())
    {
      getEdgeContainer(Priv::NM_edges)->setColor(QColor(Qt::red));
      //draw the edges
      getEdgeContainer(Priv::NM_edges)->draw(viewer, true);
    }
}

bool
Scene_polygon_soup_item::isEmpty() const {

  return (d->soup == nullptr || d->soup->points.empty());
}
void
Scene_polygon_soup_item::invalidateOpenGLBuffers()
{
    compute_bbox();
    for(int i=0; i<2; ++i)
    {
      getTriangleContainer(i)->reset_vbos(ALL);
      getEdgeContainer(i)->reset_vbos(ALL);
    }
    getPointContainer(0)->reset_vbos(ALL);
    setBuffersFilled(false);
    d->invalidate_stats();
}

void Scene_polygon_soup_item::compute_bbox() const {

  if (isEmpty())
    return;
  const Point_3& p = *(d->soup->points.begin());
  CGAL::Bbox_3 bbox(p.x(), p.y(), p.z(), p.x(), p.y(), p.z());
  for(Polygon_soup::Points::const_iterator it = d->soup->points.begin();
      it != d->soup->points.end();
      ++it) {
    bbox = bbox + it->bbox();
  }
  setBbox(Bbox(bbox.xmin(),bbox.ymin(),bbox.zmin(),
               bbox.xmax(),bbox.ymax(),bbox.zmax()));
}

void
Scene_polygon_soup_item::new_vertex(const double& x,
                                    const double& y,
                                    const double& z)
{

    d->soup->points.push_back(Point_3(x, y, z));
}

void
Scene_polygon_soup_item::new_triangle(const std::size_t i,
                                      const std::size_t j,
                                      const std::size_t k)
{

  Polygon_soup::Polygon_3 new_polygon(3);
  new_polygon[0] = i;
  new_polygon[1] = j;
  new_polygon[2] = k;
  d->soup->polygons.push_back(new_polygon);
}

template <class Point, typename Polygon>
void Scene_polygon_soup_item::load(const std::vector<Point>& points, const std::vector<Polygon>& polygons)
{
    if(!d->soup)
        d->soup = new Polygon_soup;
    d->soup->clear();

    /// add points
    d->soup->points.reserve(points.size());
    for(const Point& p : points)
            d->soup->points.push_back( Point_3(p[0], p[1], p[2]) );

    /// add polygons
    std::size_t nb_polygons=polygons.size();
    d->soup->polygons.resize(nb_polygons);
    for(std::size_t i=0; i<nb_polygons; ++i)
        d->soup->polygons[i].assign(polygons[i].begin(), polygons[i].end());

    /// fill non-manifold edges container
    //soup->fill_edges();
    d->oriented = false;
    invalidateOpenGLBuffers();
}

template <class Point, class Polygon>
void Scene_polygon_soup_item::load(const std::vector<Point>& points, const std::vector<Polygon>& polygons,
                                   const std::vector<CGAL::IO::Color>& fcolors,
                                   const std::vector<CGAL::IO::Color>& vcolors)
{
    load (points, polygons);

    d->soup->fcolors.reserve (fcolors.size());
    std::copy (fcolors.begin(), fcolors.end(), std::back_inserter (d->soup->fcolors));

    d->soup->vcolors.reserve (vcolors.size());
    std::copy (vcolors.begin(), vcolors.end(), std::back_inserter (d->soup->vcolors));
}
// Force the instanciation of the template function for the types used in the STL_io_plugin. This is needed
// because the d-pointer forbid the definition in the .h for this function.
template SCENE_POLYGON_SOUP_ITEM_EXPORT void Scene_polygon_soup_item::load<EPICK::Point_3, std::vector<int> >
(const std::vector<EPICK::Point_3>& points, const std::vector<std::vector<int> >& polygons);
template SCENE_POLYGON_SOUP_ITEM_EXPORT void Scene_polygon_soup_item::load<CGAL::Epick::Point_3, std::vector<std::size_t> >
(const std::vector<CGAL::Epick::Point_3>& points, const std::vector<std::vector<std::size_t> >& polygons);
template SCENE_POLYGON_SOUP_ITEM_EXPORT void Scene_polygon_soup_item::load<CGAL::Epick::Point_3, std::vector<std::size_t> >
(const std::vector<CGAL::Epick::Point_3>& points, const std::vector<std::vector<std::size_t> >& polygons,
 const std::vector<CGAL::IO::Color>& fcolors,
 const std::vector<CGAL::IO::Color>& vcolors);

// Local Variables:
// c-basic-offset: 4
// End:

const Scene_polygon_soup_item::Points& Scene_polygon_soup_item::points() const { return d->soup->points; }
const Scene_polygon_soup_item::Polygons& Scene_polygon_soup_item::polygons() const { return d->soup->polygons; }
bool Scene_polygon_soup_item::isDataColored() { return d->soup->fcolors.size()>0 || d->soup->vcolors.size()>0;}
std::vector<CGAL::IO::Color> Scene_polygon_soup_item::getVColors() const{return d->soup->vcolors;}
std::vector<CGAL::IO::Color> Scene_polygon_soup_item::getFColors() const{return d->soup->fcolors;}

void Scene_polygon_soup_item::itemAboutToBeDestroyed(Scene_item *item)
{
  Scene_item::itemAboutToBeDestroyed(item);
  if(d && item == this)
  {
    if(d->soup)
    {
      delete d->soup;
      d->soup=nullptr;
    }
  }
}

const Polygon_soup::Edges&
Scene_polygon_soup_item::non_manifold_edges() const
{
  return d->soup->non_manifold_edges;
}

void Scene_polygon_soup_item::initializeBuffers(Viewer_interface *v) const
{
  getTriangleContainer(Priv::Flat_facets)->initializeBuffers(v);
  getTriangleContainer(Priv::Flat_facets)->setFlatDataSize(d->nb_polys);

  getTriangleContainer(Priv::Smooth_facets)->initializeBuffers(v);
  getTriangleContainer(Priv::Smooth_facets)->setFlatDataSize(d->nb_polys);

  getEdgeContainer(Priv::Edges)->initializeBuffers(v);
  getEdgeContainer(Priv::Edges)->setFlatDataSize(d->nb_lines);

  getEdgeContainer(Priv::NM_edges)->initializeBuffers(v);
  getEdgeContainer(Priv::NM_edges)->setFlatDataSize(d->nb_nm_edges);

  getPointContainer(0)->initializeBuffers(v);
  getPointContainer(0)->setFlatDataSize(d->nb_lines);

  d->normals.resize(0);
  d->positions_poly.resize(0);
  d->normals.shrink_to_fit();
  d->positions_poly.shrink_to_fit();
  d->v_colors.resize(0);
  d->v_colors.shrink_to_fit();
  d->positions_lines.resize(0);
  d->positions_lines.shrink_to_fit();
  d->positions_nm_lines.resize(0);
  d->positions_nm_lines.shrink_to_fit();
}

void Scene_polygon_soup_item::computeElements() const
{
  QApplication::setOverrideCursor(Qt::WaitCursor);
  d->compute_normals_and_vertices();

  getTriangleContainer(Priv::Flat_facets)->allocate(
        Tc::Flat_vertices,
        d->positions_poly.data(),
        static_cast<int>(d->positions_poly.size()*sizeof(float)));

  getTriangleContainer(Priv::Flat_facets)->allocate(
        Tc::Flat_normals,
        d->normals.data(),
        static_cast<int>(d->normals.size()*sizeof(float)));
  if(!d->f_colors.empty())
  {
    getTriangleContainer(Priv::Flat_facets)->allocate(
          Tc::FColors,
          d->f_colors.data(),
          static_cast<int>(d->f_colors.size()*sizeof(float)));
  }
  getTriangleContainer(Priv::Smooth_facets)->allocate(
        Tc::Flat_vertices,
        d->positions_poly.data(),
        static_cast<int>(d->positions_poly.size()*sizeof(float)));

  getTriangleContainer(Priv::Smooth_facets)->allocate(
        Tc::Flat_normals,
        d->normals.data(),
        static_cast<int>(d->normals.size()*sizeof(float)));

  if(!d->v_colors.empty())
  {
    getTriangleContainer(Priv::Smooth_facets)->allocate(
          Tc::VColors,
          d->v_colors.data(),
          static_cast<int>(d->v_colors.size()*sizeof(float)));
  }

  d->nb_polys = d->positions_poly.size();

  getEdgeContainer(Priv::Edges)->allocate(
        Ec::Vertices,
        d->positions_lines.data(),
        static_cast<int>(d->positions_lines.size()*sizeof(float)));

  getPointContainer(0)->allocate(
        Pc::Vertices,
        d->positions_lines.data(),
        static_cast<int>(d->positions_lines.size()*sizeof(float)));

  getEdgeContainer(Priv::NM_edges)->allocate(
        Ec::Vertices,
        d->positions_nm_lines.data(),
        static_cast<int>(d->positions_nm_lines.size()*sizeof(float)));

  d->nb_nm_edges = d->positions_nm_lines.size();
  d->nb_lines = d->positions_lines.size();

  setBuffersFilled(true);
  QApplication::restoreOverrideCursor();
}

void Scene_polygon_soup_item::repair(bool erase_dup, bool req_same_orientation)
{
  QApplication::setOverrideCursor(Qt::BusyCursor);
  CGAL::Polygon_mesh_processing::repair_polygon_soup(
        d->soup->points,
        d->soup->polygons,
        CGAL::Polygon_mesh_processing::parameters::
        erase_all_duplicates(erase_dup)
        .require_same_orientation(req_same_orientation));
  QApplication::restoreOverrideCursor();

 // CGAL::Three::Three::information(
}

CGAL::Three::Scene_item::Header_data Scene_polygon_soup_item::header() const
{
  CGAL::Three::Scene_item::Header_data data;
  //categories

  data.categories.append(std::pair<QString,int>(QString("Vertices"),1));
  data.categories.append(std::pair<QString,int>(QString("Polygons"),4));
  data.categories.append(std::pair<QString,int>(QString("Edges"),6));
  data.categories.append(std::pair<QString,int>(QString("Angles"),3));

  //titles
  data.titles.append(QString("#Points"));

  data.titles.append(QString("#Polygons"));
  data.titles.append(QString("Pure Triangle"));
  data.titles.append(QString("Pure Quad"));
  data.titles.append(QString("#Degenerate Polygons"));

  data.titles.append(QString("#Edges"));
  data.titles.append(QString("Minimum Length"));
  data.titles.append(QString("Maximum Length"));
  data.titles.append(QString("Median Length"));
  data.titles.append(QString("Mean Length"));
  data.titles.append(QString("#Degenerate Edges"));

  data.titles.append(QString("Minimum"));
  data.titles.append(QString("Maximum"));
  data.titles.append(QString("Average"));
  return data;
}

QString Scene_polygon_soup_item::computeStats(int type)
{
  if(!d->stats_computed)
    d->compute_stats();

  switch(type)
  {
  case NB_VERTICES:
    return QString::number(d->soup->points.size());
  case NB_FACETS:
    return QString::number(d->soup->polygons.size());
  case NB_EDGES:
    return QString::number(d->nb_lines/6);

  case NB_DEGENERATED_FACES:
  {
    if(d->is_triangle)
    {
      return QString::number(d->nb_degen_faces);
    }
    else
      return QString("n/a");
  }

  case MIN_LENGTH:
    return QString::number(d->minl);
  case MAX_LENGTH:
    return QString::number(d->maxl);
  case MID_LENGTH:
    return QString::number(d->midl);
  case MEAN_LENGTH:
    return QString::number(d->meanl);
  case NB_NULL_LENGTH:
    return QString::number(d->nb_null_edges);

  case MIN_ANGLE:
    return QString::number(d->mini);
  case MAX_ANGLE:
    return QString::number(d->maxi);
  case MEAN_ANGLE:
    return QString::number(d->ave);

  case IS_PURE_TRIANGLE:
    if(d->is_triangle)
      return QString("yes");
    else
      return QString("no");
  case IS_PURE_QUAD:
    if (d->is_quad)
      return QString("yes");
    else
      return QString("no");
  }
  return QString();
}

void
Scene_polygon_soup_item_priv::
invalidate_stats()
{
  is_triangle = true;
  is_quad = true;
  minl=0;
  maxl=0;
  meanl=0;
  midl=0;
  mini=0;
  maxi=0;
  ave=0;
  nb_null_edges=0;
  nb_degen_faces=0;
  stats_computed = false;
}

void
Scene_polygon_soup_item_priv::compute_stats()
{
  using namespace boost::accumulators;
  accumulator_set< double,
    features< tag::min, tag::max, tag::mean , tag::median> > edges_acc;
  accumulator_set< double,
    features< tag::min, tag::max, tag::mean > > angles_acc;
  double rad_to_deg = 180. / CGAL_PI;

  for(auto poly : soup->polygons)
  {
    if(poly.size() != 3)
      is_triangle = false;
    if(poly.size() != 4)
      is_quad = false;
    for(std::size_t i = 0; i< poly.size(); ++i)
    {
      Polygon_soup::Point_3 a(soup->points[poly[i]]),
          b(soup->points[poly[(i+1)%poly.size()]]),
          c(soup->points[poly[(i+2)%poly.size()]]);
      if (a == b)
        ++nb_null_edges;
      edges_acc(CGAL::sqrt(CGAL::squared_distance(a, b)));
      typename Traits::Vector_3 ba(b, a);
      typename Traits::Vector_3 bc(b, c);
      double cos_angle = (ba * bc)
        / std::sqrt(ba.squared_length() * bc.squared_length());
      if(cos_angle == CGAL_PI || cos_angle == 0)
        ++nb_degen_faces;
      angles_acc(std::acos(cos_angle) * rad_to_deg);
    }
  }

  minl = extract_result< tag::min >(edges_acc);
  maxl = extract_result< tag::max >(edges_acc);
  meanl = extract_result< tag::mean >(edges_acc);
  midl =  extract_result< tag::median >(edges_acc);
  mini = extract_result< tag::min >(angles_acc);
  maxi = extract_result< tag::max >(angles_acc);
  ave = extract_result< tag::mean >(angles_acc);

  stats_computed = true;
}


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_IS_CONVEX_CARTESIAN_HPP
#define BOOST_GEOMETRY_STRATEGIES_IS_CONVEX_CARTESIAN_HPP

#include <boost/geometry/strategies/convex_hull/cartesian.hpp>
#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/is_convex/services.hpp>
#include <boost/geometry/strategy/cartesian/side_by_triangle.hpp>
#include <boost/geometry/strategy/cartesian/side_robust.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace is_convex
{

template <typename CalculationType = void>
using cartesian = strategies::convex_hull::cartesian<CalculationType>;

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, cartesian_tag>
{
    using type = strategies::is_convex::cartesian<>;
};

template <typename CT>
struct strategy_converter<strategy::side::side_robust<CT>>
{
    static auto get(strategy::side::side_robust<CT> const& )
    {
        return strategies::is_convex::cartesian<CT>();
    }
};

template <typename CT>
struct strategy_converter<strategy::side::side_by_triangle<CT>>
{
    static auto get(strategy::side::side_by_triangle<CT> const& )
    {
        return strategies::is_convex::cartesian<CT>();
    }
};

} // namespace services

}} // namespace strategies::is_convex

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_IS_CONVEX_CARTESIAN_HPP


#ifndef __SIGMA_HPP__
#define __SIGMA_HPP__

#include "types.hpp"
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/fusion/iterator/next.hpp>

typedef boost::adjacency_list<multisetS, vecS, undirectedS,
                              property<vertex_name_t, int>,
                              property < edge_index_t, size_t>
                              > SigmaGraph;

typedef boost::graph_traits<SigmaGraph>::edge_descriptor ED;

struct edge_to_remove
{
  edge_to_remove() { }
  edge_to_remove(const ED& e1, const ED& e2) : m_e1(e1), m_e2(e2) { }

  template <typename Edge>
  bool operator()(const Edge& e) const {
    return e != m_e1 && e != m_e2;
  }

  ED m_e1;
  ED m_e2;
};

class SigmaFinder
{
  SigmaGraph g;

  std::vector<int> vnames;

  // pair of equal edges
  // Complicted mappings are composed from pairs of elementary mapping pairs
  std::map<size_t,size_t> eq_edges;

  // TRUE if edge has same direction with edge it mapped on
  std::map<size_t,bool>   same_dir;

  // Vertex map for shrinked edges mapping
public:
  std::map<size_t,size_t> v_shrink_to;

  // Graph containing only edges with different momenta
  std::set<size_t> no_sigma_graph;
public:
  SigmaFinder(const DiagramRecord& dr)
  {

    // We find self-energy insertions only in internal lines

    for(std::vector<Prop>::const_iterator pi = dr.props.begin(); pi != dr.props.end(); ++pi)
      {
        VD u,v;
        std::vector<int>::iterator vit = std::find(vnames.begin(), vnames.end(), pi->u);

        // One-loop tadpoles not added to graph
        // They can not contribute to edges with SE insertion
        if(pi->u != pi->v)
          {
            if(vit  != vnames.end())
              {
                u = (vit - vnames.begin());

                std::cout << "u From vec " << u <<std::endl;
              }
            else
              {
                u = add_vertex(Graph_t::vertex_property_type(pi->u), g);

                std::cout << "u To   vec " << u <<std::endl;
                vnames.push_back(pi->u);
                assert(u == vnames.size() - 1);
              }

            vit = std::find(vnames.begin(), vnames.end(), pi->v);
            if( vit != vnames.end())
              {
                v = (vit - vnames.begin());
                std::cout << "v From vec " << v <<std::endl;
              }
            else
              {
                v = add_vertex(Graph_t::vertex_property_type(pi->v), g);
                std::cout << "v To   vec " << v <<std::endl;
                vnames.push_back(pi->v);
                assert(v == vnames.size() - 1);
              }

            Edge ee = add_edge (u, v, pi->id, g).first;

            // Set to negative to mark parallel edges not included in map
            // ecm[ee] = -1;

            std::cout << "Edge " << pi->type << " added" << std::endl;
          }
      }

    // When edges added and all vertices initialized
    // prepare map of vertices connected with legs

    std::vector<VD> external_v;
    for(std::vector<Leg>::const_iterator li = dr.legs.begin(); li != dr.legs.end(); ++li)
      {

        std::vector<int>::iterator vit = std::find(vnames.begin(), vnames.end(), li->v);
        if(vit  == vnames.end())
          throw;
        else
          external_v.push_back(vit - vnames.begin());
      }

    // Now remove all possible pairs of edges
    // And check that all external vertices belong to the
    // same connected component
    graph_traits < SigmaGraph >::edge_iterator ei1, ei2, ei_begin, ei_end;
    // graph_traits < SigmaGraph >::edge_iterator ei2, ei2_end;
    tie(ei_begin, ei_end) = edges(g);
    for (ei1 = ei_begin; ei1 != ei_end; ++ei1)
      for (ei2 = ei_begin; ei2 != ei1; ++ei2)
        {
          std::cout << "Removing " << *ei1 << " and " << *ei2 << std::endl;

          // We remove only edges with vertices degree more than 1
          // and not parallel
          if(
             (degree(source(*ei1,g),g) > 1 && degree(target(*ei1,g),g) > 1) &&
             (degree(source(*ei2,g),g) > 1 && degree(target(*ei2,g),g) > 1) &&
             !(source(*ei1,g) == source(*ei2,g) && target(*ei1,g) == target(*ei2,g)) &&
             !(source(*ei1,g) == target(*ei2,g) && target(*ei1,g) == source(*ei2,g))
             )
            {
              edge_to_remove filter(*ei1,*ei2);
              filtered_graph<SigmaGraph, edge_to_remove> fg(g, filter);

              std::vector<int> component(num_vertices(fg));
              size_t con_comp = connected_components(fg, &component[0]);
              std::cout << "Coooooon " << con_comp << std::endl;

              if(con_comp == 2)
                {

                  size_t    component_external_legs;
                  bool legs_in_single_component = true;
                  for(std::vector<VD>::const_iterator evit = external_v.begin(); evit != external_v.end(); ++evit)
                    {
                      if(evit == external_v.begin())
                        component_external_legs = component[*evit];
                      else if(component_external_legs != component[*evit])
                        legs_in_single_component = false;
                    }
                  // Proceed only if all legs are in the same
                  // connected component

                  if(legs_in_single_component)
                    {

                      std::cout << "Legs in single component!!!" << std::endl;
                      // we eliminate edges with larger idx
                      // with smaller idx edges
                      size_t lhs = std::max(get(edge_index, g, *ei1), get(edge_index, g, *ei2));
                      size_t rhs = std::min(get(edge_index, g, *ei1), get(edge_index, g, *ei2));
                      eq_edges[lhs] = rhs;

                      // LHS is edge to be shrinked and we fill vertex shrink map
                      // We keep vertex with larger number to highlight removed number!!!
                      v_shrink_to[std::min(dr.props[lhs].u,dr.props[lhs].v)] = std::max(dr.props[lhs].u,dr.props[lhs].v);
                      std::cout << "Now vertex " << std::min(dr.props[lhs].u,dr.props[lhs].v) << " become " << std::max(dr.props[lhs].u,dr.props[lhs].v) << std::endl;

                      if(component[source(*ei1,g)] == component[source(*ei2,g)])
                        same_dir[lhs] = false; // oposit
                      else if(component[source(*ei1,g)] == component[target(*ei2,g)])
                        same_dir[lhs] = true;  // same
                      else throw;
                    }
                }
            }

        }

    // eq_edges can contain recursive edges substitution
    // like 3->2, 2->1 ,4->1 which is equal to (3,2,4)->1

    for(std::map<size_t,size_t>::iterator eqit = eq_edges.begin(); eqit != eq_edges.end(); ++eqit)
      {
        size_t rhs = eqit->second;
        std::map<size_t,size_t>::iterator it;
        while((it = eq_edges.find(rhs)) != eq_edges.end())
          {
            rhs = it->second;
            eqit->second = rhs;
          }

        no_sigma_graph.insert(rhs);
      }

  }

  const std::map<size_t,size_t>&  emap() const
  {
    return eq_edges;
  }

  const std::map<size_t,bool>&    codir() const
  {
    return same_dir;
  }

  const std::set<size_t>& base() const
  {
    return no_sigma_graph;
  }

  bool is_base(size_t e) const
  {
    return no_sigma_graph.count(e) == 1;
  }

};

#endif  // __SIGMA_HPP__


// Copyright <disenone>

#include "squares.hpp"

#include <immintrin.h>
#include <algorithm>
#include <utility>
#include <limits>
#include <cassert>
#include <iostream>

#include <boost/timer/timer.hpp>

namespace aoi { namespace squares {

#define IfNotInXZSquare(dx, dz, x0, z0, x1, z1, radius) \
  dx = x0 - x1; \
  dz = z0 - z1; \
  if (dx > radius  || dz > radius) \

#define IfInXZRadiusSquare(dx, dz, x0, z0, x1, z1, radius_square) \
  dx = x0 - x1; \
  dz = z0 - z1; \
  if (dx * dx + dz * dz <= radius_square) \

#define IfNotInXZRadiusSquare(dx, dz, x0, z0, x1, z1, radius_square) \
  dx = x0 - x1; \
  dz = z0 - z1; \
  if (dx * dx + dz * dz > radius_square) \

BOOST_FORCEINLINE float CalcAoiDistSquare(const Pos &a, const Pos &b) {
  float dx = a.x - b.x;
  float dz = a.z - b.z;
  return dx * dx + dz * dz;
}

SquareAoi::SquareAoi(float square_size /*= 200*/)
    : square_size_(square_size),
      inverse_square_size_(1 / square_size),
      cur_aoi_map_idx_(0) {
  player_map_.reserve(100);
  squares_.reserve(100);
}

void SquareAoi::_RemoveFromSquare(Nuid nuid, PlayerAoi* pptr) {
  if (pptr->square_index < 0) {
    return;
  }
  auto square_id = pptr->square_id;
  auto square_ptr = squares_.find(square_id);
  if (square_ptr != squares_.end()) {
    auto &square = square_ptr->second;
    auto &last = square[square.size() - 1];
    auto &remove = square[pptr->square_index];
    std::swap(remove, last);
    square.pop_back();
  }
  pptr->square_index = -1;
}

void SquareAoi::_AddToSquare(Nuid nuid, PlayerAoi* pptr) {
  auto square_id = PosToId(pptr->pos.x, pptr->pos.z, inverse_square_size_);
  auto &square = squares_[square_id];
  square.push_back(pptr);
  pptr->square_id = square_id;
  pptr->square_index = square.size() - 1;
}

void SquareAoi::AddPlayer(Nuid nuid, float x, float y, float z) {
  auto piter = player_map_.find(nuid);
  PlayerAoi* pptr = nullptr;

  if (piter != player_map_.end()) {
    _RemoveFromSquare(nuid, piter->second.get());
    pptr = piter->second.get();
    pptr->UnsetFlag_Removed();
  } else {
    auto ret = player_map_.emplace(nuid, new PlayerAoi(nuid, x, y, z));
    if (ret.second) {
      pptr = ret.first->second.get();
      pptr->SetFlag_New();
    } else {
      return;
    }
  }

  _AddToSquare(nuid, pptr);
}

void SquareAoi::RemovePlayer(Nuid nuid) {
  auto piter = player_map_.find(nuid);

  if (piter != player_map_.end()) {
    auto &player = *piter->second;
    _RemoveFromSquare(nuid, &player);
    player.SetFlag_Removed();
  }
}

void SquareAoi::AddSensor(Nuid nuid, Nuid sensor_id, float radius) {
  auto piter = player_map_.find(nuid);

  if (piter == player_map_.end())
    return;

  auto& player = *piter->second;
  for (const auto& sensor : player.sensors) {
    if (sensor.sensor_id == sensor_id)
      return;
  }
  player.sensors.emplace_back(sensor_id, radius);
}

void SquareAoi::UpdatePos(Nuid nuid, float x, float y, float z) {
  auto piter = player_map_.find(nuid);

  if (piter == player_map_.end())
    return;

  auto& player = *piter->second;
  auto new_square_id = PosToId(x, z, inverse_square_size_);
  auto old_square_id = PosToId(
    player.pos.x, player.pos.z, inverse_square_size_);

  if (old_square_id != new_square_id) {
    _RemoveFromSquare(nuid, &player);
    player.pos.Set(x, y, z);
    _AddToSquare(nuid, &player);
  } else {
    player.pos.Set(x, y, z);
  }
}

AoiUpdateInfos SquareAoi::Tick() {
  // 全量做一遍 aoi
  AoiUpdateInfos update_infos;
  PlayerPtrList remove_list;

  for (auto& elem : player_map_) {
    auto& player = *elem.second;
    if (player.GetFlag_Removed()) {
      remove_list.push_back(&player);
      continue;
    }

    if (!player.sensors.empty()) {
      auto update_info = _UpdatePlayerAoi(cur_aoi_map_idx_, &player);
      if (!update_info.sensor_update_list.empty()) {
        update_infos.emplace(update_info.nuid, std::move(update_info));
      }
    }

    player.UnsetFlag_New();
  }

  for (auto pptr : remove_list) {
    player_map_.erase(pptr->nuid);
  }
  for (auto& elem : player_map_) {
    auto& player = *elem.second;
    player.last_pos = player.pos;
  }
  cur_aoi_map_idx_ = 1 - cur_aoi_map_idx_;
  return update_infos;
}

AoiUpdateInfo SquareAoi::_UpdatePlayerAoi(Uint32 cur_aoi_map_idx,
                                          PlayerAoi* pptr) {
  AoiUpdateInfo aoi_update_info;
  aoi_update_info.nuid = pptr->nuid;
  Uint32 new_aoi_map_idx = 1 - cur_aoi_map_idx;

  for (auto& sensor : pptr->sensors) {
    auto& old_aoi = sensor.aoi_players[cur_aoi_map_idx];
    auto& new_aoi = sensor.aoi_players[new_aoi_map_idx];
    _CalcAoiPlayers(*pptr, sensor, &new_aoi);

    SensorUpdateInfo update_info;

    auto& enters = update_info.enters;
    auto& leaves = update_info.leaves;
    float radius_square = sensor.radius_square;

    _CheckLeave(pptr, radius_square, old_aoi, &leaves);
    _CheckEnter(pptr, radius_square, new_aoi, &enters);

    if (enters.empty() && leaves.empty()) {
      continue;
    }

    update_info.sensor_id = sensor.sensor_id;
    aoi_update_info.sensor_update_list.push_back(std::move(update_info));
  }

  return aoi_update_info;
}

void SquareAoi::_CalcAoiPlayers(const PlayerAoi& player, const Sensor& sensor,
                                PlayerPtrList* aoi_map) {
  Nuid player_nuid = player.nuid;
  float pos_x = player.pos.x;
  float pos_z = player.pos.z;
  float radius = sensor.radius;
  float radius_square = radius * radius;

  std::vector<SquarePlayers*> check_squares;
  size_t max_num = 0;
  _GetSquaresAndPlayerNum(player.pos, radius, &check_squares, &max_num);

  aoi_map->clear();
  aoi_map->reserve(max_num);
  assert(max_num <= player_map_.size());

  float dx, dz;

  for (auto square : check_squares) {
    for (auto other_ptr : *square) {
      if (other_ptr->nuid == player_nuid || other_ptr->GetFlag_Removed()) continue;
      IfNotInXZSquare(dx, dz, pos_x, pos_z, other_ptr->pos.x, other_ptr->pos.z, radius) continue;
      if (dx * dx + dz * dz < radius_square) {
        aoi_map->push_back(other_ptr);
      }
    }
  }
}

void SquareAoi::_CheckLeave(PlayerAoi* pptr, float radius_square,
                             const PlayerPtrList &aoi_players, PlayerNuids *leaves) {
  const auto &player_pos = pptr->pos;
  float dx, dz;
  float pos_x = player_pos.x;
  float pos_z = player_pos.z;
  for (auto old_player_ptr : aoi_players) {
    if (old_player_ptr->GetFlag_Removed()) {
      leaves->push_back(old_player_ptr->nuid);
    } else {
      IfNotInXZRadiusSquare(dx, dz, old_player_ptr->pos.x, old_player_ptr->pos.z,
                            pos_x, pos_z, radius_square) {
        leaves->push_back(old_player_ptr->nuid);
      }
    }
  }
}

void SquareAoi::_CheckEnter(PlayerAoi* pptr, float radius_square,
                             const PlayerPtrList &aoi_players, PlayerNuids *enters) {
  const auto &player_last_pos = pptr->last_pos;
  float pos_x = player_last_pos.x;
  float pos_z = player_last_pos.z;

  if (pptr->GetFlag_New()) {
    enters->reserve(aoi_players.size());
    for (auto new_player_ptr : aoi_players) {
      enters->push_back(new_player_ptr->nuid);
    }
    return;
  }

  float dx, dz;
  for (auto new_player_ptr : aoi_players) {
    IfNotInXZRadiusSquare(dx, dz, new_player_ptr->last_pos.x, new_player_ptr->last_pos.z,
                          pos_x, pos_z, radius_square) {
      enters->push_back(new_player_ptr->nuid);
    }
  }
}

}  // namespace squares

}  // namespace aoi


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

#include <boost/simd/toolbox/operator/functions/multiplies.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/dispatch/meta/upgrade.hpp>

#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/constants/digits.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::multiplies_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<double_<A0>,boost::simd::tag::sse_>))
                              ((simd_<double_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_mul_pd(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::multiplies_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                              ((simd_<single_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_mul_ps(a0,a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::multiplies_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<ints8_<A0>,boost::simd::tag::sse_>))
                              ((simd_<ints8_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0,signed>::type  type;

      type mask1 = integral_constant<type,0x00FF>();
      type a0_16 = a0();
      type a1_16 = a1();
      type al    = _mm_srli_epi16(a0_16, 8);
      type bl    = _mm_srli_epi16(a1_16, 8);
      type abh   = _mm_and_si128(mask1, _mm_mullo_epi16(a0,a1));
      type ab    = _mm_mullo_epi16(al,bl);
      type abl   = _mm_slli_epi16(_mm_and_si128(mask1, ab), 8);
      return bitwise_cast<result_type>(abh | abl);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::multiplies_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                              ((simd_<ints16_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_mullo_epi16(a0, a1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::multiplies_, boost::simd::tag::sse2_
                            , (A0)
                            , ((simd_<ints32_<A0>,boost::simd::tag::sse_>))
                              ((simd_<ints32_<A0>,boost::simd::tag::sse_>))
                            )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return _mm_or_si128(
                    _mm_and_si128(
                       _mm_mul_epu32(a0,a1),
                       _mm_setr_epi32(0xffffffff,0,0xffffffff,0)
                    )
                  , _mm_slli_si128(
                        _mm_and_si128(
                            _mm_mul_epu32( _mm_srli_si128(a0,4)
                                         , _mm_srli_si128(a1,4)
                                         )
                          , _mm_setr_epi32(0xffffffff,0,0xffffffff,0)
                        )
                      , 4
                    )
             );
    }
  };
} } }

#endif
#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_FFS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_FFS_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/firstbitset.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/function/shr.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <boost/simd/constant/ratio.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF(ffs_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::type8_<A0>, X>
                          )
   {
      using result = bd::as_integer_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        result v = bitwise_cast<result>(firstbitset(a0));
        return  bitwise_and(genmask(v)
                           ,inc(bitwise_or(
                                  bitwise_or(
                                    (-( genmask(bitwise_and(v,result(0xAA)))))
                                    , shift_left(-(genmask(bitwise_and(v, result(0xCC)))),1))
                                  , shift_left(-(genmask(bitwise_and(v, result(0xF0)))),2))));
      }
   };

   BOOST_DISPATCH_OVERLOAD_IF(ffs_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::type16_<A0>, X>
                          )
   {
      using result = bd::as_integer_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        result v = bitwise_cast<result>( firstbitset(a0));
        return  bitwise_and(genmask(v)
                           ,inc(bitwise_or(
                                  bitwise_or(
                                    bitwise_or(
                                      (-( genmask(bitwise_and(v, result(0xAAAA)))))
                                      ,shift_left(-( genmask(bitwise_and(v,result(0xCCCC)))),1))
                                    ,shift_left(-( genmask(bitwise_and(v,result(0xF0F0)))),2))
                                  ,shift_left(-( genmask(bitwise_and(v,result(0xFF00)))),3))));
      }
   };

  BOOST_DISPATCH_OVERLOAD_IF(ffs_
                         , (typename A0, typename X)
                         , (detail::is_native<X>)
                         , bd::cpu_
                         , bs::pack_<bd::type32_<A0>, X>
                         )
  {
    using result = bd::as_integer_t<A0>;
    BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
    {
      result v =  bitwise_cast<result>(firstbitset(a0));
      return  bitwise_and(genmask(v)
                         ,inc(bitwise_or(
                                bitwise_or(
                                  bitwise_or(
                                    bitwise_or(
                                      (-( genmask(bitwise_and(v,result(0xAAAAAAAA)))))
                                      ,shift_left(-(genmask(bitwise_and(v,result(0xCCCCCCCC)))),1))
                                    , shift_left(-(genmask(bitwise_and(v,result(0xF0F0F0F0)))),2))
                                  , shift_left(-(genmask(bitwise_and(v,result(0xFF00FF00)))), 3))
                                , shift_left(-(genmask(bitwise_and(v,result(0xFFFF0000)))),4))));
    }
  };

} } }

#endif


#pragma once

#include <cmath>
#include <chrono>
#include <iomanip>
#include <random>
#include <stdexcept>

#include <dtl/dtl.hpp>
#include <dtl/batchwise.hpp>
#include <dtl/math.hpp>
#include <dtl/mem.hpp>
#include <dtl/simd.hpp>

#include "block_addressing_logic.hpp"
#include "blocked_bloomfilter_block_logic_zoned.hpp"
#include "blocked_bloomfilter_logic.hpp"
#include "blocked_bloomfilter_tune.hpp"
#include "hash_family.hpp"

#include <boost/math/common_factor.hpp>

namespace dtl {

//===----------------------------------------------------------------------===//
template<typename Tw = $u32>
struct zoned_blocked_bloomfilter {

  using key_t = $u32;
  using hash_value_t = $u32;
  using word_t = Tw;

  static constexpr u1 early_out = false; // TODO make configurable and also adaptive

  // The operations used for dynamic dispatching.
  enum class op_t {
    CONSTRUCT,  // constructs the Bloom filter logic
    BIND,       // assigns the function pointers of the Bloom filter API
    DESTRUCT,   // destructs the Bloom filter logic
  };

  // The first hash function to use inside the block. Note: 0 is used for block addressing
  static constexpr u32 block_hash_fn_idx = 1;

  // The block type.
  template<u32 word_cnt, u32 zone_cnt, u32 k, u1 early_out = false>
  using bbf_block_t = multizone_block<key_t, word_t, word_cnt, zone_cnt, k,
                                      hasher, hash_value_t,
                                      block_hash_fn_idx, 0, zone_cnt,
                                      early_out>;

  template<u32 word_cnt, u32 zone_cnt, u32 k, dtl::block_addressing addr = dtl::block_addressing::POWER_OF_TWO, u1 early_out = false>
  using bbf = dtl::blocked_bloomfilter_logic<key_t, hasher, bbf_block_t<word_cnt, zone_cnt, k, early_out>, addr, early_out>;

  //===----------------------------------------------------------------------===//
  // Members
  //===----------------------------------------------------------------------===//
  /// The (desired) bit length of the Bloom filter.
  $u64 m;
  /// The (actual) bit length of the Bloom filter.
  $u64 m_actual;
  /// The number of bits set per entry.
  $u32 k;
  /// The number of words per block.
  $u32 word_cnt_per_block;
  /// The number of zones per block.
  $u32 zone_cnt;
  /// Pointer to the Bloom filter logic instance.
  void* instance = nullptr;
  /// A container for the (hardware dependent) tuning parameters.
  const blocked_bloomfilter_tune& tune;
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // The API functions. (Function pointers to the actual implementation.)
  //===----------------------------------------------------------------------===//
  std::function<void(__restrict word_t* /*filter data*/, const key_t /*key*/)>
  insert;

  std::function<void(__restrict word_t* /*filter data*/, const key_t* /*keys*/, u32 /*key_cnt*/)>
  batch_insert;

  std::function<$u1(const __restrict word_t* /*filter data*/, const key_t /*key*/)>
  contains;

  std::function<$u64(const __restrict word_t* /*filter data*/,
                     const key_t* /*keys*/, u32 /*key_cnt*/,
                     $u32* /*match_positions*/, u32 /*match_offset*/)>
  batch_contains;
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  zoned_blocked_bloomfilter(const std::size_t m, u32 k, u32 word_cnt_per_block, u32 zone_cnt,
                            const blocked_bloomfilter_tune& tune)
      : m(m), k(k), word_cnt_per_block(word_cnt_per_block), zone_cnt(zone_cnt), tune(tune) {

    if (word_cnt_per_block <= zone_cnt) {
      throw std::invalid_argument("Invalid configuration: w=" + std::to_string(word_cnt_per_block)
                                      + ", z=" + std::to_string(zone_cnt));
    }

    // Construct the Bloom filter logic instance.
    dispatch(*this, op_t::CONSTRUCT);

    // Bind the API functions.
    dispatch(*this, op_t::BIND);

    // Check whether the constructed filter matches the given arguments.
    if (this->k != k
        || this->word_cnt_per_block != word_cnt_per_block) {
      dispatch(*this, op_t::DESTRUCT);
      throw std::invalid_argument("Invalid configuration: k=" + std::to_string(k)
                                  + ", w=" + std::to_string(word_cnt_per_block)
                                  + ", z=" + std::to_string(zone_cnt));
    }
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  zoned_blocked_bloomfilter(zoned_blocked_bloomfilter&& src)
      : m(src.m), m_actual(src.m_actual), k(src.k),
        word_cnt_per_block(src.word_cnt_per_block), zone_cnt(src.zone_cnt),
        instance(src.instance),
        insert(std::move(src.insert)),
        batch_insert(std::move(src.batch_insert)),
        contains(std::move(src.contains)),
        batch_contains(std::move(src.batch_contains)),
        tune(std::move(src.tune)) {
    // Invalidate pointer in src
    src.instance = nullptr;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  ~zoned_blocked_bloomfilter() {
    // Destruct logic instance (if any).
    if (instance != nullptr) dispatch(*this, op_t::DESTRUCT);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  zoned_blocked_bloomfilter&
  operator=(zoned_blocked_bloomfilter&& src) {
    m = src.m;
    m_actual = src.m_actual;
    k = src.k;
    word_cnt_per_block = src.word_cnt_per_block;
    zone_cnt = src.zone_cnt;
    instance = src.instance;
    insert = std::move(src.insert);
    batch_insert = std::move(src.batch_insert);
    contains = std::move(src.contains);
    batch_contains = std::move(src.batch_contains);
    // invalidate pointers
    src.instance = nullptr;
    return *this;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  // Dynamic Dispatching
  //===----------------------------------------------------------------------===//
  //TODO make private
  static void
  dispatch(zoned_blocked_bloomfilter& instance, op_t op) {
      switch (instance.word_cnt_per_block) {
        case  4: _z< 4>(instance, op); break;
        case  8: _z< 8>(instance, op); break;
        case 16: _z<16>(instance, op); break;
        default:
          throw std::invalid_argument("The given 'word_cnt_per_block' is not supported.");
      }
  };

  template<u32 w>
  static void
  _z(zoned_blocked_bloomfilter& instance, op_t op) {
    switch (instance.zone_cnt) {
      case  2: _k<w, boost::static_unsigned_min<w/2, 2>::value>(instance, op); break;
      case  4: _k<w, boost::static_unsigned_min<w/2, 4>::value>(instance, op); break;
      case  8: _k<w, boost::static_unsigned_min<w/2, 8>::value>(instance, op); break;
      default:
        throw std::invalid_argument("The given 'zone_cnt' is not supported.");
    }
  }

  template<u32 w, u32 z>
  static void
  _k(zoned_blocked_bloomfilter& instance, op_t op) {
    switch (instance.k) {
#ifndef FAST_BUILD
      case  2: _a<w, z, boost::static_unsigned_max<( 2 % z == 0 ?  2 : 2 /*invalid*/), z>::value>(instance, op); break;
      case  4: _a<w, z, boost::static_unsigned_max<( 4 % z == 0 ?  4 : 2 /*invalid*/), z>::value>(instance, op); break;
      case  6: _a<w, z, boost::static_unsigned_max<( 6 % z == 0 ?  6 : 2 /*invalid*/), z>::value>(instance, op); break;
#endif
      case  8: _a<w, z, boost::static_unsigned_max<( 8 % z == 0 ?  8 : 2 /*invalid*/), z>::value>(instance, op); break;
      case 10: _a<w, z, boost::static_unsigned_max<(10 % z == 0 ? 10 : 2 /*invalid*/), z>::value>(instance, op); break;
      case 12: _a<w, z, boost::static_unsigned_max<(12 % z == 0 ? 12 : 2 /*invalid*/), z>::value>(instance, op); break;
      case 14: _a<w, z, boost::static_unsigned_max<(14 % z == 0 ? 14 : 2 /*invalid*/), z>::value>(instance, op); break;
      case 16: _a<w, z, boost::static_unsigned_max<16 , z>::value>(instance, op); break;
      default:
        throw std::invalid_argument("The given 'k' is not supported.");
    }
  }

  template<u32 w, u32 z, u32 k>
  static void
  _a(zoned_blocked_bloomfilter& instance, op_t op) {
    dtl::block_addressing addr = dtl::is_power_of_two(instance.m)
                                 ? dtl::block_addressing::POWER_OF_TWO
                                 : dtl::block_addressing::MAGIC;
    switch (addr) {
      case dtl::block_addressing::POWER_OF_TWO: _u<w, z, k, dtl::block_addressing::POWER_OF_TWO>(instance, op); break;
      case dtl::block_addressing::MAGIC:        _u<w, z, k, dtl::block_addressing::MAGIC>(instance, op);        break;
      case dtl::block_addressing::DYNAMIC:      /* must not happen */                                           break;
    }
  }

  template<u32 w, u32 z, u32 k, dtl::block_addressing a>
  static void
  _u(zoned_blocked_bloomfilter& instance, op_t op) {
    blocked_bloomfilter_config c;
    c.k = k;
    c.word_size = sizeof(word_t);
    c.word_cnt_per_block = w;
    c.sector_cnt = w;
    c.zone_cnt = z;
    c.addr_mode = a;

    switch (instance.tune.get_unroll_factor(c)) {
      case  0: _o<w, z, k, a, 0>(instance, op); break;
      case  1: _o<w, z, k, a, 1>(instance, op); break;
      case  2: _o<w, z, k, a, 2>(instance, op); break;
      case  4: _o<w, z, k, a, 4>(instance, op); break;
#ifndef FAST_BUILD
      case  8: _o<w, z, k, a, 8>(instance, op); break;
#endif
      default:
        throw std::invalid_argument("The given 'unroll_factor' is not supported.");
    }
  }

  template<u32 w, u32 z, u32 k, dtl::block_addressing a, u32 unroll_factor>
  static void
  _o(zoned_blocked_bloomfilter& instance, op_t op) {
    using _t = bbf<w, z, k, a, early_out>;
    switch (op) {
      case op_t::CONSTRUCT: instance._construct_logic<_t>();           break;
      case op_t::BIND:      instance._bind_logic<_t, unroll_factor>(); break;
      case op_t::DESTRUCT:  instance._destruct_logic<_t>();            break;
    }
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Constructs a blocked Bloom filter logic instance.
  template<
      typename bf_t
  >
  void
  _construct_logic() {
    // Instantiate a Bloom filter logic.
    bf_t* bf = new bf_t(m);
    instance = bf;
    k = bf_t::k;
    word_cnt_per_block = bf_t::word_cnt_per_block;
//    zone_cnt = bf_t::zone_cnt;

    // Get the actual size of the filter.
    m_actual = bf->get_length();
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Bind API functions to the (previously constructed) Bloom filter logic.
  template<
      typename bf_t,
      u32 unroll_factor = 1
  >
  void
  _bind_logic() {
    using namespace std::placeholders;
    auto* bf = static_cast<bf_t*>(instance);

    // Bind the API functions.
    insert = std::bind(&bf_t::insert, bf, _1, _2);
    batch_insert = std::bind(&bf_t::batch_insert, bf, _1, _2, _3);
    contains = std::bind(&bf_t::contains, bf, _1, _2);

    // SIMD vector length (0 = run scalar code)
    // TODO fix unrolling for word_t = u64
    static constexpr u64 vector_len = dtl::simd::lane_count<key_t> * unroll_factor;
    batch_contains = std::bind(&bf_t::template batch_contains<vector_len>, bf, _1, _2, _3, _4, _5);
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Destructs the Bloom filter logic.
  template<typename bf_t>
  void
  _destruct_logic() {
    bf_t* bf = static_cast<bf_t*>(instance);
    delete bf;
    instance = nullptr;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the block addressing mode.
  block_addressing get_addressing_mode() const {
    return dtl::is_power_of_two(m)
           ? block_addressing::POWER_OF_TWO
           : block_addressing::MAGIC;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the (actual) size in bytes.
  std::size_t
  size_in_bytes() const noexcept {
    return (m_actual + 7) / 8;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the (total) number of words.
  std::size_t
  size() const noexcept {
    constexpr u32 word_bitlength = sizeof(word_t) * 8;
    return (m_actual + (word_bitlength - 1)) / word_bitlength;
  }
  //===----------------------------------------------------------------------===//

  //===----------------------------------------------------------------------===//
  /// Returns the name of the Bloom filter instance including the most
  /// important parameters (in JSON).
  std::string
  name() {
    blocked_bloomfilter_config c;
    c.k = k;
    c.word_size = sizeof(word_t);
    c.word_cnt_per_block = word_cnt_per_block;
    c.sector_cnt = word_cnt_per_block;
    c.zone_cnt = zone_cnt;
    c.addr_mode = get_addressing_mode();

    return "{\"name\":\"blocked_bloom_multiword\",\"size\":" + std::to_string(size_in_bytes())
         + ",\"word_size\":" + std::to_string(sizeof(word_t))
         + ",\"k\":" + std::to_string(k)
         + ",\"w\":" + std::to_string(word_cnt_per_block)
         + ",\"s\":" + std::to_string(word_cnt_per_block)
         + ",\"z\":" + std::to_string(zone_cnt)
         + ",\"u\":" + std::to_string(tune.get_unroll_factor(c))
         + ",\"e\":" + std::to_string(early_out ? 1 : 0)
         + ",\"addr\":" + (get_addressing_mode() == dtl::block_addressing::POWER_OF_TWO ? "\"pow2\"" : "\"magic\"")
         + "}";
  }
  //===----------------------------------------------------------------------===//

};

} // namespace dtl


#include <algorithm>
#include <cstdlib>
#include <iostream>
#include <numeric>
#include <string>

#include <boost/program_options.hpp>

#include "datastructs/matrix.h"
#include "datastructs/equations.h"

#include "polynomials/commutative_polynomial.h"
#include "polynomials/non_commutative_polynomial.h"

#include "semirings/commutativeRExp.h"
#include "semirings/float-semiring.h"
#include "semirings/prec-rat-semiring.h"
#include "semirings/tropical-semiring.h"
#include "semirings/pseudo_linear_set.h"
#include "semirings/semilinear_set.h"
#include "semirings/bool-semiring.h"
#include "semirings/why-set.h"
#include "semirings/viterbi-semiring.h"
#include "semirings/maxmin-semiring.h"

#ifdef USE_LIBFA
#include "semirings/lossy-finite-automaton.h"
#include "polynomials/lossy_non_commutative_polynomial.h"
#endif

#ifdef USE_GENEPI
#include "semirings/semilinSetNdd.h"
#endif

#include "parser.h"

#include "solvers/newton_generic.h"
#include "solvers/kleene_seminaive.h"
#include "solvers/solver_utils.h"

#include "utils/string_util.h"

template <typename SR, template <typename> class Poly>
ValuationMap<SR> call_solver(const std::string solver_name,  const GenericEquations<Poly, SR> &equations,
   const bool scc, const bool iteration_flag, const std::size_t iterations, const bool graphviz_output){
  if(0 == solver_name.compare("newtonSymb")) {
    std::cout << "Solver: Newton Symbolic" << std::endl;
    return apply_solver<Newton, Poly>(equations, scc, iteration_flag, iterations, graphviz_output);
  }
  else if(0 == solver_name.compare("newtonConc")) {
    std::cout << "Solver: Newton Concrete"<< std::endl;
    return apply_solver<NewtonCL, Poly>(equations, scc, iteration_flag, iterations, graphviz_output);
  }
  else if(0 == solver_name.compare("newtonCLDU")) {
    std::cout << "Solver: Newton Concrete (LDU)"<< std::endl;
    return apply_solver<NewtonCLDU, Poly>(equations, scc, iteration_flag, iterations, graphviz_output);
  }
  else if(0 == solver_name.compare("newtonSLDU")) {
      std::cout << "Solver: Newton Symbolic (LDU)"<< std::endl;
      return apply_solver<NewtonSLDU, Poly>(equations, scc, iteration_flag, iterations, graphviz_output);
  }
  else if(0 == solver_name.compare("kleene")) {
    std::cout << "Solver: Kleene solver"<< std::endl;
    return apply_solver<KleeneComm, Poly>(equations, scc, iteration_flag, iterations, graphviz_output);
  }
  else {
    // default-case
    std::cout << "Solver: Newton Concrete (LDU)"<< std::endl;
    return apply_solver<NewtonCLDU, Poly>(equations, scc, iteration_flag, iterations, graphviz_output);
  }
}

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

  namespace po = boost::program_options;

  po::options_description desc("Allowed options");
  desc.add_options()
    ( "scc", "apply newton method iteratively to strongly connected components of the equation graph" )
    ( "help,h", "print this help message" )
    ( "iterations,i", po::value<int>(), "specify the number of newton iterations. Default is number of equations + 1." )
    //( "verbose", "enable verbose output" )
    //( "debug", "enable debug output" )
    ( "test", "just for testing purposes ... explicit test defined in main()" )
    ( "file,f", po::value<std::string>(), "input file" )
    ( "float", "float semiring" )
    ( "rat", "semiring of extended rationals (arbitrary precision)" )
    ( "bool", "boolean semiring" )
    ( "why", "why semiring" )
    ( "maxmin", "MaxMin semiring" )
    ( "viterbi", "Viterbi semiring" )
    ( "tropical", "tropical semiring over the integers" )
    ( "rexp", "commutative regular expression semiring" )
    ( "slset", "explicit semilinear sets semiring, no simplification " )
#ifdef USE_GENEPI
    ( "slsetndd", po::value<std::string>(), "ndd semilinear sets semiring, give plugin name (lash-msdf, mona)" )
    ( "n", po::value<int>(), "number of variables, used for slsetndd" )
#endif
    ( "mlset", "abstraction over semilinear sets" )
    ( "vec-simpl", "vector simplification only (only semilinear and multilinear sets)" )
    ( "lin-simpl", "linear set simplification (only semilinear sets)" )
    ( "free", "free semiring" )
    ( "lossy", "lossy semiring" )
    ( "prefix", po::value<int>(), "prefix semiring with given length")
    ( "graphviz", "create the file graph.dot with the equation graph (NOTE: currently only with option --scc) " )
    ( "solver,s", po::value<std::string>(), "solver type (currently: \"newtonSymb\", \"newtonConc\", \"newtonCLDU\", \"newtonSLDU\", \"newtonNumeric\" (only for numeric semirings), or \"kleene\")" )
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("test")) {
    Newton<SemilinSetExp> newton;
    std::vector<VarId> variables;
    variables.push_back(Var::GetVarId("x"));
    std::cout << "- newton (cnt-SR):" << std::endl;

    std::vector<CommutativePolynomial<SemilinSetExp> > polynomials;
    CommutativePolynomial<SemilinSetExp> f1 = CommutativePolynomial<SemilinSetExp>({
        { SemilinSetExp(Var::GetVarId("a")), CommutativeMonomial{ {Var::GetVarId("x"),Var::GetVarId("x")} } },
        { SemilinSetExp(Var::GetVarId("c")), CommutativeMonomial{} } });

    polynomials.push_back(f1);

    Matrix<SemilinSetExp> result = newton.solve_fixpoint(polynomials, variables, 2);
    std::cout << result << std::endl;

    /*              auto s1 = CommutativeRExp(Var::GetVarId("a"));
                    auto s2 = CommutativeRExp(Var::GetVarId("b"));
                    auto m1 = Matrix<CommutativeRExp>(1,1,{s1});
                    auto m2 = Matrix<CommutativeRExp>(1,1,{s2});
                    */

    // this actually led to a strange bug with the ublas-matrix implementation!!
    /*              auto s1 = SemilinSetExp(Var::GetVarId("a"));
                    auto s2 = SemilinSetExp(Var::GetVarId("b"));
                    auto m1 = Matrix<SemilinSetExp>(1,1,{s1});
                    auto m2 = Matrix<SemilinSetExp>(1,1,{s2});

                    auto m3 = m1*m2;
                    std::cout << m3;
                    */
    return 0;
  }

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

#ifdef USE_GENEPI
  SemilinSetNdd::genepi_init();
#endif

  int iterations=0;
  if (vm.count("iterations"))
          iterations = vm["iterations"].as<int>();

  // check if we can do something useful
  if (!vm.count("float") &&
      !vm.count("rat") &&
      !vm.count("rexp") &&
      !vm.count("slset") &&
#ifdef USE_GENEPI
      !vm.count("slsetndd") &&
#endif
      !vm.count("free") &&
      !vm.count("bool") &&
      !vm.count("why") &&
      !vm.count("tropical") &&
      !vm.count("mlset") &&
      !vm.count("prefix") &&
      !vm.count("lossy") &&
      !vm.count("maxmin") &&
      !vm.count("viterbi"))
    {
    std::cout << "Please supply a supported semiring :)" << std::endl;
    return 0;
  }

  std::vector<std::string> input;
  std::string line;
  if (vm.count("file")) {
    // we are reading the input from the given file
    std::ifstream file;
    file.open(vm["file"].as<std::string>(), std::ifstream::in);
    if (file.fail()) {
      std::cerr << "Could not open input file: " << vm["file"].as<std::string>() << std::endl;
    }
    while (std::getline(file, line)) {
      input.push_back(line);
    }
  } else {
    // we are reading from stdin
    while (std::getline(std::cin, line)) {
      input.push_back(line);
    }
  }

  // join the input into one string
  std::string input_all =
    std::accumulate(input.begin(), input.end(), std::string(""));

  Parser p;
  std::string solver_name;

  if(vm.count("solver")) {
    solver_name = vm["solver"].as<std::string>();
  }
  else {
    solver_name = "newtonConc"; //seems to be the fastest
  }

  const auto iter_flag = vm.count("iterations");
  const auto graph_flag = vm.count("graphviz");
  const auto scc_flag = vm.count("scc");

  if (vm.count("slset")) {
    auto equations_raw = p.free_parser(input_all);
    auto equations = MakeCommEquationsAndMap(equations_raw, [](const FreeSemiring &c) -> SemilinSetExp {
      auto srconv = SRConverter<SemilinSetExp>();
      return c.Eval(srconv);
    });

    if (equations.empty()) return EXIT_FAILURE;
    //PrintEquations(equations);
    if (!vm.count("vec-simpl") && !vm.count("lin-simpl")) {
      DMSG("A");
      std::cout << result_string(
          call_solver(solver_name, equations, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;

    } else if (vm.count("vec-simpl") && !vm.count("lin-simpl")) {
      DMSG("B");
      auto equations2 = MapEquations(equations, [](const SemilinearSet<> &s) {
        return SemilinearSetV{s};
      });
      std::cout << result_string(
          call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    } else {
      DMSG("C");
      auto equations2 = MapEquations(equations, [](const SemilinearSet<> &s) {
        return SemilinearSetL{s};
      });
      std::cout << result_string(
          call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    }
#ifdef USE_GENEPI
  } else if (vm.count("slsetndd")) {
      SemilinSetNdd::solver_init(vm["n"].as<int>());
      auto equations = p.slsetndd_parser(input_all);
      if (equations.empty()) return EXIT_FAILURE;
      std::cout << result_string(
          call_solver(solver_name, equations, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
      SemilinSetNdd::solver_dealloc();
#endif
  } else if (vm.count("mlset")) {

    auto equations_raw = p.free_parser(input_all);
    auto equations = MakeCommEquationsAndMap(equations_raw, [](const FreeSemiring &c) -> SemilinSetExp {
      auto srconv = SRConverter<SemilinSetExp>();
      return c.Eval(srconv);
    });

    if (equations.empty()) return EXIT_FAILURE;
    if (vm.count("vec-simpl")) {
      auto m_equations = SemilinearToPseudoLinearEquations<
        DummyDivider, SparseVecSimplifier>(equations);
      //PrintEquations(m_equations);
      std::cout << result_string(
          call_solver(solver_name, equations, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    } else {
      auto m_equations = SemilinearToPseudoLinearEquations<
        DummyDivider, DummyVecSimplifier>(equations);
      //PrintEquations(m_equations);
      std::cout << result_string(
          call_solver(solver_name, equations, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    }

  } else if (vm.count("rexp")) {

    // parse the input into a list of (Var → Polynomial[SR])
    auto equations = p.rexp_parser(input_all);
    if (equations.empty()) return EXIT_FAILURE;

    //PrintEquations(equations);

    // apply solver to the equations
    std::cout << result_string(
        call_solver(solver_name, equations, scc_flag, iter_flag, iterations, graph_flag)
        ) << std::endl;

  } else if (vm.count("free")) {

    // parse the input into a list of (Var → Polynomial[SR])
	auto equations = p.free_parser(input_all);
  //auto equations2 = MakeCommEquations(equations);
  //if (equations2.empty()) return EXIT_FAILURE;

//    std::cout << result_string(
//        call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
//        ) << std::endl;
#ifdef USE_LIBFA
  } else if (vm.count("lossy")) {

    auto equations = p.free_parser(input_all);
    if (equations.empty()) return EXIT_FAILURE;

    //PrintEquations(equations);
    auto equations2 = MapEquations(equations, [](const FreeSemiring &c) -> LossyFiniteAutomaton {
      auto srconv = SRConverter<LossyFiniteAutomaton>();
      return c.Eval(srconv);
    });

    VarId S_1 = equations2[0].first;

    NCEquationsBase<LossyFiniteAutomaton> eq2 = NCEquationsBase<LossyFiniteAutomaton>(equations2.begin(), equations2.end());

    auto approximation = NonCommutativePolynomial<LossyFiniteAutomaton>::downwardClosureCourcelle(eq2, S_1);
    std::cout << "dwc:\t" << approximation.string() << std::endl;
    std::cout << "size NFA for DWC:\t" << approximation.size() << std::endl;
    std::cout << "size minimal DFA for DWC:\t" << approximation.minimize().size() << std::endl;
#endif
  } else if (vm.count("prefix")) {

    // parse the input into a list of (Var → Polynomial[SR])
    auto equations = p.prefix_parser(input_all, vm["prefix"].as<int>());
    if (equations.empty()) return EXIT_FAILURE;

    //PrintEquations(equations);

    // apply the newton method to the equations
    //auto result = apply_solver<Kleene, PrefixSemiring>(equations,
    //                                           vm.count("scc"),
    //                                           vm.count("iterations"),
    //                                           iterations,
    //                                           vm.count("graphviz"));
    //std::cout << result_string(result) << std::endl;

    //std::cout << result_string(
    //    call_solver(solver_name, equations, scc_flag, iter_flag, iterations, graph_flag)
    //    ) << std::endl;

  } else if (vm.count("float")) {
    auto equations = p.free_parser(input_all);
    //PrintEquations(equations);
    auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> FloatSemiring {
      auto srconv = SRConverter<FloatSemiring>();
      return c.Eval(srconv);
    });

    if (equations2.empty()) return EXIT_FAILURE;

    //PrintEquations(equations);
    //PrintEquations(equations2);

    if(0 == solver_name.compare("newtonNumeric")) {
      std::cout << "Solver: Newton Numeric (Float)"<< std::endl;
      std::cout << result_string(
          apply_solver<NewtonNumeric>(equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    } else  {
      std::cout << result_string(
          call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    }

  } else if (vm.count("bool")) {
    auto equations = p.free_parser(input_all);
    auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> BoolSemiring {
      auto srconv = SRConverter<BoolSemiring>();
      return c.Eval(srconv);
    });

    if (equations2.empty()) return EXIT_FAILURE;

    PrintEquations(equations);
    PrintEquations(equations2);
      std::cout << result_string(
          call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;

  } else if (vm.count("rat")) {
    auto equations = p.free_parser(input_all);
    auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> PrecRatSemiring {
      auto srconv = SRConverter<PrecRatSemiring>();
      return c.Eval(srconv);
    });

    if (equations2.empty()) return EXIT_FAILURE;

    //PrintEquations(equations);
    //PrintEquations(equations2);

    if(0 == solver_name.compare("newtonNumeric")) {
      std::cout << "Solver: Newton Numeric (Rat)"<< std::endl;
      std::cout << result_string(
          apply_solver<NewtonNumeric>(equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    } else  {
      std::cout << result_string(
          call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
          ) << std::endl;
    }
  }
  else if (vm.count("why")) {
      auto equations = p.free_parser(input_all);
      auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> WhySemiring {
        auto srconv = SRConverter<WhySemiring>();
        return c.Eval(srconv);
      });

      if (equations2.empty()) return EXIT_FAILURE;

        std::cout << result_string(
            call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
            ) << std::endl;

    }
    else if (vm.count("tropical")) {
      auto equations = p.free_parser(input_all);
      auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> TropicalSemiring {
        auto srconv = SRConverter<TropicalSemiring>();
        return c.Eval(srconv);
      });

      if (equations2.empty()) return EXIT_FAILURE;

        std::cout << result_string(
            call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
            ) << std::endl;

    }
    else if (vm.count("viterbi")) {
          auto equations = p.free_parser(input_all);
          auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> ViterbiSemiring {
            auto srconv = SRConverter<ViterbiSemiring>();
            return c.Eval(srconv);
          });

          if (equations2.empty()) return EXIT_FAILURE;

            std::cout << result_string(
                call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
                ) << std::endl;
    }
    else if (vm.count("maxmin")) {
          auto equations = p.free_parser(input_all);
          auto equations2 = MakeCommEquationsAndMap(equations, [](const FreeSemiring &c) -> MaxMinSemiring {
            auto srconv = SRConverter<MaxMinSemiring>();
            return c.Eval(srconv);
          });

          if (equations2.empty()) return EXIT_FAILURE;

            std::cout << result_string(
                call_solver(solver_name, equations2, scc_flag, iter_flag, iterations, graph_flag)
                ) << std::endl;

    }

#ifdef USE_GENEPI
  SemilinSetNdd::genepi_dealloc();
#endif

  return EXIT_SUCCESS;
}


// Copyright (c) 2019, Torsten Sattler
// 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 copyright holder nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> 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: Torsten Sattler, torsten.sattler.de@googlemail.com

#include <iostream>

#include <Eigen/StdVector>

#include "generalized_calibrated_absolute_pose_estimator.h"

namespace ransac_lib {

namespace generalized_calibrated_absolute_pose {

GeneralizedCalibratedAbsolutePoseEstimator::
    GeneralizedCalibratedAbsolutePoseEstimator(
        const MultiCameraRig& rig, const double squared_inlier_threshold,
        const Points2D& points2D, const ViewingRays& rays,
        const Points3D& points3D, const std::vector<int>& camera_indices,
        const CameraPositions& positions, const CameraRotations& rotations)
    : rig_(rig),
      squared_inlier_threshold_(squared_inlier_threshold),
      points2D_(points2D),
      points3D_(points3D),
      camera_indices_(camera_indices),
      adapter_(rays, camera_indices, points3D, positions, rotations) {
  num_data_ = static_cast<int>(points2D_.size());

  rig_.global_pose.topLeftCorner<3, 3>() = Matrix3d::Identity();
  rig_.global_pose.col(3) = Vector3d::Zero();

  num_cameras_ = static_cast<int>(rig.cameras.size());
  for (int i = 0; i < num_cameras_; ++i) {
    rig_.cameras[i].pose.topLeftCorner<3, 3>() = rotations[i].transpose();
    rig_.cameras[i].pose.col(3) = positions[i];
  }
}

int GeneralizedCalibratedAbsolutePoseEstimator::MinimalSolver(
    const std::vector<int>& sample, MultiCameraRigs* poses) const {
  poses->clear();
  CameraPoses gp3p_poses = opengv::absolute_pose::gp3p(adapter_, sample);
  if (gp3p_poses.empty()) return 0;
  for (const CameraPose& pose : gp3p_poses) {
    MultiCameraRig rig;
    AssembleRig(pose, &rig);
    const double kError = EvaluateModelOnPoint(rig, sample[3]);
    if (kError < squared_inlier_threshold_) {
      poses->push_back(rig);
      // At most one pose should be correct.
      break;
    }
  }

  return static_cast<int>(poses->size());
}

// Returns 0 if no model could be estimated and 1 otherwise.
// Implemented by a simple linear least squares solver.
int GeneralizedCalibratedAbsolutePoseEstimator::NonMinimalSolver(
    const std::vector<int>& sample, MultiCameraRig* pose) const {
  CameraPose P = opengv::absolute_pose::gpnp(adapter_, sample);
  AssembleRig(P, pose);
  return 1;
}

double GeneralizedCalibratedAbsolutePoseEstimator::EvaluateModelOnPoint(
    const MultiCameraRig& pose, int i) const {
  // Transforms into the coordinate system of the rig.
  Vector3d p_r =
      pose.global_pose.topLeftCorner<3, 3>() *
          (points3D_[i] - pose.global_pose.col(3));

  // Transforms into the coordinate system of the camera.
  const Camera& cam = rig_.cameras[camera_indices_[i]];
  Vector3d p_c = cam.pose.topLeftCorner<3, 3>() * (p_r - cam.pose.col(3));

  // Check whether point projects behind the camera.
  if (p_c[2] < 0.0) return std::numeric_limits<double>::max();

  Eigen::Vector2d p_2d = p_c.head<2>() / p_c[2];
  p_2d[0] *= cam.focal_x;
  p_2d[1] *= cam.focal_y;

  return (p_2d - points2D_[i]).squaredNorm();
}

void GeneralizedCalibratedAbsolutePoseEstimator::LeastSquares(
    const std::vector<int>& sample, MultiCameraRig* pose) const {
  // OpenGV returns the transformation from the camera to the world coordinate
  // system. We store the rotation from the world to the local coordinate
  // system instead.
  Eigen::Matrix3d R = pose->global_pose.topLeftCorner<3, 3>().transpose();
  Eigen::Vector3d c = pose->global_pose.col(3);
  // At the moment, we need to copy data as we need to add the current pose
  // estimate to the adapter, which would break the requirement that this
  // function is constant.
  const int kSampleSize = static_cast<int>(sample.size());
  opengv::bearingVectors_t bearing_vectors(kSampleSize);
  opengv::points_t points(kSampleSize);
  std::vector<int> camera_indices(kSampleSize);
  for (int i = 0; i < kSampleSize; ++i) {
    const int kIdx = sample[i];
    bearing_vectors[i] = adapter_.getBearingVector(kIdx);
    points[i] = adapter_.getPoint(kIdx);
    camera_indices[i] = camera_indices_[i];
  }

  opengv::translations_t camera_positions(num_cameras_);
  opengv::rotations_t camera_rotations(num_cameras_);
  for (int i = 0; i < num_cameras_; ++i) {
    camera_positions[i] = rig_.cameras[i].pose.col(3);
    camera_rotations[i] =
        rig_.cameras[i].pose.topLeftCorner<3, 3>().transpose();
  }

  opengv::absolute_pose::NoncentralAbsoluteAdapter lsq_adapter(
      bearing_vectors, camera_indices, points, camera_positions,
      camera_rotations, c, R);

  CameraPose P = opengv::absolute_pose::optimize_nonlinear(lsq_adapter);
  AssembleRig(P, pose);
}

void GeneralizedCalibratedAbsolutePoseEstimator::AssembleRig(
    const CameraPose& pose, MultiCameraRig* rig) const {
  *rig = rig_;
  rig->global_pose.topLeftCorner<3, 3>() =
      pose.topLeftCorner<3, 3>().transpose();
  rig->global_pose.col(3) = pose.col(3);
}

}  // namespace generalized_calibrated_absolute_pose

}  // namespace ransac_lib


#pragma once

#include <Eigen/Dense>

#include "OscProbPMNS.hh"
#include "Neutrino.hh"

namespace GNA {
  namespace GNAObjectTemplates {
    template<typename FloatType>
    class OscillationVariablesT;

    template<typename FloatType>
    class PMNSVariablesT;
  }
}
using OscillationVariables = GNA::GNAObjectTemplates::OscillationVariablesT<double>;
using PMNSVariables = GNA::GNAObjectTemplates::PMNSVariablesT<double>;

class OscProbPMNSDecoh: public OscProbPMNSBase,
                        public TransformationBind<OscProbPMNSDecoh> {
public:
  using TransformationBind<OscProbPMNSDecoh>::transformation_;
  OscProbPMNSDecoh(Neutrino from, Neutrino to);
  void calcSum(FunctionArgs fargs);
  template <int I, int J>
  void calcComponent(FunctionArgs fargs);
  template <int I, int J>
  void calcComponentCP(FunctionArgs fargs);
protected:
  variable<double> m_L;
  variable<double> m_sigma;
};


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

#define BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS
#define BOOST_MPL_LIMIT_VECTOR_SIZE 50

#include <tabula/grids.hpp>             // basic_rectangle, chequered_rectangle
#include <tabula/type_traits.hpp>       // square_t
#include <boost/mpl/vector.hpp>         // vector
#include <boost/test/unit_test.hpp>     // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_SUITE_END, BOOST_AUTO_TEST_CASE_TEMPLATE, BOOST_CHECK, BOOST_CHECK_EQUAL

using namespace tabula;

BOOST_AUTO_TEST_SUITE(Compass)

using grid_types = boost::mpl::vector
<       basic_rectangle<1, 1>
,       basic_rectangle<1, 2>
,       basic_rectangle<2, 1>
,       basic_rectangle<2, 2>
,       basic_rectangle<2, 3>
,       basic_rectangle<3, 2>
,       basic_rectangle<2, 4>
,       basic_rectangle<4, 2>
,       basic_rectangle<3, 3>
,       basic_rectangle<3, 5>
,       basic_rectangle<5, 3>
,       chequered_rectangle<1, 1, 0>
,       chequered_rectangle<1, 2, 0>
,       chequered_rectangle<2, 1, 0>
,       chequered_rectangle<2, 2, 0>
,       chequered_rectangle<2, 3, 0>
,       chequered_rectangle<3, 2, 0>
,       chequered_rectangle<2, 4, 0>
,       chequered_rectangle<4, 2, 0>
,       chequered_rectangle<3, 3, 0>
,       chequered_rectangle<3, 5, 0>
,       chequered_rectangle<5, 3, 0>
,       chequered_rectangle<1, 1, 1>
,       chequered_rectangle<1, 2, 1>
,       chequered_rectangle<2, 1, 1>
,       chequered_rectangle<2, 2, 1>
,       chequered_rectangle<2, 3, 1>
,       chequered_rectangle<3, 2, 1>
,       chequered_rectangle<2, 4, 1>
,       chequered_rectangle<4, 2, 1>
,       chequered_rectangle<3, 3, 1>
,       chequered_rectangle<3, 5, 1>
,       chequered_rectangle<5, 3, 1>
>;

BOOST_AUTO_TEST_CASE_TEMPLATE(IndexIsInvertible, Grid, grid_types)
{
        for (auto i = 0; i < Grid::size; ++i) {
                auto const s = Grid::square(i);
                BOOST_CHECK_EQUAL(i, Grid::index(s));
        }
}

BOOST_AUTO_TEST_CASE_TEMPLATE(SquareIsInvertible, Grid, grid_types)
{
        for (auto r = 0; r < Grid::height; ++r) {
                for (auto f = 0; f < Grid::width; ++f) {
                        if (auto const s = square_t<Grid>(f, r); s.is_valid()) {
                                auto const i = Grid::index(s);
                                BOOST_CHECK(s == Grid::square(i));
                        }
                }
        }
}

BOOST_AUTO_TEST_SUITE_END()