#ifndef BOOST_GEOMETRY_PROJECTIONS_OB_TRAN_HPP
#define BOOST_GEOMETRY_PROJECTIONS_OB_TRAN_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

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

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

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

// Last updated version of proj: 4.9.1

// Original copyright notice:

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

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

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

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

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

namespace boost { namespace geometry { namespace projections
{

    template <typename Geographic, typename Cartesian, typename Parameters> class factory;

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace ob_tran
    {

            static const double TOL = 1e-10;

            template <typename Geographic, typename Cartesian>
            struct par_ob_tran
            {
                boost::shared_ptr<projection<Geographic, Cartesian> > link;
                double    lamp;
                double    cphip, sphip;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_ob_tran_oblique : public base_t_fi<base_ob_tran_oblique<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_ob_tran<Geographic, Cartesian> m_proj_parm;

                inline base_ob_tran_oblique(const Parameters& par)
                    : base_t_fi<base_ob_tran_oblique<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(o_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double coslam, sinphi, cosphi;

                    coslam = cos(lp_lon);
                    sinphi = sin(lp_lat);
                    cosphi = cos(lp_lat);
                    lp_lon = adjlon(aatan2(cosphi * sin(lp_lon), this->m_proj_parm.sphip * cosphi * coslam +
                        this->m_proj_parm.cphip * sinphi) + this->m_proj_parm.lamp);
                    lp_lat = aasin(this->m_proj_parm.sphip * sinphi - this->m_proj_parm.cphip * cosphi * coslam);
                    m_proj_parm.link->fwd(lp_lon, lp_lat, xy_x, xy_y);
                }

                // INVERSE(o_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    double coslam, sinphi, cosphi;

                    m_proj_parm.link->inv(xy_x, xy_y, lp_lon, lp_lat);
                    if (lp_lon != HUGE_VAL) {
                        coslam = cos(lp_lon -= this->m_proj_parm.lamp);
                        sinphi = sin(lp_lat);
                        cosphi = cos(lp_lat);
                        lp_lat = aasin(this->m_proj_parm.sphip * sinphi + this->m_proj_parm.cphip * cosphi * coslam);
                        lp_lon = aatan2(cosphi * sin(lp_lon), this->m_proj_parm.sphip * cosphi * coslam -
                            this->m_proj_parm.cphip * sinphi);
                    }
                }

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

            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_ob_tran_transverse : public base_t_fi<base_ob_tran_transverse<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_ob_tran<Geographic, Cartesian> m_proj_parm;

                inline base_ob_tran_transverse(const Parameters& par)
                    : base_t_fi<base_ob_tran_transverse<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(t_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double cosphi, coslam;

                    cosphi = cos(lp_lat);
                    coslam = cos(lp_lon);
                    lp_lon = adjlon(aatan2(cosphi * sin(lp_lon), sin(lp_lat)) + this->m_proj_parm.lamp);
                    lp_lat = aasin(- cosphi * coslam);
                    m_proj_parm.link->fwd(lp_lon, lp_lat, xy_x, xy_y);
                }

                // INVERSE(t_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    double cosphi, t;

                    m_proj_parm.link->inv(xy_x, xy_y, lp_lon, lp_lat);
                    if (lp_lon != HUGE_VAL) {
                        cosphi = cos(lp_lat);
                        t = lp_lon - this->m_proj_parm.lamp;
                        lp_lon = aatan2(cosphi * sin(t), - sin(lp_lat));
                        lp_lat = aasin(cosphi * cos(t));
                    }
                }

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

            };

            // General Oblique Transformation
            template <typename Geographic, typename Cartesian, typename Parameters>
            double setup_ob_tran(Parameters& par, par_ob_tran<Geographic, Cartesian>& proj_parm, bool create = true)
            {
                double phip;
                Parameters pj;

                /* get name of projection to be translated */
                pj.name = pj_param(par.params, "so_proj").s;
                /* copy existing header into new */
                par.es = 0.; /* force to spherical */
                pj.params = par.params;
                pj.over = par.over;
                pj.geoc = par.geoc;
                pj.a = par.a;
                pj.es = par.es;
                pj.ra = par.ra;
                pj.lam0 = par.lam0;
                pj.phi0 = par.phi0;
                pj.x0 = par.x0;
                pj.y0 = par.y0;
                pj.k0 = par.k0;
                /* force spherical earth */
                pj.one_es = pj.rone_es = 1.;
                pj.es = pj.e = 0.;

                if (create)
                {
                    factory<Geographic, Cartesian, Parameters> fac;
                    proj_parm.link.reset(fac.create_new(pj));
                    if (! proj_parm.link.get()) throw proj_exception(-26);
                }
                if (pj_param(par.params, "to_alpha").i) {
                    double lamc, phic, alpha;

                    lamc    = pj_param(par.params, "ro_lon_c").f;
                    phic    = pj_param(par.params, "ro_lat_c").f;
                    alpha    = pj_param(par.params, "ro_alpha").f;
            /*
                    if (fabs(phic) <= TOL ||
                        fabs(fabs(phic) - geometry::math::half_pi<double>()) <= TOL ||
                        fabs(fabs(alpha) - geometry::math::half_pi<double>()) <= TOL)
            */
                    if (fabs(fabs(phic) - geometry::math::half_pi<double>()) <= TOL)
                        throw proj_exception(-32);
                    proj_parm.lamp = lamc + aatan2(-cos(alpha), -sin(alpha) * sin(phic));
                    phip = aasin(cos(phic) * sin(alpha));
                } else if (pj_param(par.params, "to_lat_p").i) { /* specified new pole */
                    proj_parm.lamp = pj_param(par.params, "ro_lon_p").f;
                    phip = pj_param(par.params, "ro_lat_p").f;
                } else { /* specified new "equator" points */
                    double lam1, lam2, phi1, phi2, con;

                    lam1 = pj_param(par.params, "ro_lon_1").f;
                    phi1 = pj_param(par.params, "ro_lat_1").f;
                    lam2 = pj_param(par.params, "ro_lon_2").f;
                    phi2 = pj_param(par.params, "ro_lat_2").f;
                    if (fabs(phi1 - phi2) <= TOL ||
                        (con = fabs(phi1)) <= TOL ||
                        fabs(con - geometry::math::half_pi<double>()) <= TOL ||
                        fabs(fabs(phi2) - geometry::math::half_pi<double>()) <= TOL) throw proj_exception(-33);
                    proj_parm.lamp = atan2(cos(phi1) * sin(phi2) * cos(lam1) -
                        sin(phi1) * cos(phi2) * cos(lam2),
                        sin(phi1) * cos(phi2) * sin(lam2) -
                        cos(phi1) * sin(phi2) * sin(lam1));
                    phip = atan(-cos(proj_parm.lamp - lam1) / tan(phi1));
                }
                if (fabs(phip) > TOL) { /* oblique */
                    proj_parm.cphip = cos(phip);
                    proj_parm.sphip = sin(phip);
                } else { /* transverse */
                }
                // return phip to choose model
                return phip;
            }

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

    /*!
        \brief General Oblique Transformation projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Projection parameters
         - o_proj (string)
         - Plus projection parameters
         - o_lat_p (degrees)
         - o_lon_p (degrees)
         - New pole
         - o_alpha: Alpha (degrees)
         - o_lon_c (degrees)
         - o_lat_c (degrees)
         - o_lon_1 (degrees)
         - o_lat_1: Latitude of first standard parallel (degrees)
         - o_lon_2 (degrees)
         - o_lat_2: Latitude of second standard parallel (degrees)
        \par Example
        \image html ex_ob_tran.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct ob_tran_oblique : public detail::ob_tran::base_ob_tran_oblique<Geographic, Cartesian, Parameters>
    {
        inline ob_tran_oblique(const Parameters& par) : detail::ob_tran::base_ob_tran_oblique<Geographic, Cartesian, Parameters>(par)
        {
            detail::ob_tran::setup_ob_tran(this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief General Oblique Transformation projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
        \par Projection parameters
         - o_proj (string)
         - Plus projection parameters
         - o_lat_p (degrees)
         - o_lon_p (degrees)
         - New pole
         - o_alpha: Alpha (degrees)
         - o_lon_c (degrees)
         - o_lat_c (degrees)
         - o_lon_1 (degrees)
         - o_lat_1: Latitude of first standard parallel (degrees)
         - o_lon_2 (degrees)
         - o_lat_2: Latitude of second standard parallel (degrees)
        \par Example
        \image html ex_ob_tran.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct ob_tran_transverse : public detail::ob_tran::base_ob_tran_transverse<Geographic, Cartesian, Parameters>
    {
        inline ob_tran_transverse(const Parameters& par) : detail::ob_tran::base_ob_tran_transverse<Geographic, Cartesian, Parameters>(par)
        {
            detail::ob_tran::setup_ob_tran(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class ob_tran_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    detail::ob_tran::par_ob_tran<Geographic, Cartesian> proj_parm;
                    Parameters p = par;
                    double phip = setup_ob_tran(p, proj_parm, false);

                    if (fabs(phip) > detail::ob_tran::TOL)
                        return new base_v_fi<ob_tran_oblique<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                    else
                        return new base_v_fi<ob_tran_transverse<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void ob_tran_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("ob_tran", new ob_tran_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

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

#endif // BOOST_GEOMETRY_PROJECTIONS_OB_TRAN_HPP


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  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 Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Ioan Sucan */

#include "geometric_shapes/mesh_operations.h"
#include "geometric_shapes/shape_operations.h"

#include <cstdio>
#include <cmath>
#include <algorithm>
#include <set>
#include <float.h>

#include <console_bridge/console.h>
#include <resource_retriever/retriever.h>

#include <assimp/scene.h>
#include <assimp/Importer.hpp>
#include <assimp/postprocess.h>

#include <Eigen/Geometry>

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

namespace shapes
{
namespace detail
{
namespace
{
/// Local representation of a vertex that knows its position in an array (used for sorting)
struct LocalVertexType
{
  LocalVertexType() : x(0.0), y(0.0), z(0.0)
  {
  }

  LocalVertexType(const Eigen::Vector3d& v) : x(v.x()), y(v.y()), z(v.z())
  {
  }

  double x, y, z;
  unsigned int index;
};

/// Sorting operator by point value
struct ltLocalVertexValue
{
  bool operator()(const LocalVertexType& p1, const LocalVertexType& p2) const
  {
    if (p1.x < p2.x)
      return true;
    if (p1.x > p2.x)
      return false;
    if (p1.y < p2.y)
      return true;
    if (p1.y > p2.y)
      return false;
    if (p1.z < p2.z)
      return true;
    return false;
  }
};

/// Sorting operator by point index
struct ltLocalVertexIndex
{
  bool operator()(const LocalVertexType& p1, const LocalVertexType& p2) const
  {
    return p1.index < p2.index;
  }
};
}  // namespace
}  // namespace detail

Mesh* createMeshFromVertices(const EigenSTL::vector_Vector3d& vertices, const std::vector<unsigned int>& triangles)
{
  unsigned int nt = triangles.size() / 3;
  Mesh* mesh = new Mesh(vertices.size(), nt);
  for (unsigned int i = 0; i < vertices.size(); ++i)
  {
    mesh->vertices[3 * i] = vertices[i].x();
    mesh->vertices[3 * i + 1] = vertices[i].y();
    mesh->vertices[3 * i + 2] = vertices[i].z();
  }

  std::copy(triangles.begin(), triangles.end(), mesh->triangles);
  mesh->computeTriangleNormals();
  mesh->computeVertexNormals();

  return mesh;
}

Mesh* createMeshFromVertices(const EigenSTL::vector_Vector3d& source)
{
  if (source.size() < 3)
    return nullptr;

  if (source.size() % 3 != 0)
    CONSOLE_BRIDGE_logError("The number of vertices to construct a mesh from is not divisible by 3. Probably "
                            "constructed triangles will not make sense.");

  std::set<detail::LocalVertexType, detail::ltLocalVertexValue> vertices;
  std::vector<unsigned int> triangles;

  unsigned int n = source.size() / 3;
  for (unsigned int i = 0; i < n; ++i)
  {
    // check if we have new vertices
    unsigned int i3 = i * 3;
    detail::LocalVertexType vt1(source[i3]);
    std::set<detail::LocalVertexType, detail::ltLocalVertexValue>::iterator p1 = vertices.find(vt1);
    if (p1 == vertices.end())
    {
      vt1.index = vertices.size();
      vertices.insert(vt1);
    }
    else
      vt1.index = p1->index;
    triangles.push_back(vt1.index);

    detail::LocalVertexType vt2(source[++i3]);
    std::set<detail::LocalVertexType, detail::ltLocalVertexValue>::iterator p2 = vertices.find(vt2);
    if (p2 == vertices.end())
    {
      vt2.index = vertices.size();
      vertices.insert(vt2);
    }
    else
      vt2.index = p2->index;
    triangles.push_back(vt2.index);

    detail::LocalVertexType vt3(source[++i3]);
    std::set<detail::LocalVertexType, detail::ltLocalVertexValue>::iterator p3 = vertices.find(vt3);
    if (p3 == vertices.end())
    {
      vt3.index = vertices.size();
      vertices.insert(vt3);
    }
    else
      vt3.index = p3->index;

    triangles.push_back(vt3.index);
  }

  // sort our vertices
  std::vector<detail::LocalVertexType> vt;
  vt.insert(vt.end(), vertices.begin(), vertices.end());
  std::sort(vt.begin(), vt.end(), detail::ltLocalVertexIndex());

  // copy the data to a mesh structure
  unsigned int nt = triangles.size() / 3;

  Mesh* mesh = new Mesh(vt.size(), nt);
  for (unsigned int i = 0; i < vt.size(); ++i)
  {
    unsigned int i3 = i * 3;
    mesh->vertices[i3] = vt[i].x;
    mesh->vertices[i3 + 1] = vt[i].y;
    mesh->vertices[i3 + 2] = vt[i].z;
  }

  std::copy(triangles.begin(), triangles.end(), mesh->triangles);
  mesh->computeTriangleNormals();
  mesh->computeVertexNormals();

  return mesh;
}

Mesh* createMeshFromResource(const std::string& resource)
{
  static const Eigen::Vector3d one(1.0, 1.0, 1.0);
  return createMeshFromResource(resource, one);
}

Mesh* createMeshFromBinary(const char* buffer, std::size_t size, const std::string& assimp_hint)
{
  static const Eigen::Vector3d one(1.0, 1.0, 1.0);
  return createMeshFromBinary(buffer, size, one, assimp_hint);
}

Mesh* createMeshFromBinary(const char* buffer, std::size_t size, const Eigen::Vector3d& scale,
                           const std::string& assimp_hint)
{
  if (!buffer || size < 1)
  {
    CONSOLE_BRIDGE_logWarn("Cannot construct mesh from empty binary buffer");
    return nullptr;
  }

  // try to get a file extension
  std::string hint;
  std::size_t pos = assimp_hint.find_last_of('.');
  if (pos != std::string::npos)
  {
    hint = assimp_hint.substr(pos + 1);
    std::transform(hint.begin(), hint.end(), hint.begin(), ::tolower);
  }
  if (hint.empty())
    hint = assimp_hint;  // send entire file name as hint if no extension was found

  // Create an instance of the Importer class
  Assimp::Importer importer;

  // Issue #38 fix: as part of the post-processing, we remove all other components in file but
  // the meshes, as anyway the resulting shapes:Mesh object just receives vertices and triangles.
  importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS, aiComponent_NORMALS | aiComponent_TANGENTS_AND_BITANGENTS |
                                                          aiComponent_COLORS | aiComponent_TEXCOORDS |
                                                          aiComponent_BONEWEIGHTS | aiComponent_ANIMATIONS |
                                                          aiComponent_TEXTURES | aiComponent_LIGHTS |
                                                          aiComponent_CAMERAS | aiComponent_MATERIALS);

  // And have it read the given file with some post-processing
  const aiScene* scene = importer.ReadFileFromMemory(reinterpret_cast<const void*>(buffer), size,
                                                     aiProcess_Triangulate | aiProcess_JoinIdenticalVertices |
                                                         aiProcess_SortByPType | aiProcess_RemoveComponent,
                                                     hint.c_str());
  if (!scene)
    return nullptr;

  // Assimp enforces Y_UP convention by rotating models with different conventions.
  // However, that behaviour is confusing and doesn't match the ROS convention
  // where the Z axis is pointing up.
  // Hopefully this doesn't undo legit use of the root node transformation...
  // Note that this is also what RViz does internally.
  scene->mRootNode->mTransformation = aiMatrix4x4();

  // These post processing steps flatten the root node transformation into child nodes,
  // so they must be delayed until after clearing the root node transform above.
  importer.ApplyPostProcessing(aiProcess_OptimizeMeshes | aiProcess_OptimizeGraph);

  return createMeshFromAsset(scene, scale, hint);
}

Mesh* createMeshFromResource(const std::string& resource, const Eigen::Vector3d& scale)
{
  resource_retriever::Retriever retriever;
  resource_retriever::MemoryResource res;
  try
  {
    res = retriever.get(resource);
  }
  catch (resource_retriever::Exception& e)
  {
    CONSOLE_BRIDGE_logError("%s", e.what());
    return nullptr;
  }

  if (res.size == 0)
  {
    CONSOLE_BRIDGE_logWarn("Retrieved empty mesh for resource '%s'", resource.c_str());
    return nullptr;
  }

  Mesh* m = createMeshFromBinary(reinterpret_cast<const char*>(res.data.get()), res.size, scale, resource);
  if (!m)
  {
    CONSOLE_BRIDGE_logWarn("Assimp reports no scene in %s.", resource.c_str());
    CONSOLE_BRIDGE_logWarn("This could be because of a resource filename that is too long for the Assimp library, a "
                           "known bug. As a workaround shorten the filename/path.");
  }
  return m;
}

namespace
{
void extractMeshData(const aiScene* scene, const aiNode* node, const aiMatrix4x4& parent_transform,
                     const Eigen::Vector3d& scale, EigenSTL::vector_Vector3d& vertices,
                     std::vector<unsigned int>& triangles)
{
  aiMatrix4x4 transform = parent_transform;
  transform *= node->mTransformation;
  for (unsigned int j = 0; j < node->mNumMeshes; ++j)
  {
    const aiMesh* a = scene->mMeshes[node->mMeshes[j]];
    unsigned int offset = vertices.size();
    for (unsigned int i = 0; i < a->mNumVertices; ++i)
    {
      aiVector3D v = transform * a->mVertices[i];
      vertices.push_back(Eigen::Vector3d(v.x * scale.x(), v.y * scale.y(), v.z * scale.z()));
    }
    for (unsigned int i = 0; i < a->mNumFaces; ++i)
      if (a->mFaces[i].mNumIndices == 3)
      {
        triangles.push_back(offset + a->mFaces[i].mIndices[0]);
        triangles.push_back(offset + a->mFaces[i].mIndices[1]);
        triangles.push_back(offset + a->mFaces[i].mIndices[2]);
      }
  }

  for (unsigned int n = 0; n < node->mNumChildren; ++n)
    extractMeshData(scene, node->mChildren[n], transform, scale, vertices, triangles);
}
}  // namespace

Mesh* createMeshFromAsset(const aiScene* scene, const std::string& resource_name)
{
  static const Eigen::Vector3d one(1.0, 1.0, 1.0);
  return createMeshFromAsset(scene, one, resource_name);
}

Mesh* createMeshFromAsset(const aiScene* scene, const Eigen::Vector3d& scale, const std::string& resource_name)
{
  if (!scene->HasMeshes())
  {
    CONSOLE_BRIDGE_logWarn("Assimp reports scene in %s has no meshes", resource_name.c_str());
    return nullptr;
  }
  EigenSTL::vector_Vector3d vertices;
  std::vector<unsigned int> triangles;
  extractMeshData(scene, scene->mRootNode, aiMatrix4x4(), scale, vertices, triangles);
  if (vertices.empty())
  {
    CONSOLE_BRIDGE_logWarn("There are no vertices in the scene %s", resource_name.c_str());
    return nullptr;
  }
  if (triangles.empty())
  {
    CONSOLE_BRIDGE_logWarn("There are no triangles in the scene %s", resource_name.c_str());
    return nullptr;
  }

  return createMeshFromVertices(vertices, triangles);
}

Mesh* createMeshFromShape(const Shape* shape)
{
  if (shape->type == shapes::SPHERE)
    return shapes::createMeshFromShape(static_cast<const shapes::Sphere&>(*shape));
  else if (shape->type == shapes::BOX)
    return shapes::createMeshFromShape(static_cast<const shapes::Box&>(*shape));
  else if (shape->type == shapes::CYLINDER)
    return shapes::createMeshFromShape(static_cast<const shapes::Cylinder&>(*shape));
  else if (shape->type == shapes::CONE)
    return shapes::createMeshFromShape(static_cast<const shapes::Cone&>(*shape));
  else
    CONSOLE_BRIDGE_logError("Conversion of shape of type '%s' to a mesh is not known", shapeStringName(shape).c_str());
  return nullptr;
}

Mesh* createMeshFromShape(const Box& box)
{
  double x = box.size[0] / 2.0;
  double y = box.size[1] / 2.0;
  double z = box.size[2] / 2.0;

  // define vertices of box mesh
  Mesh* result = new Mesh(8, 12);
  result->vertices[0] = -x;
  result->vertices[1] = -y;
  result->vertices[2] = -z;

  result->vertices[3] = x;
  result->vertices[4] = -y;
  result->vertices[5] = -z;

  result->vertices[6] = x;
  result->vertices[7] = -y;
  result->vertices[8] = z;

  result->vertices[9] = -x;
  result->vertices[10] = -y;
  result->vertices[11] = z;

  result->vertices[12] = -x;
  result->vertices[13] = y;
  result->vertices[14] = z;

  result->vertices[15] = -x;
  result->vertices[16] = y;
  result->vertices[17] = -z;

  result->vertices[18] = x;
  result->vertices[19] = y;
  result->vertices[20] = z;

  result->vertices[21] = x;
  result->vertices[22] = y;
  result->vertices[23] = -z;

  static const unsigned int tri[] = { 0, 1, 2, 2, 3, 0, 4, 3, 2, 2, 6, 4, 7, 6, 2, 2, 1, 7,
                                      3, 4, 5, 5, 0, 3, 0, 5, 7, 7, 1, 0, 7, 5, 4, 4, 6, 7 };
  memcpy(result->triangles, tri, sizeof(unsigned int) * 36);
  result->computeTriangleNormals();
  result->computeVertexNormals();
  return result;
}

Mesh* createMeshFromShape(const Sphere& sphere)
{
  // this code is adapted from FCL
  EigenSTL::vector_Vector3d vertices;
  std::vector<unsigned int> triangles;

  const double r = sphere.radius;
  const double pi = boost::math::constants::pi<double>();
  const unsigned int seg = std::max<unsigned int>(6, 0.5 + 2.0 * pi * r / 0.01);  // split the sphere longitudinally up
                                                                                // to a resolution of 1 cm at the
                                                                                // ecuator, or a minimum of 6 segments
  const unsigned int ring = std::max<unsigned int>(6, 2.0 * r / 0.01);  // split the sphere into rings along latitude,
                                                                        // up to a height of at most 1 cm, or a minimum
                                                                        // of 6 rings

  double phi, phid;
  phid = pi * 2.0 / seg;
  phi = 0.0;

  double theta, thetad;
  thetad = pi / (ring + 1);
  theta = 0;

  for (unsigned int i = 0; i < ring; ++i)
  {
    double theta_ = theta + thetad * (i + 1);
    for (unsigned int j = 0; j < seg; ++j)
      vertices.push_back(Eigen::Vector3d(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid),
                                         r * cos(theta_)));
  }
  vertices.push_back(Eigen::Vector3d(0.0, 0.0, r));
  vertices.push_back(Eigen::Vector3d(0.0, 0.0, -r));

  for (unsigned int i = 0; i < ring - 1; ++i)
  {
    for (unsigned int j = 0; j < seg; ++j)
    {
      unsigned int a, b, c, d;
      a = i * seg + j;
      b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
      c = (i + 1) * seg + j;
      d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
      triangles.push_back(a);
      triangles.push_back(c);
      triangles.push_back(b);
      triangles.push_back(b);
      triangles.push_back(c);
      triangles.push_back(d);
    }
  }

  for (unsigned int j = 0; j < seg; ++j)
  {
    unsigned int a, b;
    a = j;
    b = (j == seg - 1) ? 0 : (j + 1);
    triangles.push_back(ring * seg);
    triangles.push_back(a);
    triangles.push_back(b);

    a = (ring - 1) * seg + j;
    b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
    triangles.push_back(a);
    triangles.push_back(ring * seg + 1);
    triangles.push_back(b);
  }
  return createMeshFromVertices(vertices, triangles);
}

Mesh* createMeshFromShape(const Cylinder& cylinder)
{
  // this code is adapted from FCL
  EigenSTL::vector_Vector3d vertices;
  std::vector<unsigned int> triangles;

  // magic number defining how many triangles to construct for the unit cylinder; perhaps this should be a param
  static unsigned int tot_for_unit_cylinder = 100;

  double r = cylinder.radius;
  double h = cylinder.length;

  const double pi = boost::math::constants::pi<double>();
  unsigned int tot = std::max<unsigned int>(6, ceil(tot_for_unit_cylinder * r));
  double phid = pi * 2 / tot;

  double circle_edge = phid * r;
  unsigned int h_num = ceil(std::abs(h) / circle_edge);

  double phi = 0;
  double hd = h / h_num;

  for (unsigned int i = 0; i < tot; ++i)
    vertices.push_back(Eigen::Vector3d(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2));

  for (unsigned int i = 0; i < h_num - 1; ++i)
    for (unsigned int j = 0; j < tot; ++j)
      vertices.push_back(Eigen::Vector3d(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));

  for (unsigned int i = 0; i < tot; ++i)
    vertices.push_back(Eigen::Vector3d(r * cos(phi + phid * i), r * sin(phi + phid * i), -h / 2));

  vertices.push_back(Eigen::Vector3d(0, 0, h / 2));
  vertices.push_back(Eigen::Vector3d(0, 0, -h / 2));

  for (unsigned int i = 0; i < tot; ++i)
  {
    triangles.push_back((h_num + 1) * tot);
    triangles.push_back(i);
    triangles.push_back((i == tot - 1) ? 0 : (i + 1));
  }

  for (unsigned int i = 0; i < tot; ++i)
  {
    triangles.push_back((h_num + 1) * tot + 1);
    triangles.push_back(h_num * tot + ((i == tot - 1) ? 0 : (i + 1)));
    triangles.push_back(h_num * tot + i);
  }

  for (unsigned int i = 0; i < h_num; ++i)
  {
    for (unsigned int j = 0; j < tot; ++j)
    {
      int a, b, c, d;
      a = j;
      b = (j == tot - 1) ? 0 : (j + 1);
      c = j + tot;
      d = (j == tot - 1) ? tot : (j + 1 + tot);

      int start = i * tot;
      triangles.push_back(start + b);
      triangles.push_back(start + a);
      triangles.push_back(start + c);
      triangles.push_back(start + b);
      triangles.push_back(start + c);
      triangles.push_back(start + d);
    }
  }
  return createMeshFromVertices(vertices, triangles);
}

Mesh* createMeshFromShape(const Cone& cone)
{
  // this code is adapted from FCL
  EigenSTL::vector_Vector3d vertices;
  std::vector<unsigned int> triangles;

  // magic number defining how many triangles to construct for the unit cylinder; perhaps this should be a param
  static unsigned int tot_for_unit_cone = 100;

  double r = cone.radius;
  double h = cone.length;

  const double pi = boost::math::constants::pi<double>();
  unsigned int tot = tot_for_unit_cone * r;
  double phid = pi * 2 / tot;

  double circle_edge = phid * r;
  unsigned int h_num = ceil(h / circle_edge);

  double phi = 0;
  double hd = h / h_num;

  for (unsigned int i = 0; i < h_num - 1; ++i)
  {
    double h_i = h / 2 - (i + 1) * hd;
    double rh = r * (0.5 - h_i / h);
    for (unsigned int j = 0; j < tot; ++j)
      vertices.push_back(Eigen::Vector3d(rh * cos(phi + phid * j), rh * sin(phi + phid * j), h_i));
  }

  for (unsigned int i = 0; i < tot; ++i)
    vertices.push_back(Eigen::Vector3d(r * cos(phi + phid * i), r * sin(phi + phid * i), -h / 2));

  vertices.push_back(Eigen::Vector3d(0, 0, h / 2));
  vertices.push_back(Eigen::Vector3d(0, 0, -h / 2));

  for (unsigned int i = 0; i < tot; ++i)
  {
    triangles.push_back(h_num * tot);
    triangles.push_back(i);
    triangles.push_back((i == tot - 1) ? 0 : (i + 1));
  }

  for (unsigned int i = 0; i < tot; ++i)
  {
    triangles.push_back(h_num * tot + 1);
    triangles.push_back((h_num - 1) * tot + ((i == tot - 1) ? 0 : (i + 1)));
    triangles.push_back((h_num - 1) * tot + i);
  }

  for (unsigned int i = 0; i < h_num - 1; ++i)
    for (unsigned int j = 0; j < tot; ++j)
    {
      int a, b, c, d;
      a = j;
      b = (j == tot - 1) ? 0 : (j + 1);
      c = j + tot;
      d = (j == tot - 1) ? tot : (j + 1 + tot);

      int start = i * tot;
      triangles.push_back(start + b);
      triangles.push_back(start + a);
      triangles.push_back(start + c);
      triangles.push_back(start + b);
      triangles.push_back(start + c);
      triangles.push_back(start + d);
    }
  return createMeshFromVertices(vertices, triangles);
}

namespace
{
inline void writeFloatToSTL(char*& ptr, const float data)
{
  memcpy(ptr, &data, sizeof(float));
  ptr += sizeof(float);
}
}  // namespace

void writeSTLBinary(const Mesh* mesh, std::vector<char>& buffer)
{
  buffer.resize(84 + mesh->triangle_count * 50);
  memset(&buffer[0], 0, 80);
  char* ptr = &buffer[80];
  uint32_t nt = mesh->triangle_count;
  memcpy(ptr, &nt, sizeof(uint32_t));
  ptr += sizeof(uint32_t);
  for (unsigned int i = 0; i < mesh->triangle_count; ++i)
  {
    unsigned int i3 = i * 3;

    if (mesh->triangle_normals)
    {
      writeFloatToSTL(ptr, mesh->triangle_normals[i3]);
      writeFloatToSTL(ptr, mesh->triangle_normals[i3 + 1]);
      writeFloatToSTL(ptr, mesh->triangle_normals[i3 + 2]);
    }
    else
    {
      memset(ptr, 0, sizeof(float) * 3);
      ptr += sizeof(float) * 3;
    }

    unsigned int index = mesh->triangles[i3] * 3;
    writeFloatToSTL(ptr, mesh->vertices[index]);
    writeFloatToSTL(ptr, mesh->vertices[index + 1]);
    writeFloatToSTL(ptr, mesh->vertices[index + 2]);
    index = mesh->triangles[i3 + 1] * 3;
    writeFloatToSTL(ptr, mesh->vertices[index]);
    writeFloatToSTL(ptr, mesh->vertices[index + 1]);
    writeFloatToSTL(ptr, mesh->vertices[index + 2]);
    index = mesh->triangles[i3 + 2] * 3;
    writeFloatToSTL(ptr, mesh->vertices[index]);
    writeFloatToSTL(ptr, mesh->vertices[index + 1]);
    writeFloatToSTL(ptr, mesh->vertices[index + 2]);
    memset(ptr, 0, 2);
    ptr += 2;
  }
}
}  // namespace shapes


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     isovist_orientation_gateway_generator.cpp
 * \author   Collin Johnson
 *
 * Definition of IsovistOrientationGatewayGenerator.
 */

#include "hssh/local_topological/area_detection/gateways/isovist_orientation_gateway_generator.h"
#include "hssh/local_topological/area_detection/gateways/endpoint_validator.h"
#include "hssh/local_topological/area_detection/gateways/gateway_utils.h"
#include "hssh/local_topological/area_detection/gateways/isovist_gradients.h"
#include "hssh/local_topological/area_detection/gateways/isovist_maxima.h"
#include "hssh/local_topological/area_detection/local_topo_isovist_field.h"
#include "hssh/local_topological/area_detection/voronoi/voronoi_edges.h"
#include "hssh/types.h"
#include "utils/algorithm_ext.h"
#include "utils/ray_tracing.h"
#include <boost/range/algorithm_ext.hpp>
#include <boost/range/as_array.hpp>
#include <boost/range/iterator_range.hpp>

namespace vulcan
{
namespace hssh
{

double isovist_orientation(const utils::Isovist& isovist);
bool is_shorter_gateway(const Gateway& lhs, const Gateway& rhs, cell_t maximumCell);
bool is_shorter_gateway(const Line<int>& lhs, const Line<int>& rhs, cell_t lhsCell, cell_t rhsCell, cell_t maximumCell);
bool is_better_gateway(const Gateway& lhs,
                       const Gateway& rhs,
                       cell_t lhsCell,
                       cell_t rhsCell,
                       position_value_t maximumValue);

IsovistOrientationGatewayGenerator::IsovistOrientationGatewayGenerator(
  const isovist_orientation_gateway_generator_params_t& params)
: nextGatewayId_(0)
, params_(params)
{
}

std::vector<WeightedGateway>
  IsovistOrientationGatewayGenerator::generateGateways(const std::vector<WeightedGateway>& priorGateways,
                                                       const VoronoiIsovistField& isovists,
                                                       const VoronoiSkeletonGrid& grid,
                                                       const EndpointValidator& validator)
{
    VoronoiEdges edges(grid, SKELETON_CELL_REDUCED_SKELETON);
    VoronoiIsovistGradients gradients(edges);
    gradients.calculateGradients(utils::Isovist::kShapeEccentricity, isovists);

    VoronoiIsovistMaxima maxima(gradients, edges, grid, params_.numAboveMean, params_.saveGradientData);

    gradients_ = &gradients;
    isovists_ = &isovists;
    grid_ = &grid;
    validator_ = &validator;
    sourceToSkeleton_ = extract_source_cells(grid, SKELETON_CELL_REDUCED_SKELETON);

    // Create new gateways for every new maximum
    for (auto& maximum : maxima) {
        createGatewaysForMaximum(maximum);
    }

    return gatewayHypotheses_;
}

void IsovistOrientationGatewayGenerator::createGatewaysForMaximum(const isovist_local_maximum_t& maximum)
{
    extractEdgeCellsForMaximum(maximum);

    const double kAngleSearchRange = M_PI / 4.0;   // search +/- 45 degrees from where the minimum is

    std::vector<WeightedGateway> possibleGateways;
    for (auto& c : maximumCells_) {
        // Skip cells that don't have a gradient because their weight will be invalid
        if ((c.value == 0.0) || !isovists_->contains(c.position)) {
            continue;
        }

        double isoOrientation = isovist_orientation(isovists_->at(c.position));

        auto bestGateway = gateway_boundary_line_at_cell(c.position, isoOrientation, *grid_);
        double bestNormal = isoOrientation;
        position_value_t bestCell = c;

        for (double orientation = -kAngleSearchRange; orientation < kAngleSearchRange; orientation += M_PI / 180.0f) {
            auto cellGateway = gateway_boundary_line_at_cell(c.position, orientation + isoOrientation, *grid_);

            if (cellGateway   // must have found a gateway for the cell
                              //                 && validator_->isValidGateway(*cellGateway) // and it must be valid
                && (!bestGateway
                    || is_shorter_gateway(*cellGateway,
                                          *bestGateway,
                                          c.position,
                                          bestCell.position,
                                          maximum.maximum.position)))   // and if shorter, it's the new best
            {
                bestGateway = cellGateway;
                bestNormal = orientation + isoOrientation;
                bestCell = c;
            }
        }

        if (bestGateway) {
            auto g = create_gateway_at_cell(bestCell.position, bestNormal, nextGatewayId_, *grid_);
            if (g) {
                possibleGateways.push_back({*g, std::abs(bestCell.value), false});
            }
        }
    }

    double meanDist = 0.0;
    double sumDistWeights = 0.0;
    for (auto& c : maximumCells_) {
        meanDist += c.value * grid_->getMetricDistance(c.position.x, c.position.y);
        sumDistWeights += c.value;
    }
    meanDist /= sumDistWeights;

    std::sort(possibleGateways.begin(),
              possibleGateways.end(),
              [meanDist](const WeightedGateway& lhs, const WeightedGateway& rhs) -> bool {
                  // If the differences are approximately the same, then take the gateway with the higher weight.
                  // Otherwise opt for the gateway with the closest distance to the mean.
                  double lhsDiff = std::abs(lhs.gateway.length() - meanDist) / lhs.weight;
                  double rhsDiff = std::abs(rhs.gateway.length() - meanDist) / rhs.weight;
                  return (absolute_fuzzy_equal(lhsDiff, rhsDiff) && lhs.weight > rhs.weight) || (lhsDiff < rhsDiff);
              });

    auto bestGatewayIt = possibleGateways.begin();

    //     double isoOrientation = isovist_orientation(isovists_->at(maximum.maximum.position));
    //     for(double orientation = -kAngleSearchRange; orientation < kAngleSearchRange; orientation += 0.01)
    //     {
    //         auto cellGateway = create_gateway_at_cell(maximum.maximum.position, orientation + isoOrientation,
    //         nextGatewayId_, *grid_); if(cellGateway
    //             && validator_->isValidGateway(*cellGateway)
    //             && (!haveBest || is_shorter_gateway(*cellGateway, *bestGateway, maximum.maximum.position)))
    //         {
    //             bestGateway = cellGateway;
    //             haveBest = true;
    //         }
    //     }
    //
    //     for(auto& c : maximumCells_)
    //     {
    //         double isoOrientation = isovist_orientation(isovists_->at(c));
    //
    //         for(double orientation = -kAngleSearchRange; orientation < kAngleSearchRange; orientation += 0.01)
    //         {
    //             auto cellGateway = create_gateway_at_cell(c, orientation + isoOrientation, nextGatewayId_, *grid_);
    //
    //             if(cellGateway
    //                 && validator_->isValidGateway(*cellGateway)
    //                 && (!haveBest || is_better_gateway(*cellGateway, *bestGateway, c, bestGatewayCell,
    //                 maximum.maximum)))
    //             {
    //                 bestGateway = cellGateway;
    //                 bestGatewayCell = c;
    //                 haveBest = true;
    //             }
    //         }
    //     }

    ++nextGatewayId_;

    // Now that the gateway has been found, do the expensive normal calculation
    if (bestGatewayIt != possibleGateways.end()) {
        gatewayHypotheses_.push_back({bestGatewayIt->gateway, std::abs(maximum.totalChange), false});
    }
}

void IsovistOrientationGatewayGenerator::extractEdgeCellsForMaximum(const isovist_local_maximum_t& maximum)
{
    // Remove any cells associated with the sources that aren't part of the current edge
    edgeCells_.clear();
    extract_edge_from_skeleton(maximum.maximum.position, *grid_, edgeCells_);

    if (edgeCells_.size() < 3) {
        return;
    }
    // Ignore the junctions at either end of the edge
    edgeCells_.erase(edgeCells_.begin());
    edgeCells_.pop_back();

    maximumCells_.clear();
    // Sort the maximum cells in order along the edge
    for (auto c : edgeCells_) {
        // See if this edge cell is part of the maximum
        auto maxIt =
          std::find_if(maximum.skeletonCells.begin(), maximum.skeletonCells.end(), [c](const position_value_t& pv) {
              return pv.position == c;
          });

        // If so, then add it to the maximum cells
        if (maxIt != maximum.skeletonCells.end()) {
            maximumCells_.push_back(*maxIt);
        }
    }

    // Grow out the edge cells in each direction until the gradient begins increasing again. This accounts for a
    // maxima that doesn't quite reach the associated source/skeleton that produce a valid gateway as a consequence
    // of clutter or other noise sources
    // Start growing the front
    bool madeChange = !maximumCells_.empty();
    while (madeChange) {
        madeChange = false;
        cell_t growCell = maximumCells_.front().position;
        auto growValue = gradients_->gradientAt(growCell).value;

        for (auto source :
             boost::make_iterator_range(grid_->beginSourceCells(growCell), grid_->endSourceCells(growCell))) {
            for (auto& skel : sourceToSkeleton_[source]) {
                bool onSameEdge = utils::contains(edgeCells_, skel);
                bool isAdded = utils::contains_if(maximumCells_, [skel](const position_value_t& pv) {
                    return pv.position == skel;
                });
                auto gradient = gradients_->gradientAt(skel);

                if (onSameEdge                            // only grow along the same edge
                    && !isAdded                           // don't add duplicates
                    && (gradient.value * growValue > 0)   // don't cross any zero crossings of the gradient
                    && (std::abs(gradient.value) <= std::abs(growValue)))   // only allow lower gradients
                {
                    maximumCells_.push_front(gradient);
                    madeChange = true;
                }
            }
        }
    }

    // Then grow at the back
    madeChange = !maximumCells_.empty();
    while (madeChange) {
        madeChange = false;
        cell_t growCell = maximumCells_.back().position;
        auto growValue = gradients_->gradientAt(growCell).value;

        for (auto source :
             boost::make_iterator_range(grid_->beginSourceCells(growCell), grid_->endSourceCells(growCell))) {
            for (auto& skel : sourceToSkeleton_[source]) {
                bool onSameEdge = utils::contains(edgeCells_, skel);
                bool isAdded = utils::contains_if(maximumCells_, [skel](const position_value_t& pv) {
                    return pv.position == skel;
                });
                auto gradient = gradients_->gradientAt(skel);

                if (onSameEdge                            // only grow along the same edge
                    && !isAdded                           // don't add duplicates
                    && (gradient.value * growValue > 0)   // don't cross any zero crossings of the gradient
                    && (std::abs(gradient.value) <= std::abs(growValue)))   // only allow lower gradients
                {
                    maximumCells_.push_back(gradient);
                    madeChange = true;
                }
            }
        }
    }

    // Remove any duplicate cells -- sort in decreasing order of value for equal positions, as erase unique removes all
    // after the first stored value
    std::sort(maximumCells_.begin(), maximumCells_.end(), [](const position_value_t& lhs, const position_value_t& rhs) {
        return (lhs.position < rhs.position) || ((lhs.position == rhs.position) && (lhs.value > rhs.value));
    });
    utils::erase_unique(maximumCells_);
}

double isovist_orientation(const utils::Isovist& isovist)
{
    return isovist.scalar(utils::Isovist::kMinDistOrientation) + M_PI_2;
}

bool is_shorter_gateway(const Gateway& lhs, const Gateway& rhs, cell_t maximumCell)
{
    double lhsLength = length(lhs.cellBoundary());
    double rhsLength = length(rhs.cellBoundary());

    if (lhsLength == rhsLength) {
        return distance_between_points(lhs.skeletonCell(), maximumCell)
          < distance_between_points(rhs.skeletonCell(), maximumCell);
    }

    return lhsLength < rhsLength;
}

bool is_shorter_gateway(const Line<int>& lhs, const Line<int>& rhs, cell_t lhsCell, cell_t rhsCell, cell_t maximumCell)
{
    double lhsLength = length(lhs);
    double rhsLength = length(rhs);

    if (lhsLength == rhsLength) {
        return distance_between_points(lhsCell, maximumCell) < distance_between_points(rhsCell, maximumCell);
    }

    return lhsLength < rhsLength;
}

bool is_better_gateway(const Gateway& lhs,
                       const Gateway& rhs,
                       cell_t lhsCell,
                       cell_t rhsCell,
                       position_value_t maximumValue)
{
    double lhsScore = length(lhs.cellBoundary()) + distance_between_points(lhsCell, maximumValue.position);
    double rhsScore = length(rhs.cellBoundary()) + distance_between_points(rhsCell, maximumValue.position);
    return lhsScore < rhsScore;
}

}   // namespace hssh
}   // namespace vulcan


#include <boost/optional/optional.hpp>
#include <array>
#include <vector>

#include "Util/Arc_Assert.hpp"

namespace Quoridor {

  const int BOARD_SIZE = 9;
  const int BOARD_BYTES = (BOARD_SIZE * (BOARD_SIZE - 1) * 2 + 8 * 8) / 8;

  enum Direction : int8_t {
    UP,
    DOWN,
    LEFT,
    RIGHT
  };

  enum WallOrientation : int8_t {
    HORIZONTAL,
    VERTICAL
  };

  enum Player : int8_t {
    PLAYER_ONE = 0,
    PLAYER_TWO = 1
  };

  const int8_t MASK_HALF_BYTE = 0x0F;
  // Point class that supports only the range needed for the game. Ie. [0,9) x [0,9)
  // first 4 bits are X, next 4 bits are Y.
  class Point {
  public:
    Point(int8_t x, int8_t y)
      : pos((x & MASK_HALF_BYTE) | ((y & MASK_HALF_BYTE) << 4))
    {}

    inline int8_t x() const {
      return pos & MASK_HALF_BYTE;
    }
    inline int8_t y() const {
      return (pos >> 4) & MASK_HALF_BYTE;
    }

    inline bool operator==(const Point& other) const {
      return other.pos == this->pos;
    }
    inline bool operator<(const Point& other) const {
      return this->pos < other.pos;
    }
  private:
    int8_t pos;
  };

  const int8_t STARTING_WALL_COUNTS = 10;
  // Like the point class this only  supports the range of walls needed for the game. Ie. [0,10) x [0,10)
  // first 4 bits are p1 walls, next 4 bits are p2 walls.
  class WallCounts {
  public:
    WallCounts()
      : _counts((STARTING_WALL_COUNTS & MASK_HALF_BYTE) | ((STARTING_WALL_COUNTS & MASK_HALF_BYTE) << 4))
    {}

    inline int8_t wallCountForPlayer(Player player) const {
      return (_counts >> (4 * player)) & MASK_HALF_BYTE;
    }
    inline void decrementWallCountForPlayer(Player player) {
      int8_t wallCount = wallCountForPlayer(player);
      ARC_ASSERT(wallCount > 0);
      --wallCount;
      wallCount <<= (4 * player);
      const int8_t CLEAR_MASK = ~(MASK_HALF_BYTE << (4 * player));
      _counts = (_counts & CLEAR_MASK) | wallCount;
    }
  private:
    int8_t _counts;
  };

  enum MoveType : int8_t {
    MOVE_PIECE = 0,
    PLACE_HORIZONAL_WALL = 1,
    PLACE_VERTICAL_WALL = 2,
    JUMP_PIECE = 3
  };

  union MoveInfo {
    MoveInfo(Direction);
    MoveInfo(Point);

    Direction pieceMoveDirection;
    Point wallCenter;
  };

  class Move {
  public:
    Move(Player player, Direction direction);
    Move(Player player, MoveType orientation, Point wallCenter);

    bool operator==(const Move& other) const;
    bool operator<(const Move& other) const;

    Player player;
    MoveType type;
    MoveInfo info;
  };

  class Wall {
  public:
    int centerX;
    int centerY;
    bool isVertical;

    bool operator==(const Wall& other) const;
    bool operator<(const Wall& other) const;
  };

  const int8_t INVALID_WALL_NUMBER = -1;
  const int8_t INVALID_WALL_VALUE = -2;
  const int8_t TWO_BIT_MASK = 0x03;
  class WallsState {
  public:
    WallsState();

    // Provides a list of all moves where walls could be placed without collision.
    std::vector<Move> availableWallPlacements(Player player) const;

    void placeWall(int8_t centerX, int8_t centerY, MoveType type);
    std::vector<Wall> walls() const;
  private:

    inline int8_t wallByteOffset(int8_t pointNumber) const {
      return pointNumber / 4;
    }

    inline int8_t wallBitOffset(int8_t pointNumber) const {
      return (pointNumber % 4) * 2;
    }

    inline int8_t wallValue(int8_t wallNumber) const {
      const int8_t byteNumber = wallByteOffset(wallNumber);
      const int8_t bitOffset = wallBitOffset(wallNumber);
      if (byteNumber < 0 || byteNumber > _state.size()) {
        return INVALID_WALL_VALUE;
      }
      return (_state[byteNumber] >> bitOffset) & TWO_BIT_MASK;
    }

    std::array<int8_t, BOARD_BYTES> _state;
  };

  class Board final {
  public:
    Board();

    // For visualization
    Point playerPosition(Player player) const;
    int wallCount(Player player) const;
    std::vector<Wall> walls() const;

    // For changing state
    const std::vector<Move> availableMoves(Player player);
    void doMove(const Move& move);
  private:
    std::vector<Move> availablePieceMovesForPlayer(Player player);
    std::vector<Move> availableWallPlacementsForPlayer(Player player);

    Point _playerOnePosition;
    Point _playerTwoPosition;
    WallCounts _playerWalls;
    WallsState _wallsState;
  };
}


#include <boost/hana.hpp>


//////////////////////////////////////////////////////////////////////////////
// data::field::survival::accumulator::mean_response.hpp                    //
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DATA_FIELD_SURVIVAL_ACCUMULATOR_MEAN_RESPONSE_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_DATA_FIELD_SURVIVAL_ACCUMULATOR_MEAN_RESPONSE_HPP_ER_2010
#include <boost/statistics/detail/data/field/framework/function/field_value.hpp>
#include <boost/statistics/detail/data/field/survival/fields/entry_time.hpp>
#include <boost/statistics/detail/data/field/survival/keyword/entry_time.hpp>
#include <boost/statistics/detail/data/keyword/survival/entry_time.hpp>

#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>
#include <boost/parameter/binding.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/parameters/accumulator.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics_fwd.hpp>

#include <boost/statistics/detail/accumulator/statistics/count_less_than.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace data{
namespace accumulator{
namespace impl
{

	// U is a unit
	// S is a sequence of functors
    template<typename U,typename S>
    class dispatcher
    {

		typedef tags_; // extract the tags from U
		typedef pairs_; // pairs (Tag,Stat)

        public:

        typedef void result_type;

        dispatcher(const S& s):s_(s){}

        template<typename U1>
        void operator()(const U1& u){
        	boost::fusion::for_each(stats,f(this->unit));
        }

		template<typename Tag,typename Stat,typename Unit>
		struct f{
        	f(U& u):u_(u){}

			template<typename T>
            void operator()(const t& in){
				field_value<Tag>(this->unit) = this->stat(
					field_value<Tag>(in);
				);
        	}

        };

		U u_;
        pairs_ pairs;

    };

}//impl

///////////////////////////////////////////////////////////////////////////////
// tag::proportion_less_than
namespace tag
{
    struct proportion_less_than
      : boost::accumulators::depends_on<
        boost::accumulators::tag::count,
        tag::count_less_than
    >
    {
      typedef statistics::detail::accumulator::
      	impl::proportion_less_than<boost::mpl::_1> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::proportion_less_than
namespace extract
{

  	template<typename AccSet>
  	typename boost::mpl::apply<
    	AccSet,
        statistics::detail::accumulator::tag::proportion_less_than
	>::type::result_type
  	proportion_less_than(AccSet const& acc){
    	typedef statistics::detail::accumulator
        	::tag::proportion_less_than the_tag;
    	return boost::accumulators::extract_result<the_tag>(acc);
  	}

}

using extract::proportion_less_than;

}// field
}// data
}// detail
}// statistics
}// boost

#endif


/* Copyright (c) 2010 Stanford University
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR(S) DISCLAIM ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL AUTHORS BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

/**
 * \file
 * Benchmark for Crc32C, a Nehalem instruction implementation of CRC32
 * with the Castagnoli polynomial.
 */

// RAMCloud pragma [CPPLINT=0]

#include <Common.h>
#include <BenchUtil.h>
#include "Crc32C.h"
#include <boost/crc.hpp>

using namespace RAMCloud;

/**
 * Adapter for boost's CRC library.
 * This thing is ~80MB/sec awful.
 */
class BoostCrc32C {
  public:
    typedef uint32_t ResultType;
    BoostCrc32C() : crc() {}
    BoostCrc32C& update(const void* buffer, uint32_t bytes) {
        crc.process_bytes(buffer, bytes);
        return *this;
    }
    ResultType getResult() {
        return crc();
    }
  private:
    boost::crc_optimal<32, 0x1EDC6F41, 0xFFFFFFFF, 0xFFFFFFFF, true, true> crc;
};

#if BOOST_CRC32C
typedef BoostCrc32C Checksum;
#else
typedef Crc32C Checksum;
#endif

static void
measure(int bytes, bool print = true)
{
    uint8_t *array = reinterpret_cast<uint8_t *>(xmalloc(bytes));
    Checksum crc;

    // randomize input
    for (int i = 0; i < bytes; i++)
        array[i] = generateRandom();

    // do more runs for smaller inputs
    int runs = 1;
    if (bytes < 4096)
        runs = 100;

    // run the test. be sure method call isn't removed by the compiler.
    uint64_t total = 0;
    for (int i = 0; i < runs; i++) {
        uint64_t before = rdtsc();
        crc.update(array, bytes);
        total += (rdtsc() - before);
    }
    total /= runs;

    if (print) {
        uint64_t nsec = RAMCloud::cyclesToNanoseconds(total);
        printf("%10d bytes: %10llu ticks    %10llu nsec    %3llu nsec/byte   "
            "%7llu MB/sec    crc32c 0x%08x\n",
            bytes,
            total,
            nsec,
            nsec / bytes,
            (uint64_t)(1.0e9 / ((float)nsec / bytes) / (1024*1024)),
            crc.getResult());
    }

    free(array);
}

int
main()
{
    measure(4096, false);   // warm up
    for (int i = 1; i < 128; i++)
        measure(i);
    for (int i = 128; i <= (16 * 1024 * 1024); i *= 2)
        measure(i);

    return 0;
}


// This example program shows how to use dlib's implementation of the paper:
//    One Millisecond Face Alignment with an Ensemble of Regression Trees by
//    Vahid Kazemi and Josephine Sullivan, CVPR 2014
//
// http://www.csc.kth.se/~vahidk/papers/KazemiCVPR14.pdf
//
// Changelog:
//  - support regression in shape space for simple size reduction
//  - support line indexed features (more pose invariant in some cases (per RCPR))
//  - support normalized pixel differences (can improve regression in some cases)
//  - support trailing ellipse parameters (embedded in point list)
//    for simultaneous contour and ellipse model regression

#include "drishti/core/drishti_stdlib_string.h" // FIRST !!!

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <dlib/data_io.h>
#include <dlib/image_transforms/assign_image.h>
#include <dlib/statistics/statistics.h>
#include <dlib/image_processing/shape_predictor.h>
#include <dlib/opencv/cv_image.h>
#include <dlib/vectorstream.h>
#include <dlib/serialize.h>
#include <dlib/data_io/load_image_dataset.h>

#include "drishti/core/string_utils.h"
#include "drishti/core/Line.h"
#include "drishti/ml/shape_predictor_archive.h"
#include "drishti/ml/shape_predictor_trainer.h"
#include "drishti/geometry/Ellipse.h"
#include "drishti/core/drishti_cereal_pba.h"
#include "drishti/core/drishti_cv_cereal.h"

#include "RecipeIO.h"

#include "cxxopts.hpp"

#include <iostream>

//#define _SP dlib
#define _SP drishti::ml

// _cascade_depth = 10;
// _tree_depth = 4;
// _num_trees_per_cascade_level = 500;
// _nu = 0.1;
// _oversampling_amount = 20;
// _feature_pool_size = 400;
// _lambda = 0.1;
// _num_test_splits = 20;
// _feature_pool_region_padding = 0;

static std::vector<std::vector<double>>
get_interocular_distances(const std::vector<std::vector<dlib::full_object_detection>>& objects);

using DlibImageArray = dlib::array<dlib::array2d<uint8_t>>;
using DlibObjectSet = std::vector<std::vector<dlib::full_object_detection>>;

static void reduce_images(DlibImageArray& images_train, DlibObjectSet& faces_train, int ellipse_count, int width);
static void dump_thumbs(DlibImageArray& images_train, DlibObjectSet& faces_train, const std::string& dir, int ellipse_count);

static std::vector<int> parse_dimensions(const std::string& str);
static dlib::rectangle parse_roi(const std::string& str);

static void view_images(DlibImageArray& images_train, DlibObjectSet& faces_train)
{
    for (int i = 0; i < images_train.size(); i++)
    {
        auto& src = images_train[i];
        cv::Mat image(src.nr(), src.nc(), CV_8UC1, (void*)&src[0][0], src.width_step());
        cv::Mat canvas = image.clone();

        for (int j = 0; j < faces_train[i].size(); j++)
        {
            // reduce roi:
            for (int k = 0; k < faces_train[i][j].num_parts(); k++)
            {
                auto& part = faces_train[i][j].part(k);
                cv::Point2f p(part.x(), part.y());
                cv::circle(canvas, p, 4, { 255, 255, 255 }, -1, 8);
                cv::imwrite("/tmp/foo.png", canvas);
            }
        }
    }
}

int gauze_main(int argc, char* argv[])
{
    auto logger = drishti::core::Logger::create("train_shape_predictor");

    const auto argumentCount = argc;

    bool do_help = false;
    bool do_threads = false;
    bool do_thumbs = false;
    bool do_verbose = false;
    bool do_silent = false;

    drishti::dlib::Recipe recipe;

    std::string sModel;
    std::string sRoi;
    std::string sTest;
    std::string sTrain;
    std::string sRecipe;
    std::string sRecipeOut;
    std::string sOutput;

    cxxopts::Options options("train_shape_predictor", "Command line interface for dlib shape_predictor training");

    // clang-format off
    options.add_options()
        ( "output", "Output directory for intermediate results", cxxopts::value<std::string>(sOutput))
        ( "train", "training file", cxxopts::value<std::string>(sTrain))
        ( "test", "testing file", cxxopts::value<std::string>(sTest))
        ( "model", "Model file", cxxopts::value<std::string>(sModel))
        ( "thumbs", "dump thumbnails of width n", cxxopts::value<bool>(do_thumbs))
        ( "roi", "exclusion roi (x,y,w,h) normalized cs", cxxopts::value<std::string>(sRoi))

        // Regression parameters:
        ( "recipe", "Cascaded pose regression training recipe", cxxopts::value<std::string>(sRecipe))
        ( "boilerplate", "Output boilerplate recipe file", cxxopts::value<std::string>(sRecipeOut))

        ( "threads", "Use worker threads when possible", cxxopts::value<bool>(do_threads))
        ( "verbose", "Print verbose diagnostics", cxxopts::value<bool>(do_verbose))
        ( "silent", "Disable logging entirely", cxxopts::value<bool>(do_silent))
        ( "help", "Print the help message", cxxopts::value<bool>(do_help));
    // clang-format on

    auto opts = options.parse(argc, argv);

    if(do_silent)
    {
        logger->set_level(spdlog::level::off);
    }

    if((argumentCount <= 1) || opts.count("help"))
    {
        logger->info(options.help({""}));
        return 0;
    }

    if(!sRecipeOut.empty())
    {
        recipe.dimensions = { 4, 8, 12, 16, 20, 24 };
        saveJSON(sRecipeOut, recipe);
        return 0;
    }

    if(sRecipe.empty())
    {
        logger->error("Must specify valid training recipe.");
        return 1;
    }

    loadJSON(sRecipe, recipe);

    if(sTrain.empty())
    {
        logger->error("Must specify valid XML training file.");
        return 1;
    }

    if(sModel.empty())
    {
        logger->error("Must specify output *.dat model file.");
        return 1;
    }

    if(do_verbose)
    {
        logger->info("cascade_depth: {}", recipe.cascades);
        logger->info("tree_depth: {}", recipe.depth);
        logger->info("num_trees_per_cascade_level: {}", recipe.trees_per_level);
        logger->info("nu: {}", recipe.nu);
        logger->info("oversampling_amount: {}", recipe.oversampling);
        logger->info("feature_pool_size: {}", recipe.features);
        logger->info("lambda: {}", recipe.lambda);
        logger->info("num_test_splits: {}", recipe.splits);
        logger->info("feature_pool_region_padding: {}", recipe.padding);
        logger->info("use npd: {}", recipe.npd);
        logger->info("affine: {}", recipe.do_affine);
        logger->info("interpolated: {}", recipe.do_interpolate);
    }

    dlib::array<dlib::array2d<uint8_t>> images_train, images_test;
    std::vector<std::vector<dlib::full_object_detection> > faces_train, faces_test;

    dlib::image_dataset_file source(sTrain);
    source.skip_empty_images();
    load_image_dataset(images_train, faces_train, source);

    if(faces_train.empty())
    {
        logger->error("No shapes specified for training");
        return 1;
    }

    if(do_thumbs)
    {
        dump_thumbs(images_train, faces_train, sOutput, recipe.ellipse_count);
        return 0;
    }

    // Here we optionally downsample:
    if(recipe.width > 0)
    {
        reduce_images(images_train, faces_train, recipe.ellipse_count, recipe.width);
    }

    dlib::drectangle roi;
    if(!sRoi.empty())
    {
        roi = parse_roi(sRoi);
    }

    // Now make the object responsible for training the model.
    _SP::shape_predictor_trainer trainer;
    trainer.set_cascade_depth(recipe.cascades);
    trainer.set_tree_depth(recipe.depth);
    trainer.set_num_trees_per_cascade_level(recipe.trees_per_level);
    trainer.set_nu(recipe.nu);                            // regularization (smaller nu == more regularization)
    trainer.set_oversampling_amount(recipe.oversampling); // amount of oversampling for training data
    trainer.set_feature_pool_size(recipe.features);
    trainer.set_lambda(recipe.lambda);                    // feature separation (not learning rate)
    trainer.set_num_test_splits(recipe.splits);
    trainer.set_feature_pool_region_padding(recipe.padding);

    // new parameters
    if(recipe.dimensions.size())
    {
        trainer.set_dimensions(recipe.dimensions);
        trainer.set_cascade_depth(recipe.dimensions.size());
    }
    trainer.set_ellipse_count(recipe.ellipse_count);
    trainer.set_do_npd(recipe.npd);
    trainer.set_do_affine(recipe.do_affine);
    trainer.set_roi(roi);
    trainer.set_do_line_indexed(recipe.do_interpolate);

    trainer.set_num_threads(8);

    if(do_verbose)
    {
        trainer.be_verbose();
    }

    int max_dim = faces_train[0][0].num_parts() * 2;
    for(const auto &dim : recipe.dimensions)
    {
        CV_Assert(0 < dim && dim <= max_dim);
    }

    if(do_verbose)
    {
        logger->info("Begin training...");
    }

    std::map<int, float> weights;
    for(const auto &w : recipe.weights)
    {
        weights[std::stoi(w.first)] = w.second;
    }

    //_SP::shape_predictor sp;
    _SP::shape_predictor sp = trainer.train(images_train, faces_train, weights);
    if(do_verbose)
    {
        logger->info("Done training...");
    }

    // Finally, we save the model to disk so we can use it later.
    //dlib::serialize( sModel.c_str() ) << sp;

    if(do_verbose)
    {
        logger->info("Saving to ...{}", sModel);
    }

    save_cpb(sModel, sp);
    sp.populate_f16(); // populate half precision leaf nodes

    if(do_verbose)
    {
        logger->info("Done saving ...{}", sModel);
    }

    auto train_iod = get_interocular_distances(faces_train);
    double training_error = test_shape_predictor(sp, images_train, faces_train, train_iod);

    if(do_verbose)
    {
        logger->info("Mean training error: {}", training_error);
    }

    if(!sTest.empty())
    {
        load_image_dataset(images_test, faces_test, sTest);

        auto test_iod = get_interocular_distances(faces_test);
        float test_error = test_shape_predictor(sp, images_test, faces_test, test_iod);

        if(do_verbose)
        {
            logger->info("Mean testing error: {}", test_error);
        }
    }

    return 0;
}

int main(int argc, char *argv[])
{
    try
    {
        gauze_main(argc, argv);
    }
    catch (std::exception& e)
    {
        std::cerr << "Exception thrown:" << e.what();
    }
}

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

static double interocular_distance (const dlib::full_object_detection& det)
{
    double length = 0;
    for(int i = 0; i < det.num_parts(); i++)
        for(int j = i+1; j < det.num_parts(); j++)
        {
            length = std::max(double(dlib::length(det.part(i) - det.part(j))), length);
        }

    return length;
}

static std::vector<std::vector<double> > get_interocular_distances (const DlibObjectSet& objects)
{
    std::vector<std::vector<double> > temp(objects.size());
    for (unsigned long i = 0; i < objects.size(); ++i)
    {
        for (unsigned long j = 0; j < objects[i].size(); ++j)
        {
            temp[i].push_back(interocular_distance(objects[i][j]));
        }
    }
    return temp;
}

static void dump_thumbs(DlibImageArray &images_train, DlibObjectSet &faces_train, const std::string &dir, int ellipse_count)
{
    static std::vector< cv::Vec3b > rainbow // RGB (inuitive)
    {
        {255,0,0},     // red
        {255,127,0},   // orange
        {255,255,0},   // yellow
        {0,255,0},     // green
        {0,0,255},     // blue
        {75, 0, 130},  // indigo
        {139, 0, 255}, // violet
        {127,127,127}, // white
    };
    for(auto &c : rainbow)
    {
        std::swap(c[0], c[2]); // RGB -> BGR
    }

    for(int i = 0; i < images_train.size(); i++)
    {
        auto &src = images_train[i];
        cv::Mat image(src.nr(), src.nc(), CV_8UC1, (void *)&src[0][0], src.width_step()), small;

        for(int j = 0; j < faces_train[i].size(); j++)
        {
            auto &roiIn = faces_train[i][j].get_rect();
            cv::Rect roi(roiIn.left(), roiIn.top(), roiIn.width(), roiIn.height());

            cv::Mat canvas;
            try
            {
                if(image.channels() != 3)
                {
                    cv::cvtColor(image(roi), canvas, cv::COLOR_GRAY2BGR);
                }
                else
                {
                    canvas = image(roi).clone();
                }
            }
            catch(...)
            {
                std::cout << "BAD ROI: " <<  i << " " << j << std::endl;
                continue;
            }

            int end =faces_train[i][j].num_parts() - (ellipse_count * 5);
            for(int k = 0; k < end; k++)
            {
                auto &part = faces_train[i][j].part(k);
                cv::Point p(part.x(), part.y());
                cv::circle(canvas, p - roi.tl(), 3, rainbow[k%rainbow.size()], -1, 8);
            }

            std::stringstream ss;
            ss << dir << "/" << "roi_" <<  i << "_" << j << ".png";
            cv::imwrite(ss.str(), canvas);
        }
    }
}

static void reduce_int(long &value, float scale)
{
    value = static_cast<long>(value * scale + 0.5f);
}

static void reduce_int(dlib::rectangle &roi, float scale)
{
    reduce_int(roi.top(), scale);
    reduce_int(roi.left(), scale);
    reduce_int(roi.right(), scale);
    reduce_int(roi.bottom(), scale);
}

static void reduce_images(DlibImageArray &images_train, DlibObjectSet &faces_train, int ellipse_count, int width)
{
    for(int i = 0; i < images_train.size(); i++)
    {
        auto &src = images_train[i];
        cv::Mat image(src.nr(), src.nc(), CV_8UC1, (void *)&src[0][0], src.width_step()), small;

        const float scale = float(width) / image.cols;
        cv::resize(image, small, {}, scale, scale, cv::INTER_LANCZOS4);
        dlib::assign_image(src, dlib::cv_image<uint8_t>(small));

        for(int j = 0; j < faces_train[i].size(); j++)
        {
            // reduce roi:
            reduce_int(faces_train[i][j].get_rect(), scale);

            const int end = faces_train[i][j].num_parts() - (ellipse_count * 5);
            for(int k = 0; k < end; k++)
            {
                auto &part = faces_train[i][j].part(k);
                reduce_int(part.x(), scale);
                reduce_int(part.y(), scale);
                CV_Assert(part.x() < 1000 && part.y() < 1000);
            }

            // Handle the ellipse data (optional):
            for(int k = end; k < faces_train[i][j].num_parts(); k+= 5)
            {
                auto & obj = faces_train[i][j];
                obj.part(k+0).x() *= scale;
                obj.part(k+1).x() *= scale;
                obj.part(k+2).x() *= scale;
                obj.part(k+3).x() *= scale;
            }
        }
    }
}

static dlib::rectangle parse_roi(const std::string &str)
{
    std::vector<std::string> tokens;
    drishti::core::tokenize(str, tokens);
    float l = std::stof(tokens[0]);
    float t = std::stof(tokens[1]);
    float r = std::stof(tokens[2]);
    float b = std::stof(tokens[3]);
    return dlib::drectangle(l, t, r-l, b-t);
}


#include <boost/test/unit_test.hpp>

#include <physics/objects/Body.h>
#include <physics/World.h>
#include <physics/objects/ObjectsFactory.h>

using namespace Physics;
using namespace Objects;

struct BodyTestFixture {
	BodyTestFixture() :
		w(b2Vec2(0.0f, 9.8f), 4, 8),
		obj(ObjectsFactory::init(w.getWorld())) {
	}
	World w;
	ObjectsFactory& obj;
	std::vector<float> lenghts = { 0.1f, 0.2f, 0.3f, 0.4f, 0.5f, 0.6f, 0.7f, 0.8f };
};

BOOST_FIXTURE_TEST_SUITE(ObjectsFactoryTests, BodyTestFixture);

BOOST_AUTO_TEST_CASE(check_there_are_8_vertices)
{
	Body b(lenghts, Vector2(0.0f, 0.0f));
	BOOST_CHECK_EQUAL(b.getVertices().size(), 8);
}

BOOST_AUTO_TEST_CASE(check_body_vertices_are_calculated_propertly)
{
	Body b(lenghts, Vector2(0.0f, 0.0f));
	auto v = b.getVertices();

	BOOST_CHECK_EQUAL(v[0].x, 0.0f);
	BOOST_CHECK_EQUAL(v[0].y, 0.1f);

	BOOST_CHECK_EQUAL(v[1].x, -0.2f);
	BOOST_CHECK_EQUAL(v[1].y, 0.2f);

	BOOST_CHECK_EQUAL(v[2].x, -0.3f);
	BOOST_CHECK_EQUAL(v[2].y, 0.0f);

	BOOST_CHECK_EQUAL(v[3].x, -0.4f);
	BOOST_CHECK_EQUAL(v[3].y, -0.4f);

	BOOST_CHECK_EQUAL(v[4].x, 0.0f);
	BOOST_CHECK_EQUAL(v[4].y, -0.5f);

	BOOST_CHECK_EQUAL(v[5].x, 0.6f);
	BOOST_CHECK_EQUAL(v[5].y, -0.6f);

	BOOST_CHECK_EQUAL(v[6].x, 0.7f);
	BOOST_CHECK_EQUAL(v[6].y, 0.0f);

	BOOST_CHECK_EQUAL(v[7].x, 0.8f);
	BOOST_CHECK_EQUAL(v[7].y, 0.8f);
}

BOOST_AUTO_TEST_CASE(check_body_current_vertices_are_calculated_propertly)
{
	Body b(lenghts, Vector2(1.0f, 2.0f));
	auto v = b.getCurrentVertices();

	BOOST_CHECK_EQUAL(v[0].x, 1.0f);
	BOOST_CHECK_EQUAL(v[0].y, 2.1f);

	BOOST_CHECK_EQUAL(v[1].x, 0.8f);
	BOOST_CHECK_EQUAL(v[1].y, 2.2f);

	BOOST_CHECK_EQUAL(v[2].x, 0.7f);
	BOOST_CHECK_EQUAL(v[2].y, 2.0f);

	BOOST_CHECK_EQUAL(v[3].x, 0.6f);
	BOOST_CHECK_EQUAL(v[3].y, 1.6f);

	BOOST_CHECK_EQUAL(v[4].x, 1.0f);
	BOOST_CHECK_EQUAL(v[4].y, 1.5f);

	BOOST_CHECK_EQUAL(v[5].x, 1.6f);
	BOOST_CHECK_EQUAL(v[5].y, 1.4f);

	BOOST_CHECK_EQUAL(v[6].x, 1.7f);
	BOOST_CHECK_EQUAL(v[6].y, 2.0f);

	BOOST_CHECK_EQUAL(v[7].x, 1.8f);
	BOOST_CHECK_EQUAL(v[7].y, 2.8f);

}

BOOST_AUTO_TEST_SUITE_END();


#include <boost/test/unit_test.hpp>

#include <graphene/chain/database.hpp>
#include <graphene/chain/exceptions.hpp>
#include <graphene/chain/hardfork.hpp>

#include <graphene/chain/account_object.hpp>
#include <graphene/chain/asset_object.hpp>
#include <graphene/chain/committee_member_object.hpp>
#include <graphene/chain/proposal_object.hpp>

#include <fc/crypto/digest.hpp>

#include "../common/database_fixture.hpp"
#include "../common/test_utils.hpp"

using namespace graphene::chain;
using namespace graphene::chain::test;

BOOST_FIXTURE_TEST_SUITE( asset_parameters_tests, database_fixture )

int64_t percent_to_int(double percent) { return percent * 100000; };

BOOST_AUTO_TEST_CASE( create_edc )
{
   try {

      BOOST_TEST_MESSAGE( "=== create_edc ===" );

      asset_id_type edc_asset_id = db.get_index<asset_object>().get_next_id();
      (void)edc_asset_id;

      asset_create_operation creator;
      creator.issuer = account_id_type();
      creator.fee = asset();
      creator.symbol = "EDC";
      creator.common_options.max_supply = 10000000000;
      creator.precision = 3;
      creator.common_options.market_fee_percent = GRAPHENE_MAX_MARKET_FEE_PERCENT/100; /*1%*/
      creator.common_options.issuer_permissions = UIA_ASSET_ISSUER_PERMISSION_MASK;
      creator.common_options.flags = charge_market_fee;
      creator.common_options.core_exchange_rate = price({asset(2),asset(1,asset_id_type(1))});
      trx.operations.push_back(std::move(creator));
      PUSH_TX( db, trx, ~0 );
      generate_block();
   } catch (...) {
      throw;
   }
}

BOOST_AUTO_TEST_CASE( test_premine )
{
   try {

      BOOST_TEST_MESSAGE( "=== test_premine ===" );

      ACTOR( abcde1 );
      ACTOR( abcde2 );
      ACTOR( alice );

      //assign privileges for create_asset_operation
      SET_ACTOR_CAN_CREATE_ASSET(alice_id);

      INVOKE(create_edc);
      asset_parameters ap;
      ap.bonus_percent = 0.1;
      ap.mining = false;
      ap.maturing_bonus_balance = false;
      ap.coin_maturing = false;
      ap.mandatory_transfer = 0;
      ap.premine = 1000000000;
      auto test_asset = create_user_issued_asset( "TEST", ap, alice_id );
      generate_block();

      BOOST_CHECK( get_balance(alice_id, test_asset.id) == ap.premine);

   } catch (fc::exception& e) {
      edump((e.to_detail_string()));
      throw;
   }
}

BOOST_AUTO_TEST_CASE( test_daily_bonus )
{
   try {

      BOOST_TEST_MESSAGE( "=== test_daily_bonus ===" );

      ACTOR( abcde1 );
      ACTOR( abcde2 );
      ACTOR( alice );

      //assign privileges for create_asset_operation
      SET_ACTOR_CAN_CREATE_ASSET(alice_id);

      INVOKE(create_edc);

      generate_blocks(fc::time_point_sec(HARDFORK_620_TIME));

      set_expiration(db, this->trx);
      asset_parameters ap;
      ap.bonus_percent = percent_to_int(0.1);
      ap.mining = false;
      ap.maturing_bonus_balance = false;
      ap.daily_bonus = true;
      ap.coin_maturing = false;
      ap.mandatory_transfer = 0;
      auto asset1 = create_user_issued_asset( "TESTA", ap, alice_id );
      ap.bonus_percent = percent_to_int(0.2);
      auto asset2 = create_user_issued_asset( "TESTB", ap, alice_id );

      ap.bonus_percent = percent_to_int(0.5);
      auto asset3 = create_user_issued_asset( "TESTC", ap, alice_id );

      set_expiration(db, this->trx);

      issue_uia( alice_id, asset( 300000, asset1.id ) );
      issue_uia( alice_id, asset( 300000, asset2.id ) );
      issue_uia( alice_id, asset( 300000, asset3.id ) );

      auto target = HARDFORK_620_TIME + fc::days( 1 );
      while( db.head_block_time() < target )
      {
         generate_block();
      }
      while( db.head_block_time() < target + fc::days( 2 ) )
      {
         generate_block();
      }

      auto& index = db.get_index_type<bonus_balances_index>().indices().get<by_account>();
      (void)index;

      BOOST_CHECK( get_balance(alice_id, asset1.id) == 399300  );
      BOOST_CHECK( get_balance(alice_id, asset2.id) == 518400  );
      BOOST_CHECK( get_balance(alice_id, asset3.id) == 1012500 );

   } catch (fc::exception& e) {
      edump((e.to_detail_string()));
      throw;
   }
}

BOOST_AUTO_TEST_CASE( test_asset_bonus_balances )
{
   try {

      BOOST_TEST_MESSAGE( "=== test_asset_bonus_balances ===" );

      ACTOR( abcde1 );
      ACTOR( abcde2 );
      ACTOR( alice );

      //assign privileges for create_asset_operation
      SET_ACTOR_CAN_CREATE_ASSET(alice_id);

      INVOKE(create_edc);

      generate_blocks(fc::time_point_sec(HARDFORK_620_TIME));

      set_expiration(db, this->trx);
      asset_parameters ap;
      ap.bonus_percent = percent_to_int(0.1);
      ap.mining = false;
      ap.maturing_bonus_balance = false;
      ap.coin_maturing = false;
      auto asset1 = create_user_issued_asset( "EDCA", ap, alice_id );
      ap.bonus_percent = 0.2;
      auto asset2 = create_user_issued_asset( "EDCB", ap, alice_id );
      ap.bonus_percent = 0.5;
      auto asset3 = create_user_issued_asset( "EDCC", ap, alice_id );
      set_expiration(db, this->trx);
      issue_uia( alice_id, asset( 300000, asset1.id ) );
      issue_uia( alice_id, asset( 300000, asset2.id ) );
      issue_uia( alice_id, asset( 300000, asset3.id ) );

      while( db.head_block_time() < HARDFORK_620_TIME + fc::days( 3 ) ) {
         generate_block();
      }

      auto& index = db.get_index_type<bonus_balances_index>().indices().get<by_account>();
      (void)index;

      BOOST_CHECK(true);
      // BOOST_CHECK( get_balance(alice_id, asset1.get_id()) == 399300  );
      // BOOST_CHECK( get_balance(alice_id, asset2.get_id()) == 518400  );
      // BOOST_CHECK( get_balance(alice_id, asset3.get_id()) == 1012500 );

   } catch (fc::exception& e) {
      edump((e.to_detail_string()));
      throw;
   }
}

BOOST_AUTO_TEST_CASE( mining_test )
{
   // std::cout << boost::unit_test::framework::current_test_case().p_name << std::endl;
   BOOST_TEST_MESSAGE( "=== mining_test ===" );

   ACTOR( abcde1 );
   ACTOR( abcde2 );
   ACTOR( alice );

   //assign privileges for create_asset_operation
   SET_ACTOR_CAN_CREATE_ASSET(alice_id);

   INVOKE(create_edc);

   generate_blocks(fc::time_point_sec(HARDFORK_620_TIME));

   set_expiration(db, this->trx);
   asset_parameters ap;
   ap.bonus_percent = percent_to_int(0.1);
   ap.mining = true;
   ap.maturing_bonus_balance = false;
   ap.coin_maturing = false;
   auto asset1 = create_user_issued_asset( "EDCA", ap, alice_id );
   issue_uia( alice_id, asset( 301000, asset1.id ) );

   auto target = db.get_dynamic_global_properties().next_maintenance_time + fc::minutes(2);
   while( db.head_block_time() < target)
   {
      generate_block();
   }
   target += fc::days(1);
   set_expiration(db, this->trx);
   db.modify(accounts_online_id_type()(db), [&](accounts_online_object& aoo) {
      aoo.online_info.emplace(alice_id, 720);
   });
   transfer(alice_id, account_id_type(), asset(1000, asset1.id), asset(0, asset_id_type(1)));
   while( db.head_block_time() < target)
   {
      generate_block();
   }
   BOOST_CHECK( get_balance( alice_id, asset1.id ) == 300000 + 300000 * 0.1 / 2);
}

BOOST_AUTO_TEST_CASE( test_mandatory_transfer )
{
   BOOST_TEST_MESSAGE( "=== test_mandatory_transfer ===" );

   try {
      ACTOR( abcde1 );
      ACTOR( abcde2 );
      ACTOR( alice );

      //assign privileges for create_asset_operation
      SET_ACTOR_CAN_CREATE_ASSET(alice_id);

      INVOKE(create_edc);

      generate_blocks(fc::time_point_sec(HARDFORK_620_TIME));
      set_expiration(db, this->trx);
      asset_parameters ap;
      ap.bonus_percent = percent_to_int(0.1);
      ap.daily_bonus = true;
      ap.mining = false;
      ap.maturing_bonus_balance = false;
      ap.coin_maturing = false;
      ap.mandatory_transfer = 999;
      ap.fee_paying_asset = asset_id_type();
      auto test_asset = create_user_issued_asset( "TEST", ap, alice_id );
      generate_block();

      issue_uia( alice_id, asset( 301899, test_asset.id ) );
      auto target = HARDFORK_620_TIME + fc::days( 1 );
      while( db.head_block_time() < target )
      {
         generate_block();
      }
      set_expiration(db, this->trx);
      BOOST_CHECK( get_balance(alice_id, test_asset.id) == 301899 );
      transfer(alice_id, account_id_type(), asset(900, test_asset.id), asset(0, test_asset.id));
      target += fc::days(1);
      while( db.head_block_time() < target )
      {
         generate_block();
      }
      set_expiration(db, this->trx);
      BOOST_CHECK( get_balance(alice_id, test_asset.id) == 300999 );
      transfer(alice_id, account_id_type(), asset(999, test_asset.id), asset(0, test_asset.id));
      target += fc::days(1);
      while( db.head_block_time() < target )
      {
         generate_block();
      }
      BOOST_CHECK( get_balance(alice_id, test_asset.id) == 330000 );
   } catch (fc::exception& e) {
      edump((e.to_detail_string()));
      throw;
   }
}

BOOST_AUTO_TEST_CASE( fee_paying_asset_test )
{
   BOOST_TEST_MESSAGE( "=== fee_paying_asset_test ===" );

   try {
      ACTOR( abcde1 );
      ACTOR( abcde2 );
      ACTOR( alice );

      // alice should be able to create assets
      SET_ACTOR_CAN_CREATE_ASSET(alice_id);

      INVOKE(create_edc);

      generate_blocks(fc::time_point_sec(HARDFORK_620_TIME) + fc::minutes(2));
      set_expiration(db, this->trx);
      asset_parameters ap;
      ap.fee_paying_asset = asset_id_type(0);
      auto test_asset = create_user_issued_asset( "TEST", ap, alice_id );
      issue_uia( account_id_type(), asset( 10000, asset_id_type( 1 ) ) );
      issue_uia( alice_id, asset( 10000, asset_id_type( 1 ) ) );
      transfer( account_id_type(), alice_id, asset( 1000 ), asset(1, asset_id_type(1) ) );
      issue_uia( alice_id, asset( 10000, test_asset.id ) );
      transfer( alice_id, account_id_type(), asset( 1000, test_asset.id ), asset( 100, test_asset.id ) );
      asset_parameters ap1;
      generate_block();
      auto edc_fee_asset = create_user_issued_asset( "EDCFEE", ap1, alice_id );
      issue_uia( alice_id, asset( 10000, edc_fee_asset.id ) );
      transfer( alice_id, account_id_type(), asset( 1000, edc_fee_asset.id ), asset( 100,  asset_id_type(1) ) );
   } catch (fc::exception& e) {
      edump((e.to_detail_string()));
      throw;
   }
}

BOOST_AUTO_TEST_SUITE_END()


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

// Copyright (c) 2007-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2014 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2014 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_MULTI_TO_MULTI_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_MULTI_TO_MULTI_HPP

#include <boost/range.hpp>

#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/distance_comparable_to_regular.hpp>
#include <boost/geometry/strategies/tags.hpp>

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

#include <boost/geometry/algorithms/not_implemented.hpp>

#include <boost/geometry/algorithms/dispatch/distance.hpp>

#include <boost/geometry/algorithms/detail/distance/single_to_multi.hpp>
#include <boost/geometry/algorithms/detail/distance/geometry_to_geometry_rtree.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace distance
{

template <typename Multi1, typename Multi2, typename Strategy>
class distance_multi_to_multi_generic
{
private:
    typedef typename strategy::distance::services::comparable_type
        <
            Strategy
        >::type comparable_strategy;

    typedef typename strategy::distance::services::return_type
        <
            comparable_strategy,
            typename point_type<Multi1>::type,
            typename point_type<Multi2>::type
        >::type comparable_return_type;

public:
    typedef typename strategy::distance::services::return_type
        <
            Strategy,
            typename point_type<Multi1>::type,
            typename point_type<Multi2>::type
        >::type return_type;

    static inline return_type apply(Multi1 const& multi1,
                Multi2 const& multi2, Strategy const& strategy)
    {
        comparable_return_type min_cdist = comparable_return_type();
        bool first = true;

        comparable_strategy cstrategy =
            strategy::distance::services::get_comparable
                <
                    Strategy
                >::apply(strategy);

        for(typename range_iterator<Multi1 const>::type it = boost::begin(multi1);
                it != boost::end(multi1);
                ++it, first = false)
        {
            comparable_return_type cdist =
                dispatch::splitted_dispatch::distance_single_to_multi
                    <
                        typename range_value<Multi1>::type,
                        Multi2,
                        comparable_strategy,
                        typename tag<typename range_value<Multi1>::type>::type,
                        typename tag<Multi2>::type,
                        typename strategy::distance::services::tag
                            <
                                comparable_strategy
                            >::type
                    >::apply(*it, multi2, cstrategy);
            if (first || cdist < min_cdist)
            {
                min_cdist = cdist;
                if ( geometry::math::equals(min_cdist, 0) )
                {
                    break;
                }
            }
        }

        return strategy::distance::services::comparable_to_regular
            <
                comparable_strategy,
                Strategy,
                Multi1,
                Multi2
            >::apply(min_cdist);
    }
};

}} // namespace detail::distance
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

namespace splitted_dispatch
{

template
<
    typename MultiGeometry1,
    typename MultiGeometry2,
    typename Strategy,
    typename Tag1,
    typename Tag2,
    typename StrategyTag
>
struct distance_multi_to_multi
    : not_implemented<MultiGeometry1, MultiGeometry2>
{};

template
<
    typename MultiPoint,
    typename MultiPolygon,
    typename Strategy
>
struct distance_multi_to_multi
    <
        MultiPoint, MultiPolygon, Strategy,
        multi_point_tag, multi_polygon_tag,
        strategy_tag_distance_point_segment
    > : detail::distance::distance_multi_to_multi_generic
        <
            MultiPoint, MultiPolygon, Strategy
        >
{};

template
<
    typename MultiLinestring1,
    typename MultiLinestring2,
    typename Strategy
>
struct distance_multi_to_multi
    <
        MultiLinestring1, MultiLinestring2, Strategy,
        multi_linestring_tag, multi_linestring_tag,
        strategy_tag_distance_point_segment
    > : detail::distance::geometry_to_geometry_rtree
        <
            MultiLinestring1, MultiLinestring2, Strategy
        >
{};

template
<
    typename MultiLinestring,
    typename MultiPolygon,
    typename Strategy
>
struct distance_multi_to_multi
    <
        MultiLinestring, MultiPolygon, Strategy,
        multi_linestring_tag, multi_polygon_tag,
        strategy_tag_distance_point_segment
    > : detail::distance::geometry_to_geometry_rtree
        <
            MultiLinestring, MultiPolygon, Strategy
        >
{};

template <typename MultiPolygon1, typename MultiPolygon2, typename Strategy>
struct distance_multi_to_multi
    <
        MultiPolygon1, MultiPolygon2, Strategy,
        multi_polygon_tag, multi_polygon_tag,
        strategy_tag_distance_point_segment
    > : detail::distance::geometry_to_geometry_rtree
        <
            MultiPolygon1, MultiPolygon2, Strategy
        >
{};

} // namespace splitted_dispatch

template
<
    typename MultiGeometry1,
    typename MultiGeometry2,
    typename Strategy,
    typename StrategyTag
>
struct distance
    <
        MultiGeometry1, MultiGeometry2, Strategy, multi_tag, multi_tag,
        StrategyTag, false
    > : splitted_dispatch::distance_multi_to_multi
        <
            MultiGeometry1, MultiGeometry2, Strategy,
            typename geometry::tag<MultiGeometry1>::type,
            typename geometry::tag<MultiGeometry2>::type,
            StrategyTag
        >
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_DISTANCE_MULTI_TO_MULTI_HPP


/**
 * \ file ConvolutionFilter.cpp
 */

#include <ATK/config.h>

#include <ATK/Mock/TriangleGeneratorFilter.h>
#include <ATK/Mock/TriangleCheckerFilter.h>

#include <ATK/Tools/VolumeFilter.h>
#include <ATK/Tools/SumFilter.h>

#include <ATK/EQ/CustomFIRFilter.h>
#include <ATK/EQ/IIRFilter.h>
#include <ATK/Special/ConvolutionFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#define PROCESSSIZE (2048)

// Original time (FIR time): 20s
// Split convolution: 7.4s
// fast FFT: 1s

BOOST_AUTO_TEST_CASE( ConvolutionFilter_ramp_test )
{
  const int impulse_size = 10000;
  const int split_size = 128;

  ATK::TriangleGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(48000);
  generator.set_amplitude(-1);
  generator.set_frequency(1000);
  generator.process(48000/1000/4);

  std::vector<double> impulse;
  for(int i = 1; i < impulse_size; ++i)
  {
    impulse.push_back(i / 10000.);
  }

  ATK::IIRFilter<ATK::CustomFIRCoefficients<double> > filter;
  filter.set_input_sampling_rate(48000);
  filter.set_coefficients_in(std::vector<double>(impulse.rbegin(), impulse.rend()));

  ATK::ConvolutionFilter<double> convolution;
  convolution.set_input_sampling_rate(48000);
  convolution.set_split_size(split_size);
  convolution.set_impulse(std::move(impulse));

  ATK::VolumeFilter<double> gainfilter;
  gainfilter.set_input_sampling_rate(48000);
  gainfilter.set_volume(-1);

  ATK::SumFilter<double> sumfilter;
  sumfilter.set_input_sampling_rate(48000);

  ATK::TriangleCheckerFilter<double> checker;
  checker.set_input_sampling_rate(48000);
  checker.set_amplitude(0);
  checker.set_frequency(1000);

  filter.set_input_port(0, &generator, 0);
  convolution.set_input_port(0, &generator, 0);

  gainfilter.set_input_port(0, &filter, 0);

  sumfilter.set_input_port(0, &convolution, 0);
  sumfilter.set_input_port(1, &gainfilter, 0);

  checker.set_input_port(0, &sumfilter, 0);

  for(int i= 1; i < PROCESSSIZE; ++i)
    checker.process(i);
}


/**
 * @file test_decentralization.cpp
 * @author Pierre-Yves Lajoie (lajoie.py@gmail.com)
 * @brief unit tests for decentralization of the multi robot case simulated by loading the datasets
 */
#include <distributed_mapper/run_distributed_mapper.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
#include <cmath>

using namespace std;
using namespace gtsam;
using namespace distributed_mapper;
using namespace evaluation_utils;

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

TEST(DistributedMapper, test_decentralized_distributed_estimation_without_outliers_2robots) {
    // Parameters

    // Call decentralized optimization

    // Check if the test worked
}

/****************************************************************************** */
int main() {
    TestResult tr;
    return TestRegistry::runAllTests(tr);
}
//******************************************************************************