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

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

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

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

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

// Last updated version of proj: 5.0.0

// Original copyright notice:

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

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

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

#ifndef BOOST_GEOMETRY_PROJECTIONS_BONNE_HPP
#define BOOST_GEOMETRY_PROJECTIONS_BONNE_HPP

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

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

#include <boost/math/special_functions/hypot.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace bonne
    {

            static const double epsilon10 = 1e-10;

            template <typename T>
            struct par_bonne
            {
                T phi1;
                T cphi1;
                T am1;
                T m1;
                detail::en<T> en;
            };

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

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

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

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

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

                    T s, rh;

                    rh = boost::math::hypot(xy_x, xy_y = this->m_proj_parm.am1 - xy_y);
                    lp_lat = pj_inv_mlfn(this->m_proj_parm.am1 + this->m_proj_parm.m1 - rh, this->m_par.es, this->m_proj_parm.en);
                    if ((s = fabs(lp_lat)) < half_pi) {
                        s = sin(lp_lat);
                        lp_lon = rh * atan2(xy_x, xy_y) *
                           sqrt(1. - this->m_par.es * s * s) / cos(lp_lat);
                    } else if (fabs(s - half_pi) <= epsilon10)
                        lp_lon = 0.;
                    else
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                }

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

            };

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

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

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    T E, rh;

                    rh = this->m_proj_parm.cphi1 + this->m_proj_parm.phi1 - lp_lat;
                    if (fabs(rh) > epsilon10) {
                        xy_x = rh * sin(E = lp_lon * cos(lp_lat) / rh);
                        xy_y = this->m_proj_parm.cphi1 - rh * cos(E);
                    } else
                        xy_x = xy_y = 0.;
                }

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

                    T rh;

                    rh = boost::math::hypot(xy_x, xy_y = this->m_proj_parm.cphi1 - xy_y);
                    lp_lat = this->m_proj_parm.cphi1 + this->m_proj_parm.phi1 - rh;
                    if (fabs(lp_lat) > half_pi) {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }
                    if (fabs(fabs(lp_lat) - half_pi) <= epsilon10)
                        lp_lon = 0.;
                    else
                        lp_lon = rh * atan2(xy_x, xy_y) / cos(lp_lat);
                }

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

            };

            // Bonne (Werner lat_1=90)
            template <typename Params, typename Parameters, typename T>
            inline void setup_bonne(Params const& params, Parameters& par, par_bonne<T>& proj_parm)
            {
                static const T half_pi = detail::half_pi<T>();

                T c;

                proj_parm.phi1 = pj_get_param_r<T, srs::spar::lat_1>(params, "lat_1", srs::dpar::lat_1);
                if (fabs(proj_parm.phi1) < epsilon10)
                    BOOST_THROW_EXCEPTION( projection_exception(error_lat1_is_zero) );

                if (par.es != 0.0) {
                    proj_parm.en = pj_enfn<T>(par.es);
                    proj_parm.m1 = pj_mlfn(proj_parm.phi1, proj_parm.am1 = sin(proj_parm.phi1),
                        c = cos(proj_parm.phi1), proj_parm.en);
                    proj_parm.am1 = c / (sqrt(1. - par.es * proj_parm.am1 * proj_parm.am1) * proj_parm.am1);
                } else {
                    if (fabs(proj_parm.phi1) + epsilon10 >= half_pi)
                        proj_parm.cphi1 = 0.;
                    else
                        proj_parm.cphi1 = 1. / tan(proj_parm.phi1);
                }
            }

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

    /*!
        \brief Bonne (Werner lat_1=90) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Conic
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_1: Latitude of first standard parallel (degrees)
        \par Example
        \image html ex_bonne.gif
    */
    template <typename T, typename Parameters>
    struct bonne_ellipsoid : public detail::bonne::base_bonne_ellipsoid<T, Parameters>
    {
        template <typename Params>
        inline bonne_ellipsoid(Params const& params, Parameters const& par)
            : detail::bonne::base_bonne_ellipsoid<T, Parameters>(par)
        {
            detail::bonne::setup_bonne(params, this->m_par, this->m_proj_parm);
        }
    };

    /*!
        \brief Bonne (Werner lat_1=90) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Conic
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_1: Latitude of first standard parallel (degrees)
        \par Example
        \image html ex_bonne.gif
    */
    template <typename T, typename Parameters>
    struct bonne_spheroid : public detail::bonne::base_bonne_spheroid<T, Parameters>
    {
        template <typename Params>
        inline bonne_spheroid(Params const& params, Parameters const& par)
            : detail::bonne::base_bonne_spheroid<T, Parameters>(par)
        {
            detail::bonne::setup_bonne(params, this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_bonne, bonne_spheroid, bonne_ellipsoid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_FI2(bonne_entry, bonne_spheroid, bonne_ellipsoid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(bonne_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(bonne, bonne_entry);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_BONNE_HPP


/****************************************************************************
** Copyright (C) 2019 TU Wien, ACIN, Vision 4 Robotics (V4R) group
** Contact: v4r.acin.tuwien.ac.at
**
** This file is part of V4R - EasyLabel
**
** V4R - EasyLabel is distributed under dual licenses - GPLv3 or closed source.
**
** GNU General Public License Usage
** V4R - EasyLabel is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published
** by the Free Software Foundation version 3.
**
** V4R - EasyLabel is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** Please review the following information to ensure the GNU General Public
** License requirements will be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
** Commercial License Usage
** If GPL is not suitable for your project, you must purchase a commercial
** license to use V4R - EasyLabel.
** Licensees holding valid commercial V4R - EasyLabel licenses may
** use this file in accordance with the commercial license agreement
** provided with the Software or, alternatively, in accordance with the
** terms contained in a written agreement between you and TU Wien, ACIN, V4R.
** For licensing terms and conditions please contact office<at>acin.tuwien.ac.at.
**
**
** The copyright holder additionally grants the author(s) of the file the right
** to use, copy, modify, merge, publish, distribute, sublicense, and/or
** sell copies of their contributions without any restrictions.
**
****************************************************************************/

/**
 * @file remove_proc.cpp
 * @author Markus Suchi (suchi@acin.tuwien.ac.at)
 * @date 2019
 * @brief : Commandline program to remove small patches from pointclouds,
 *          using Euclidian distance clustering.
 *
 */

#include <iostream>
#include <string>

#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/program_options.hpp>

typedef pcl::PointXYZRGBL PointT;

static int MIN_CLUSTER_SIZE = 5;
static float CLUSTER_TOLERANCE = 0.03f;

static void extractEuclidianDistanceCluster(const pcl::PointCloud<PointT>::Ptr cloud,
                                            pcl::PointCloud<PointT>::Ptr cloud_clustered) {
  // copy to output
  *cloud_clustered = *cloud;
  pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
  tree->setInputCloud(cloud_clustered);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<PointT> ec;
  ec.setClusterTolerance(CLUSTER_TOLERANCE);
  ec.setMinClusterSize(1);
  ec.setSearchMethod(tree);
  ec.setInputCloud(cloud_clustered);
  ec.extract(cluster_indices);

  int label = 100;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
    if (it->indices.size() <= MIN_CLUSTER_SIZE) {
      for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++) {
        if (!std::isnan(cloud_clustered->points[*pit].z)) {
          cloud_clustered->points[*pit].label = label;
        }
      }
    }
  }
}

static void maskPointCloud(const pcl::PointCloud<PointT>::Ptr input, const std::vector<int> &indices,
                           pcl::PointCloud<PointT>::Ptr result) {
  const float init_value = std::numeric_limits<float>::quiet_NaN();
  PointT init_point;
  init_point.x = init_point.y = init_point.z = init_value;

  result->is_dense = input->is_dense;
  result->height = input->height;
  result->width = input->width;
  result->points.resize(input->width * input->height, init_point);

  for (std::vector<int>::const_iterator it = indices.begin(); it != indices.end(); ++it) {
    result->points[*it] = input->points[*it];
  }
}

static void filterCloudByLabel(const uint32_t label, const pcl::PointCloud<PointT>::Ptr input) {
  for (size_t iindex = 0; iindex < input->size(); iindex++) {
    if (input->points[iindex].label == label) {
      input->points[iindex].label = 0;
      input->points[iindex].x = std::numeric_limits<float>::quiet_NaN();
      input->points[iindex].y = std::numeric_limits<float>::quiet_NaN();
      input->points[iindex].z = std::numeric_limits<float>::quiet_NaN();
    }
  }
}

int main(int argc, char *argv[]) {
  namespace po = boost::program_options;

  std::string source_file("");
  std::string output_file("");
  bool view = false;

  po::options_description desc(
      "el_remove: Remove small patches from pcd files"
      "\n=============================================="
      "\n**Allowed options");
  desc.add_options()("help,h", "produce help message")(
      "source_file,s", po::value<std::string>(&source_file)->required(), "source pcd file")(
      "target_file,t", po::value<std::string>(&output_file)->required(), "File with filtered point cloud.")(
      "view_results,v", po::bool_switch(&view), "View results.")(
      "cluster_tolerance,c",
      po::value<float>(&CLUSTER_TOLERANCE)->default_value(CLUSTER_TOLERANCE)->implicit_value(CLUSTER_TOLERANCE),
      "Cluster tolerance.")(
      "cluster_min_size,u",
      po::value<int>(&MIN_CLUSTER_SIZE)->default_value(MIN_CLUSTER_SIZE)->implicit_value(MIN_CLUSTER_SIZE),
      "Minimal cluster size.");

  po::variables_map vm;
  po::parsed_options parsed = po::command_line_parser(argc, argv).options(desc).allow_unregistered().run();

  std::vector<std::string> to_pass_further = po::collect_unrecognized(parsed.options, po::include_positional);
  po::store(parsed, vm);
  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return 0;
  }
  try {
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl;
    return 0;
  }

  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
  pcl::PointCloud<PointT>::Ptr result_clustered(new pcl::PointCloud<PointT>);

  std::cout << source_file << std::endl;
  pcl::io::loadPCDFile(source_file, *cloud);
  extractEuclidianDistanceCluster(cloud, result_clustered);

  // show result
  if (view) {
    pcl::visualization::PCLVisualizer viewer = pcl::visualization::PCLVisualizer("PCL Viewer");
    // differ result vis
    int v1(0);
    viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
    // cluster result vis
    int v2(0);
    viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);

    pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
    pcl::visualization::PointCloudColorHandlerGenericField<PointT> labelvis(result_clustered, "label");

    while (!viewer.wasStopped()) {
      if (!viewer.updatePointCloud<PointT>(cloud, rgb, "result")) {
        viewer.addPointCloud<PointT>(cloud, rgb, "result", v1);
      }
      if (!viewer.updatePointCloud<PointT>(result_clustered, labelvis, "template")) {
        viewer.addPointCloud<PointT>(result_clustered, labelvis, "template", v2);
      }
      usleep(100);
      viewer.spinOnce();
    }
  }
  // copy the clustered cloud and filter by label
  pcl::PointCloud<PointT>::Ptr result_filtered(new pcl::PointCloud<PointT>);
  *result_filtered = *result_clustered;
  filterCloudByLabel(100, result_filtered);

  // copy for rgb output
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr result_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
  copyPointCloud(*result_filtered, *result_rgb);

  // save labeled pointcloud frame
  if (output_file != "") {
    std::cout << "saving " << output_file << std::endl;
    pcl::io::savePCDFile(output_file, *result_rgb, true);
  }
  return 0;
}


#include <pcl/common/common_headers.h>
#include <pcl/console/parse.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <boost/thread/thread.hpp>
#include <iostream>

#define pi 3.1415926
void showCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,
               pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2) {
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
      new pcl::visualization::PCLVisualizer("3D Viewer"));
  int v1(0);
  viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
  viewer->setBackgroundColor(0, 0, 0, v1);
  viewer->addPointCloud<pcl::PointXYZ>(cloud1, "sample cloud1", v1);

  int v2(0);
  viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
  viewer->addPointCloud<pcl::PointXYZ>(cloud2, "sample cloud2", v2);
  viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);
  viewer->addCoordinateSystem(1.0);

  viewer->initCameraParameters();
  while (!viewer->wasStopped()) {
    viewer->spinOnce(100);
  };
}
int main(int argc, char** argv) {
  pcl::PointCloud<pcl::PointXYZ> cloud;

  cloud.width = 2500;  // number of points from the cylinder

  cloud.height = 1;
  cloud.is_dense = false;
  cloud.points.resize(cloud.width * cloud.height);

  size_t i = 0;

  // number of points from the cylinder = number of points fron a circle plane *
  // number of planes (number of points on the Z-axis)

  for (int j = 0; j <= 10 * pi; j++)

    for (int k = 0; k <= 30; k++)

    {
      cloud.points[i].x = cosf((float)j / 5) * 0.0075 + 0.1;
      cloud.points[i].y = sinf((float)j / 5) * 0.0075 + 0.1;
      cloud.points[i].z = (float)k * 5 / 3000 + 0.1;

      i++;
    }

  // number of points from the two planes of the cylinder
  for (int l = 0; l <= 10 * pi; l++)
    for (int m = 0; m <= 10; m++) {
      cloud.points[i].x = m * 0.00075 * cosf((float)l / 5) + 0.1;
      cloud.points[i].y = m * 0.00075 * sinf((float)l / 5) + 0.1;
      cloud.points[i].z = 0.1;

      i++;
    }
  for (int l = 0; l <= 10 * pi; l++)
    for (int m = 0; m <= 10; m++) {
      cloud.points[i].x = m * 0.00075 * cosf((float)l / 5) + 0.1;
      cloud.points[i].y = m * 0.00075 * sinf((float)l / 5) + 0.1;
      cloud.points[i].z = 0.15;

      i++;
    }

  pcl::io::savePCDFileASCII(
      "/home/gjx/orbslam/catkin_ws/src/ZJUBinPicking/pcd_files/cylinder.pcd",
      cloud);  // save the data to a PCD file

  pcl::PointCloud<pcl::PointXYZ>* ptr = &cloud;
  pcl::PointCloud<pcl::PointXYZ>::Ptr mycloud(ptr);

  showCloud(mycloud, mycloud);

  return (0);
}


#include "MeshGraph.h"
#include <geogram/basic/common.h>
#include <geogram/mesh/mesh_repair.h>
#include <geogram/basic/logger.h>
#include <geogram/basic/attributes.h>

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

namespace OasisLib
{

    MeshHeightSlicer::MeshHeightSlicer(sh_mesh_ptr m_ptr)
    {
        Logger::div("Init MeshHeightSlicer");
        this->mesh_ptr = m_ptr;
        Logger::out("MeshHeightSlicer") << "Input mesh has " \
                                        << this->mesh_ptr->facets.nb() << " facets," << \
                                        this->mesh_ptr->vertices.nb() << " vertices" \
                                        << std::endl;

        check_mesh();
        this->initialize_rtree();
        this->nb_nodes = this->mesh_tree.size();
    }

    void MeshHeightSlicer::check_mesh()
    {
        try
        {
            if (this->mesh_ptr->cells.nb() == 0)
            {
                Attribute<int> cell_id(this->mesh_ptr->facets.attributes(), "cell_id");
                auto c_vector = cell_id.get_vector();
                std::set<int> counter(c_vector.cbegin(), c_vector.cend());
                Logger::out("MeshHeightSlicer") << counter.size() << " cells" << std::endl;

                if (counter.size() == 0)
                {
                    throw 0;
                }
            }
        }
        catch (int e)
        {
            Logger::err("MeshHeightSlicer") << "input mesh has no cells" << std::endl;
        }
    }

    void MeshHeightSlicer::initialize_rtree()
    {
        Attribute<int> cell_id(this->mesh_ptr->facets.attributes(), "cell_id");
        index_t nb_f = this->mesh_ptr->facets.nb();
        for (index_t f = 0; f < nb_f; ++f)
        {
            auto c = cell_id[f];
            auto c_begin = this -> mesh_ptr -> facets.corners_begin(f);
            auto c_end = this -> mesh_ptr -> facets.corners_end(f);
            for (auto c_i = c_begin; c_i != c_end; ++c_i)
            {
                pt min_p;
                pt max_p;
                auto p1_id = this->mesh_ptr->facet_corners.vertex(c_i);
                auto c_i_1 = this->mesh_ptr->facets.next_corner_around_facet(f, c_i);
                auto p2_id = this->mesh_ptr->facet_corners.vertex(c_i_1);

                auto p_1_pos = this->mesh_ptr->vertices.point(p1_id);
                auto p_2_pos = this->mesh_ptr->vertices.point(p2_id);

                for (int i = 0; i < 3; ++i)
                {
                    if (p_1_pos[i] < p_2_pos[i])
                    {
                        min_p[i] = p_1_pos[i];
                        max_p[i] = p_2_pos[i];
                    }
                    else
                    {
                        min_p[i] = p_2_pos[i];
                        max_p[i] = p_1_pos[i];
                    }
                }
                c_range bx(min_p, max_p);
                std::array<index_t, 3> id = {(index_t)c, p1_id, p2_id};
                this->mesh_tree.insert(std::make_pair(bx, id));
            }
        }
    }

    void MeshHeightSlicer::get_layer(Mesh &m_out, double z)
    {
        std::vector<value> results;
        auto bds = this->mesh_tree.bounds();
        auto min_v = bds.min_corner();
        auto max_v = bds.max_corner();
        double rel_z = min_v.get<2>() + z;
        pt l_pt({min_v.get<0>(), min_v.get<1>(), rel_z - 0.1});
        pt m_pt({max_v.get<0>(), max_v.get<1>(), rel_z + 0.1});

        c_range bbx(l_pt, m_pt);

        this->mesh_tree.query(bgi::intersects(bbx), std::back_inserter(results));

        Logger::out("MeshHeightSlicer") << "computed " << results.size() << " intersection" << std::endl;

        this->clip_cell(results, m_out, rel_z);
    }

    void MeshHeightSlicer::clip_cell(std::vector<value> &target_facet, Mesh &m_out, double z)
    {
        std::map<index_t, mpt2d> pt_accumulator;
        for (auto it = target_facet.cbegin(); it != target_facet.cend(); ++it)
        {
            auto box = it -> first;
            auto id = it->second;
            auto c_id = id[0];

            auto c_it = pt_accumulator.find(c_id);

            if (c_it == pt_accumulator.cend())
            {
                pt_accumulator.insert({c_id, mpt2d()});
            }

            auto p_1 = this->mesh_ptr->vertices.point(id[1]);
            auto p_2 = this->mesh_ptr->vertices.point(id[2]);

            auto p_1_diff = p_1[2] - z;
            auto p_2_diff = p_2[2] - z;

            auto c_1_sign = geo_sgn(p_1_diff);
            auto c_2_sign = geo_sgn(p_2_diff);

            if (c_1_sign != c_2_sign && (abs(p_1_diff) >= 1e-4 && abs(p_1_diff) >= 1e-4))
            {
                pt2d inter_p2d;
                double d = (z - p_1[2]) / (p_2[2]- p_1[2]);
                auto inter_p = p_1 + d * (p_2 - p_1);

                inter_p2d[0] = inter_p[0];
                inter_p2d[1] = inter_p[1];
                bg::append(pt_accumulator[c_id], inter_p2d);
            }
            else
            {
                if (abs(p_1_diff) < 1e-4)
                {
                    pt2d inter_p2d;
                    inter_p2d[0] = p_1[0];
                    inter_p2d[1] = p_1[1];
                    bg::append(pt_accumulator[c_id], inter_p2d);
                }

                if (abs(p_1_diff) < 1e-4)
                {
                    pt2d inter_p2d;
                    inter_p2d[0] = p_2[0];
                    inter_p2d[1] = p_2[1];
                    bg::append(pt_accumulator[c_id], inter_p2d);
                }
            }
        }

        for (auto c_it = pt_accumulator.cbegin(); c_it != pt_accumulator.cend(); ++c_it)
        {
            auto c_id = c_it->first;
            auto m_pt = c_it->second;

            auto count = bg::num_points(m_pt);
            if(count >= 3){
                contour2d hull;
                bg::convex_hull(m_pt, hull);

                std::vector<index_t> added_id;

                for (auto p_it = std::cbegin(hull); p_it != std::cend(hull); ++p_it)
                {
                    pt a_v;
                    auto hull_pt = *p_it;
                    a_v[0] = hull_pt[0];
                    a_v[1] = hull_pt[1];
                    a_v[2] = z;
                    auto a_id = m_out.vertices.create_vertex(a_v.data());
                    added_id.push_back(a_id);
                }

                auto f_id = m_out.facets.create_polygon(added_id.size(), &added_id[0]);
            }
        }
        mesh_repair(m_out, MESH_REPAIR_COLOCATE, 1e-4);
        m_out.facets.connect();
    }

}


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * 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 Autonomous Systems Lab, ETH Zurich 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 Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm 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.
 *
*/

#ifndef KINDR_ROTATIONS_EIGEN_LOCALANGULARVELOCITY_HPP_
#define KINDR_ROTATIONS_EIGEN_LOCALANGULARVELOCITY_HPP_

#include <Eigen/Core>

#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/rotations/RotationDiffBase.hpp"
#include "kindr/rotations/RotationEigen.hpp"
#include "kindr/quaternions/QuaternionEigen.hpp"
#include "kindr/phys_quant/eigen/AngularVelocity.hpp"
#include "kindr/linear_algebra/LinearAlgebra.hpp"

namespace kindr {
namespace rotations {
namespace eigen_impl {

/*! \class LocalAngularVelocity
 * \brief Angular velocity in 3D-space expressed in local coordinates (frame fixed to the body).
 *
 * This class implements an angular velocity of a rigid body in 3D-space expressed in body fixed (local) frame.
 *
 * Note that only the version with the active usage type makes sense to represent a physical angular velocity of a body.
 * The angular velocity should represent the absolute rotational velocity of a rigid body with respect to an inertial (global) frame I
 * and its coordinates are expressed in the body fixed (local) frame (\f$\B_Omega = B_\omega_{IB}$\f)
 *
 * \tparam PrimType_  Primitive type of the coordinates.
 * \ingroup rotations
 */
template<typename PrimType_, enum RotationUsage Usage_>
class LocalAngularVelocity : public LocalAngularVelocityBase<LocalAngularVelocity<PrimType_, Usage_>, Usage_>, public phys_quant::eigen_impl::AngularVelocity<PrimType_, 3> {
 private:
  /*! \brief The base type.
   */
  typedef phys_quant::eigen_impl::AngularVelocity<PrimType_, 3> Base;
 public:
  /*! \brief The implementation type.
   *
   *  The implementation type is always an Eigen object.
   */
  typedef typename Base::Implementation Implementation;

  /*! \brief The primitive type of the velocities.
   */
  typedef PrimType_ Scalar;

  /*! \brief Default constructor initializes all velocities with zero.
   */
  LocalAngularVelocity()
    : Base(Base::Zero()) {
  }

  /*! Constructor with three components (x,y,z)
   * \param x   x-coordinate expressed in body fixed (local) frame
   * \param y   y-coordinate expressed in body fixed (local) frame
   * \param z   z-coordinate expressed in body fixed (local) frame
   */
  LocalAngularVelocity(Scalar x, Scalar y, Scalar z)
    : Base(x, y, z) {
  }

  /*! \brief Constructor using vector class.
   *  \param other   phys_quant::eigen_impl::AngularVelocity<PrimType_, 3>
   */
  explicit LocalAngularVelocity(const Base& other)
    : Base(other) {
   }

  /*! Constructor with three components (x,y,z)
   * \param vector  Eigen::Matrix<Scalar,3,1>
   */
  LocalAngularVelocity(const Implementation & vector)
    : Base(vector) {
  }

  /*! Constructor with a time derivative of a rotation with a different parameterization
   *
   * \param rotation    rotation the time derivative is taken at
   * \param other       other time derivative
   */
  template<typename RotationDerived_, enum RotationUsage RotationUsage_, typename OtherDerived_, enum RotationUsage OtherUsage_>
  inline explicit LocalAngularVelocity(const RotationBase<RotationDerived_, RotationUsage_>& rotation, const RotationDiffBase<OtherDerived_, OtherUsage_>& other)
    : Base(internal::RotationDiffConversionTraits<LocalAngularVelocity, OtherDerived_, RotationDerived_>::convert(rotation.derived(), other.derived())){
  }

  inline Implementation vector() const {
    return Base::toImplementation();
  }

  inline Implementation& vector() {
    return Base::toImplementation();
  }

  inline Implementation& toImplementation() {
    return Base::toImplementation();
  }

  inline const Implementation& toImplementation() const {
    return Base::toImplementation();
  }

  /*! \brief Cast to the base type.
   *  \returns the base (recommended only for advanced users)
   */
  inline Base& toBase() {
    return static_cast<Base&>(*this);
  }

  /*! \brief Cast to the base type.
   *  \returns the base (recommended only for advanced users)
   */
  inline const Base& toBase() const {
    return static_cast<const Base&>(*this);
  }

  /*! \brief Addition of two angular velocities.
   */
  template<typename Other_>
  LocalAngularVelocity operator +(const Other_& other) const {
    return LocalAngularVelocity(this->toBase() + other.toBase());
  }

  /*! \brief Subtraction of two angular velocities.
   */
  template<typename Other_>
  LocalAngularVelocity operator -(const Other_& other) const {
    return LocalAngularVelocity(this->toBase() - other.toBase());
  }

  /*! \brief Multiplication of an angular velocity with a scalar.
   */
  template<typename PrimTypeFactor_>
  LocalAngularVelocity operator *(const PrimTypeFactor_& factor) const {
    return LocalAngularVelocity(this->toBase()*factor);
  }

  /*! \brief Addition of two angular velocities.
   * \param other   other angular velocity
   */
  template<typename Other_>
  LocalAngularVelocity& operator +=(const Other_& other) {
    this->toBase() += other.toBase();
    return *this;
  }

  /*! \brief Subtraction of two angular velocities.
   * \param other   other angular velocity
   */
  template<typename Other_>
  LocalAngularVelocity& operator -=(const Other_& other) {
    this->toBase() -= other.toBase();
    return *this;
  }

  /*! \brief Sets all components of the angular velocity to zero.
   * \returns reference
   */
  LocalAngularVelocity& setZero() {
    Base::setZero();
    return *this;
  }
};

/*! \brief Multiplication of an angular velocity with a scalar.
 */
template<typename PrimType_, enum RotationUsage Usage_, typename PrimTypeFactor_>
LocalAngularVelocity<PrimType_, Usage_> operator *(PrimTypeFactor_ factor, const LocalAngularVelocity<PrimType_, Usage_>& localAngularVelocity) {
  return LocalAngularVelocity<PrimType_, Usage_>(localAngularVelocity.toBase()*factor);
}

//! \brief 3D angular velocity with primitive type double
typedef LocalAngularVelocity<double, RotationUsage::PASSIVE>  LocalAngularVelocityPD;
//! \brief 3D angular velocity with primitive type float
typedef LocalAngularVelocity<float, RotationUsage::PASSIVE>  LocalAngularVelocityPF;
//! \brief 3D angular velocity with primitive type double
typedef LocalAngularVelocity<double, RotationUsage::ACTIVE>  LocalAngularVelocityAD;
//! \brief 3D angular velocity with primitive type float
typedef LocalAngularVelocity<float, RotationUsage::ACTIVE>  LocalAngularVelocityAF;

} // namespace eigen_impl

namespace internal {

/* -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
 * Conversion Traits
 * ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- */

template<typename PrimType_, enum RotationUsage Usage_>
class RotationDiffConversionTraits<eigen_impl::LocalAngularVelocity<PrimType_, Usage_>, eigen_impl::RotationQuaternionDiff<PrimType_, Usage_>, eigen_impl::RotationQuaternion<PrimType_, Usage_>> {
 public:
  inline static eigen_impl::LocalAngularVelocity<PrimType_, Usage_> convert(const eigen_impl::RotationQuaternion<PrimType_, Usage_>& rquat, const eigen_impl::RotationQuaternionDiff<PrimType_, Usage_>& rquatdiff) {
    return eigen_impl::LocalAngularVelocity<PrimType_, Usage_>(2.0*rquat.getLocalQuaternionDiffMatrix()*rquatdiff.toQuaternion().vector());
  }
};

//! \f$B_\hat{w}_IB = (R_IB*dR_IB')$\f
template<typename PrimType_, enum RotationUsage Usage_>
class RotationDiffConversionTraits<eigen_impl::LocalAngularVelocity<PrimType_, Usage_>, eigen_impl::RotationMatrixDiff<PrimType_, Usage_>, eigen_impl::RotationMatrix<PrimType_, Usage_>> {
 public:
  inline static eigen_impl::LocalAngularVelocity<PrimType_, Usage_> convert(const eigen_impl::RotationMatrix<PrimType_, Usage_>& rotationMatrix, const eigen_impl::RotationMatrixDiff<PrimType_, Usage_>& rotationMatrixDiff) {
//    return eigen_impl::LocalAngularVelocity<PrimType_, Usage_>(linear_algebra::getVectorFromSkewMatrix<PrimType_>(rotationMatrixDiff.toImplementation()*rotationMatrix.toImplementation().transpose()));

    if (Usage_ == RotationUsage::ACTIVE) {
      return eigen_impl::LocalAngularVelocity<PrimType_, Usage_>(linear_algebra::getVectorFromSkewMatrix<PrimType_>(rotationMatrixDiff.toImplementation()*rotationMatrix.toImplementation().transpose()));
    }
    if (Usage_ == RotationUsage::PASSIVE) {
      return eigen_impl::LocalAngularVelocity<PrimType_, Usage_>(linear_algebra::getVectorFromSkewMatrix<PrimType_>(rotationMatrix.toImplementation().transpose()*rotationMatrixDiff.toImplementation()));
    }

  }
};

//
//template<typename PrimType_, enum RotationUsage Usage_>
//class RotationDiffConversionTraits<eigen_impl::LocalAngularVelocity<PrimType_, Usage_>, eigen_impl::RotationVectorDiff<PrimType_, Usage_>, eigen_impl::RotationVector<PrimType_, Usage_>> {
// public:
//  inline static eigen_impl::LocalAngularVelocity<PrimType_, Usage_> convert(const eigen_impl::RotationVector<PrimType_, Usage_>& rotationVector, const eigen_impl::RotationVectorDiff<PrimType_, Usage_>& rotationVectorDiff) {
//    typedef typename eigen_impl::RotationVector<PrimType_, Usage_>::Implementation Vector;
//    typedef PrimType_ Scalar;
//    typedef typename Eigen::Matrix<PrimType_, 3, 3> Matrix3x3;
//
//
//    // not tested:
////    const Vector rv = rotationVector.toImplementation();
////    const Vector rvDiff = rotationVectorDiff.toImplementation();
////    const Matrix3x3 rv_hat = linear_algebra::getSkewMatrixFromVector(rv);
////    const Scalar angle = rv.norm();
////    const Vector angularVelocity = rvDiff-rv_hat*rvDiff*(1-cos(angle)/(angle*angle)) + rv_hat*rv_hat*rvDiff*((angle-sin(angle))/(angle*angle*angle));
////    return eigen_impl::LocalAngularVelocity<PrimType_, Usage_>(angularVelocity);
//
//    const PrimType_ v = rotationVector.vector().norm();
//    const PrimType_ v1 = rotationVector.x();
//    const PrimType_ v2 = rotationVector.y();
//    const PrimType_ v3 = rotationVector.z();
//    const PrimType_ dv1 = rotationVectorDiff.x();
//    const PrimType_ dv2 = rotationVectorDiff.y();
//    const PrimType_ dv3 = rotationVectorDiff.z();
//
//
//    if (v < common::internal::NumTraits<Scalar>::dummy_precision()) {
//      // small angle
//      const PrimType_ t2 = v3*(1.0/2.0);
//      const PrimType_ t3 = v1*v2*(1.0/4.0);
//      const PrimType_ t4 = v2*(1.0/2.0);
//      const PrimType_ t5 = v1*(1.0/2.0);
//      const PrimType_ t6 = v2*v3*(1.0/4.0);
//      const PrimType_ w1 = dv2*(t2+t3)+dv1*((v1*v1)*(1.0/4.0)+1.0)-dv3*(t4-v1*v3*(1.0/4.0));
//      const PrimType_ w2 = dv3*(t5+t6)+dv2*((v2*v2)*(1.0/4.0)+1.0)-dv1*(t2-t3);
//      const PrimType_ w3 = dv3*((v3*v3)*(1.0/4.0)+1.0)-dv2*(t5-t6)+dv1*(t4+v1*v3*(1.0/4.0));
//      return eigen_impl::LocalAngularVelocity<PrimType_, Usage_>(w1, w2, w3);
////      if (Usage_ == RotationUsage::ACTIVE) {
////        return eigen_impl::LocalAngularVelocity<PrimType_, Usage_>(-w1, -w2, -w3);
////      }
////      if (Usage_ == RotationUsage::PASSIVE) {
////        return eigen_impl::LocalAngularVelocity<PrimType_, Usage_>(w1, w2, w3);
////      }
//    }
//
//    const PrimType_ t2 = 1.0/(v*v*v);
//    const PrimType_ t3 = cos(v);
//    const PrimType_ t4 = sin(v);
//    const PrimType_ t5 = v1*v1;
//    const PrimType_ t6 = v1*v2;
//    const PrimType_ t7 = v*v;
//    const PrimType_ t8 = t4*t7;
//    const PrimType_ t9 = v2*v2;
//    const PrimType_ t10 = v2*v3;
//    const PrimType_ t11 = v1*v3;
//    const PrimType_ t12 = t3*v2;
//    const PrimType_ t13 = v3*v3;
//    const PrimType_ w1 = dv3*t2*(v*(t11+t12-v2)-t4*v1*v3)+dv1*t2*(t8-t4*t5+t5*v)+dv2*t2*(v*(t6+v3-t3*v3)-t4*v1*v2);
//    const PrimType_ w2 = dv1*t2*(v*(t6-v3+t3*v3)-t4*v1*v2)+dv2*t2*(t8-t4*t9+t9*v)+dv3*t2*(v*(t10+v1-t3*v1)-t4*v2*v3);
//    const PrimType_ w3 = dv2*t2*(v*(t10-v1+t3*v1)-t4*v2*v3)+dv1*t2*(v*(t11-t12+v2)-t4*v1*v3)+dv3*t2*(t8-t4*t13+t13*v);
//
//    return eigen_impl::LocalAngularVelocity<PrimType_, Usage_>(w1, w2, w3);
//
////    if (Usage_ == RotationUsage::ACTIVE) {
////      return eigen_impl::LocalAngularVelocity<PrimType_, Usage_>(-w1, -w2, -w3);
////    }
////    if (Usage_ == RotationUsage::PASSIVE) {
////      return eigen_impl::LocalAngularVelocity<PrimType_, Usage_>(w1, w2, w3);
////    }
//
//  }
//};

template<typename PrimType_, enum RotationUsage Usage_>
class RotationDiffConversionTraits<eigen_impl::LocalAngularVelocity<PrimType_, Usage_>, eigen_impl::EulerAnglesZyxDiff<PrimType_, Usage_>, eigen_impl::EulerAnglesZyx<PrimType_, Usage_>> {
 public:
  inline static eigen_impl::LocalAngularVelocity<PrimType_, Usage_> convert(const eigen_impl::EulerAnglesZyx<PrimType_, Usage_>& eulerAngles, const eigen_impl::EulerAnglesZyxDiff<PrimType_, Usage_>& eulerAnglesDiff) {
    const PrimType_ phi = eulerAngles.roll();
    const PrimType_ theta = eulerAngles.pitch();
    const PrimType_ dphi = eulerAnglesDiff.roll();
    const PrimType_ dtheta = eulerAnglesDiff.pitch();
    const PrimType_ dpsi = eulerAnglesDiff.yaw();
    const PrimType_ t2 = sin(phi);
    const PrimType_ t3 = cos(phi);
    const PrimType_ t4 = cos(theta);
    return eigen_impl::LocalAngularVelocity<PrimType_, Usage_>(dphi-dpsi*sin(theta), dtheta*t3+dpsi*t2*t4, -dtheta*t2+dpsi*t3*t4);
  }
};

template<typename PrimType_, enum RotationUsage Usage_>
class RotationDiffConversionTraits<eigen_impl::LocalAngularVelocity<PrimType_, Usage_>, eigen_impl::EulerAnglesXyzDiff<PrimType_, Usage_>, eigen_impl::EulerAnglesXyz<PrimType_, Usage_>> {
 public:
  inline static eigen_impl::LocalAngularVelocity<PrimType_, Usage_> convert(const eigen_impl::EulerAnglesXyz<PrimType_, Usage_>& eulerAngles, const eigen_impl::EulerAnglesXyzDiff<PrimType_, Usage_>& eulerAnglesDiff) {
    const PrimType_ beta = eulerAngles.pitch();
    const PrimType_ gamma = eulerAngles.yaw();
    const PrimType_ dalpha = eulerAnglesDiff.roll();
    const PrimType_ dbeta = eulerAnglesDiff.pitch();
    const PrimType_ dgamma = eulerAnglesDiff.yaw();
    const PrimType_ t2 = cos(gamma);
    const PrimType_ t3 = cos(beta);
    const PrimType_ t4 = sin(gamma);
    return eigen_impl::LocalAngularVelocity<PrimType_, Usage_>(dbeta*t4+dalpha*t2*t3, dbeta*t2-dalpha*t3*t4, dgamma+dalpha*sin(beta));
  }
};

} // namespace internal
} // namespace rotations
} // namespace kindr

#endif /* KINDR_ROTATIONS_EIGEN_LOCALANGULARVELOCITY_HPP_ */


/**
 * @brief 遅延レンダリングのテストシーン
 */

#include "SceneDeferred.h"

#include <boost/assert.hpp>
#include <iostream>

#include <glm/gtc/constants.hpp>
#include <glm/gtc/matrix_transform.hpp>

// ********************************************************************************
// Override functions
// ********************************************************************************

void SceneDeferred::OnInit() {
  if (const auto msg = CompileAndLinkShader()) {
    std::cerr << msg.value() << std::endl;
    BOOST_ASSERT_MSG(false, "failed to compile or link!");
  } else {
    prog_.Use();
    prog_.SetUniform("Light.Ld", glm::vec3(1.0f));
    prog_.SetUniform("PositionTex", 0);
    prog_.SetUniform("NormalTex", 1);
    prog_.SetUniform("ColorTex", 2);
  }

  // 四角形ポリゴン用配列
  GLfloat verts[] = {-1.0f, -1.0f, 0.0f, 1.0f, -1.0f, 0.0f, 1.0f,  1.0f, 0.0f,
                     -1.0f, -1.0f, 0.0f, 1.0f, 1.0f,  0.0f, -1.0f, 1.0f, 0.0f};
  GLfloat tc[] = {0.0f, 0.0f, 1.0f, 0.0f, 1.0f, 1.0f,
                  0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 1.0f};

  glGenBuffers(vbo_.size(), vbo_.data());

  glBindBuffer(GL_ARRAY_BUFFER, vbo_[0]);
  glBufferData(GL_ARRAY_BUFFER, 6 * 3 * sizeof(float), verts, GL_STATIC_DRAW);

  glBindBuffer(GL_ARRAY_BUFFER, vbo_[1]);
  glBufferData(GL_ARRAY_BUFFER, 6 * 2 * sizeof(float), tc, GL_STATIC_DRAW);

  // 頂点配列オブジェクトの設定
  glGenVertexArrays(1, &quad_);
  glBindVertexArray(quad_);

  glBindBuffer(GL_ARRAY_BUFFER, vbo_[0]);
  glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, nullptr);
  glEnableVertexAttribArray(0); // VertexPosition

  glBindBuffer(GL_ARRAY_BUFFER, vbo_[1]);
  glVertexAttribPointer(2, 2, GL_FLOAT, GL_FALSE, 0, nullptr);
  glEnableVertexAttribArray(2); // TexCoord

  glBindVertexArray(0);

  gbuffer_.OnInit(width_, height_);

  glClearColor(0.1f, 0.1f, 0.1f, 1.0f);
}

void SceneDeferred::OnDestroy() {
  glDeleteVertexArrays(1, &quad_);
  glDeleteBuffers(vbo_.size(), vbo_.data());
}

void SceneDeferred::OnUpdate(float t) {
  const float deltaT = tPrev_ == 0.0f ? 0.0f : t - tPrev_;
  tPrev_ = t;

  angle_ += rotSpeed_ * deltaT;
  if (angle_ > glm::two_pi<float>()) {
    angle_ -= glm::two_pi<float>();
  }
}

void SceneDeferred::OnRender() {
  Pass1();
  Pass2();
}

void SceneDeferred::OnResize(int w, int h) {
  SetDimensions(w, h);
  glViewport(0, 0, w, h);
}

// ********************************************************************************
// Member functions
// ********************************************************************************

void SceneDeferred::SetMatrices() {
  const glm::mat4 mv = view_ * model_;
  prog_.SetUniform("ModelViewMatrix", mv);
  prog_.SetUniform("NormalMatrix", glm::mat3(glm::vec3(mv[0]), glm::vec3(mv[1]),
                                             glm::vec3(mv[2])));
  prog_.SetUniform("MVP", proj_ * mv);
}

std::optional<std::string> SceneDeferred::CompileAndLinkShader() {
  // Compile and links
  return prog_.CompileAndLink(
      {{"./Assets/Shaders/Deferred/Deferred.vs.glsl", ShaderType::Vertex},
       {"./Assets/Shaders/Deferred/Deferred.fs.glsl", ShaderType::Fragment}});
}

void SceneDeferred::Pass1() {
  prog_.SetUniform("Pass", 1);

  glBindFramebuffer(GL_FRAMEBUFFER, gbuffer_.GetDeferredFBO());
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glEnable(GL_DEPTH_TEST);

  view_ = glm::lookAt(glm::vec3(7.0f * cos(angle_), 4.0f, 7.0f * sin(angle_)),
                      glm::vec3(0.0f, 0.0f, 0.0f), glm::vec3(0.0f, 1.0f, 0.0f));
  proj_ = glm::perspective(
      glm::radians(60.0f),
      static_cast<float>(width_) / static_cast<float>(height_), 0.3f, 100.0f);

  // ティーポットの描画
  prog_.SetUniform("Light.Position", glm::vec4(0.0f, 0.0f, 0.0f, 1.0f));
  prog_.SetUniform("Material.Kd", 0.9f, 0.9f, 0.9f);
  model_ = glm::mat4(1.0f);
  model_ = glm::translate(model_, glm::vec3(0.0f, 0.0f, 0.0f));
  model_ =
      glm::rotate(model_, glm::radians(-90.0f), glm::vec3(1.0f, 0.0f, 0.0f));
  SetMatrices();
  teapot_.Render();

  // 平面の描画
  prog_.SetUniform("Material.Kd", 0.4f, 0.4f, 0.4f);
  model_ = glm::mat4(1.0f);
  model_ = glm::translate(model_, glm::vec3(0.0f, -0.75f, 0.0f));
  SetMatrices();
  plane_.Render();

  // トーラスの描画
  prog_.SetUniform("Light.Position", glm::vec4(0.0f, 0.0f, 0.0f, 1.0f));
  prog_.SetUniform("Material.Kd", 0.9f, 0.5f, 0.2f);
  model_ = glm::mat4(1.0f);
  model_ = glm::translate(model_, glm::vec3(3.0f, 1.0f, 3.0f));
  model_ =
      glm::rotate(model_, glm::radians(90.0f), glm::vec3(1.0f, 0.0f, 0.0f));
  SetMatrices();
  torus_.Render();

  glFlush();
}

void SceneDeferred::Pass2() {
  prog_.SetUniform("Pass", 2);

  // デフォルトのフレームバッファに戻します
  glBindFramebuffer(GL_FRAMEBUFFER, 0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glDisable(GL_DEPTH_TEST);

  view_ = glm::mat4(1.0f);
  proj_ = glm::mat4(1.0f);
  model_ = glm::mat4(1.0f);
  SetMatrices();
  gbuffer_.OnPreRender();

  // 四角形ポリゴンとして描画していく
  glBindVertexArray(quad_);
  glDrawArrays(GL_TRIANGLES, 0, 6);
  glBindVertexArray(0);
}


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ConstExprSliceTest

#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/half.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ir.hpp>
#include <popart/names.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/l1.hpp>
#include <popart/sgd.hpp>
#include <popart/tensor.hpp>
#include <popart/tensordata.hpp>
#include <popart/tensors.hpp>
#include <popart/testdevice.hpp>

#include <math.h>

using namespace popart;

BOOST_AUTO_TEST_CASE(ConstExprTest_Slice0) {
  // clang-format off
  //
  // [Constant] -> (d) -> [Slice] -> (h0)
  // {(h0), (in1)} -> [Add] -> (*)
  //
  // where d is constant, should become
  //
  // {(h0), (in1)} -> [Add] -> (*)
  //
  // clang-format on

  std::vector<float> raw_const_data{1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0};
  ConstVoidData const_data = {raw_const_data.data(), {"FLOAT", Shape{2, 4}}};

  TensorInfo in_info{"FLOAT", Shape{1}};

  auto builder    = Builder::create();
  auto aiOnnx     = builder->aiOnnxOpset9();
  auto const_node = aiOnnx.constant(const_data, "const_data");
  auto slice_node = aiOnnx.slice({const_node}, {2, 3}, {1, 0}, {0, 1});
  auto in_id      = builder->addInputTensor(in_info);
  auto out_id     = aiOnnx.add({slice_node, in_id});
  auto l1         = builder->aiGraphcoreOpset1().l1loss({out_id}, 0.1);

  auto proto       = builder->getModelProto();
  auto model_proto = io::getModelFromString(proto);

  // Create the IR, adding outId as an anchor
  auto art       = AnchorReturnType("All");
  auto data_flow = DataFlow(1, {{out_id, art}});
  auto optimizer = ConstSGD(0.01);
  auto device    = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({model_proto,
              InputShapeInfo(),
              data_flow,
              l1,
              &optimizer,
              *device,
              {}, // no SessionOptions
              Patterns({}).enableRuntimeAsserts(false)});

  // Check the ir
  // 1) that the Add Op is present,
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Add).size() == 1);
  // 2) that the Slice Op is not present,
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Slice).size() == 0);
  // 3) that the shape of the output tensor is as specified.
  Shape ref_shape{1, 3};
  BOOST_CHECK(ir.getMainGraphTensors().get(out_id)->info.shape() == ref_shape);
}

template <typename T> std::string getTypeName();

template <> std::string getTypeName<int32_t>() { return "INT32"; }
template <> std::string getTypeName<float_t>() { return "FLOAT"; }
template <> std::string getTypeName<float16_t>() { return "FLOAT16"; }

template <typename T> void ConstExprTest_Slice0_Type() {
  // clang-format off
  //
  // [Constant] -> (d) -> [Slice] -> (h0)
  // {(h0), (in1)} -> [Add] -> (*)
  //
  // where d is constant, should become
  //
  // {(h0), (in1)} -> [Add] -> (*)
  //
  // clang-format on

  std::vector<T> raw_const_data(7);
  std::iota(raw_const_data.begin(), raw_const_data.end(), 1);
  ConstVoidData const_data = {raw_const_data.data(),
                              {getTypeName<T>(), Shape{2, 4}}};

  TensorInfo in_info{getTypeName<T>(), Shape{1}};

  auto builder    = Builder::create();
  auto aiOnnx     = builder->aiOnnxOpset9();
  auto const_node = aiOnnx.constant(const_data, "const_data");
  auto slice_node = aiOnnx.slice({const_node}, {2, 3}, {1, 0}, {0, 1});
  auto in_id      = builder->addInputTensor(in_info);
  auto out_id     = aiOnnx.add({slice_node, in_id});
  auto l1         = builder->aiGraphcoreOpset1().l1loss({out_id}, 0.1);

  auto proto       = builder->getModelProto();
  auto model_proto = io::getModelFromString(proto);

  // Create the IR, adding outId as an anchor
  auto art       = AnchorReturnType("All");
  auto data_flow = DataFlow(1, {{out_id, art}});
  auto optimizer = ConstSGD(0.01);
  auto device    = createTestDevice(TEST_TARGET);

  Ir ir;
  ir.prepare({model_proto,
              InputShapeInfo(),
              data_flow,
              l1,
              &optimizer,
              *device,
              {}, // no SessionOptions
              Patterns({}).enableRuntimeAsserts(false)});

  // Check the ir
  // 1) that the Add Op is present,
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Add).size() == 1);
  // 2) that the Slice Op is not present,
  BOOST_CHECK(ir.opsOfType(Onnx::AiOnnx::OpSet9::Slice).size() == 0);
  // 3) that the shape of the output tensor is as specified.
  Shape ref_shape{1, 3};
  BOOST_CHECK(ir.getMainGraphTensors().get(out_id)->info.shape() == ref_shape);
}

BOOST_AUTO_TEST_CASE(ConstExprTest_Slice0_Types) {
  ConstExprTest_Slice0_Type<float_t>();
  ConstExprTest_Slice0_Type<float16_t>();
  ConstExprTest_Slice0_Type<int32_t>();
}


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.

// Legacy code to test some BroadcastVectorInner vertices.
// This code is no longer used.

#define BOOST_TEST_MODULE AddToChannel

#include "../lib/popops/ExprOpUtil.hpp"
#include <boost/multi_array.hpp>
#include <functional>
#include <iostream>
#include <limits>
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <poplibs_test/Util.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <poputil/VertexTemplates.hpp>

// Tolerances used in tests
#define FLOAT_REL_TOL 0.01
#define HALF_REL_TOL 0.1
#define FLOAT_ABS_TOL 1e-6
#define HALF_ABS_TOL 1e-3

using namespace poplar;
using namespace poplar::program;
using namespace poputil;
using namespace poplibs_test::util;
using namespace poplibs_support;

const OptionFlags options;

struct TestCase {
  poplar::Type type;
  std::size_t addendLen;
  std::size_t actsLen;
  bool subtract;
};

struct TestCaseData {
  std::vector<double> addend;
  std::vector<double> acts;

  std::unique_ptr<char[]> rawAddend;
  std::unique_ptr<char[]> rawActs;
};

static bool addToChannelTests(const std::vector<TestCase> &cases) {

  const std::size_t overwriteLen = 32;

  auto device = createTestDevice(TEST_TARGET, 1, 1);
  const auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);

  // One compute set, with a vertex for each test case.
  auto cs = graph.addComputeSet("cs");

  Sequence uploadProg;
  Sequence downloadProg;
  std::vector<std::pair<std::string, char *>> tmap;
  std::vector<TestCaseData> tcData(cases.size());

  for (std::size_t i = 0; i < cases.size(); ++i) {
    const auto &tc = cases[i];
    if (tc.actsLen % tc.addendLen != 0) {
      return false;
    }

    std::cout << "Test case [" << i << "]: "
              << " addendLen: " << tc.addendLen << " actsLen: " << tc.actsLen
              << " subtract: " << tc.subtract << " type: " << tc.type.toString()
              << "\n";

    std::string suffix = "_" + std::to_string(i);

    auto addend = graph.addVariable(tc.type, {tc.addendLen}, "addend" + suffix);
    graph.setTileMapping(addend, 0);
    auto acts = graph.addVariable(tc.type, {tc.actsLen + overwriteLen},
                                  "acts" + suffix);
    graph.setTileMapping(acts, 0);

    popops::expr::BinaryOpType op = tc.subtract
                                        ? popops::expr::BinaryOpType::SUBTRACT
                                        : popops::expr::BinaryOpType::ADD;
    std::string templateVertexName = templateVertex(
        "popops::BroadcastVectorInnerInPlaceSupervisor", op, tc.type);

    auto v = graph.addVertex(cs, templateVertexName,
                             {{"data", acts}, {"B", addend}});

    auto actsBlockCount = tc.actsLen / tc.addendLen;
    auto actsBlockCountPacked =
        ((actsBlockCount / 6) << 3) | (actsBlockCount % 6);
    uint16_t actsBlockCountPacked16 = actsBlockCountPacked;

    if (actsBlockCountPacked16 != actsBlockCountPacked)
      return false;

    graph.setInitialValue(v["dataBlockCountPacked"], actsBlockCountPacked16);

    graph.setTileMapping(v, 0);

    tcData[i].rawAddend = allocateHostMemoryForTensor(
        addend, "addend" + suffix, graph, uploadProg, downloadProg, tmap);
    tcData[i].rawActs = allocateHostMemoryForTensor(
        acts, "acts" + suffix, graph, uploadProg, downloadProg, tmap);

    tcData[i].addend.resize(tc.addendLen);
    tcData[i].acts.resize(tc.actsLen + overwriteLen);

    std::mt19937 randomEngine;
    writeRandomValues(target, addend.elementType(), tcData[i].addend.data(),
                      tcData[i].addend.data() + tcData[i].addend.size(), -2.,
                      2., randomEngine);
    writeRandomValues(target, acts.elementType(), tcData[i].acts.data(),
                      tcData[i].acts.data() + tcData[i].acts.size(), -2., 2.,
                      randomEngine);

    copy(target, tcData[i].addend.data(), tcData[i].addend.size(),
         addend.elementType(), tcData[i].rawAddend.get());
    copy(target, tcData[i].acts.data(), tcData[i].acts.size(),
         acts.elementType(), tcData[i].rawActs.get());
  }

  std::cout << "Executing engine\n";

  auto prog = Execute(cs);
  Engine engine(graph, Sequence(uploadProg, prog, downloadProg), options);

  attachStreams(engine, tmap);

  device.bind([&](const Device &d) {
    engine.load(d);
    engine.run(0);
  });

  std::cout << "Checking results\n";

  // Check the results for each test case.

  for (std::size_t i = 0; i < tcData.size(); ++i) {
    const auto &tc = cases[i];

    std::cout << "Checking case [" << i << "]\n";

    // Convert back to double.
    std::vector<double> actsOut(tcData[i].acts.size(), 0.0);

    copy(target, tc.type, tcData[i].rawActs.get(), actsOut.data(),
         actsOut.size());

    // Check the answer.

    auto actsRef = tcData[i].acts;
    for (std::size_t k = 0; k < tc.actsLen; ++k)
      actsRef[k] += tcData[i].addend[k % tc.addendLen] * (tc.subtract ? -1 : 1);

    const double absoluteTolerance =
        tc.type == FLOAT ? FLOAT_ABS_TOL : HALF_ABS_TOL;
    const double relativeTolerance =
        tc.type == FLOAT ? FLOAT_REL_TOL : HALF_REL_TOL;

    auto matchesModel =
        checkIsClose("out", actsOut.data(), {actsOut.size()}, actsRef.data(),
                     actsOut.size(), relativeTolerance, absoluteTolerance);
    if (!matchesModel)
      return false;
  }

  return true;
}

void runAddToChannelTests(std::vector<TestCase> cases) {
  // Test ScaledAddToChannel
  BOOST_TEST(addToChannelTests(cases));

  // Test AddToChannel
  for (auto &tc : cases)
    tc.subtract = false;
  BOOST_TEST(addToChannelTests(cases));
}

const bool isNotSim = !isSimulator(TEST_TARGET);

BOOST_AUTO_TEST_CASE(AddToChannelTiny) {
  std::vector<TestCase> cases = {
      {HALF, 1, 8, false},   {HALF, 4, 16, false},  {HALF, 8, 32, false},
      {HALF, 5, 15, false},  {FLOAT, 1, 8, false},  {FLOAT, 4, 16, false},
      {FLOAT, 8, 32, false}, {FLOAT, 5, 15, false},
  };
  runAddToChannelTests(cases);
}

BOOST_AUTO_TEST_CASE(AddToChannelSmall,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {HALF, 1, 480, false},   {HALF, 4, 480, false},   {HALF, 8, 480, false},
      {HALF, 12, 480, false},  {HALF, 16, 480, false},  {HALF, 1, 15, false},
      {HALF, 4, 12, false},    {HALF, 8, 40, false},    {HALF, 5, 15, false},
      {HALF, 8, 168, false},

      {FLOAT, 1, 480, false},  {FLOAT, 4, 480, false},  {FLOAT, 8, 480, false},
      {FLOAT, 12, 480, false}, {FLOAT, 16, 480, false}, {FLOAT, 1, 15, false},
      {FLOAT, 4, 12, false},   {FLOAT, 8, 40, false},   {FLOAT, 5, 15, false},
      {FLOAT, 8, 168, false},
  };
  runAddToChannelTests(cases);
}

std::size_t maxBlockCount() {
  // This is the maximum acts_block_count the vertex supports.
  // We can only test it for addend length of 1 otherwise it doesn't
  // fit in memory.
  return 4094 * 6;
}

BOOST_AUTO_TEST_CASE(AddToChannelLarge1_half,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {HALF, 1, maxBlockCount(), false},
  };
  runAddToChannelTests(cases);
}

BOOST_AUTO_TEST_CASE(AddToChannelLarge8_half,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {HALF, 8, 8000, false},
  };
  runAddToChannelTests(cases);
}

BOOST_AUTO_TEST_CASE(AddToChannelLarge1_float,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {FLOAT, 1, maxBlockCount(), false},
  };
  runAddToChannelTests(cases);
}

BOOST_AUTO_TEST_CASE(AddToChannelLarge8_float,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  std::vector<TestCase> cases = {
      {FLOAT, 8, 8000, false},
  };
  runAddToChannelTests(cases);
}

// Above an addend length over 2048, we switch to scalar code. Check that works.
BOOST_AUTO_TEST_CASE(AddToChannel_MaxChannels_MultipleOfFour_half,
                     *boost::unit_test::precondition(enableIfNotSim())) {
  for (std::size_t addendLen = 2044; addendLen <= 2056; addendLen += 4) {
    std::vector<TestCase> cases = {
        {HALF, addendLen, addendLen * 4, false},
    };
    runAddToChannelTests(cases);
  }
}


#include "MeshProcessingConnections_EIK.h"
#include "MeshProcessingConnections.h"

#include <CGAL/Polyhedron_3.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/boost/graph/Face_filtered_graph.h>
#include <boost/property_map/property_map.hpp>
#include <boost/iterator/function_output_iterator.hpp>

void* MeshProcessingConnections_EIK_Create()
{
	return MeshProcessingConnections<EIK>::NewMeshProcessingConnections();
}

void MeshProcessingConnections_EIK_Release(void* ptr)
{
	MeshProcessingConnections<EIK>::DeleteMeshProcessingConnections(ptr);
}

//Polyhedron

int MeshProcessingConnections_EIK_ConnectedComponents_PH(void* meshPtr)
{
	return MeshProcessingConnections<EIK>::ConnectedComponents_PH(meshPtr);
}

int MeshProcessingConnections_EIK_ConnectedComponent_PH(void* ptr, void* meshPtr, int index)
{
	return MeshProcessingConnections<EIK>::ConnectedComponent_PH(ptr, meshPtr, index);
}

void MeshProcessingConnections_EIK_GetConnectedComponentsFaceIndex_PH(void* ptr, void* meshPtr, int* indices, int count)
{
	MeshProcessingConnections<EIK>::GetConnectedComponentFaceIndex_PH(ptr, meshPtr, indices, count);
}

int MeshProcessingConnections_EIK_SplitConnectedComponents_PH(void* ptr, void* meshPtr)
{
	return MeshProcessingConnections<EIK>::SplitConnectedComponents_PH(ptr, meshPtr);
}

void MeshProcessingConnections_EIK_GetSplitConnectedComponents_PH(void* ptr, void** meshPtrs, int count)
{
	MeshProcessingConnections<EIK>::GetSplitConnectedComponents_PH(ptr, meshPtrs, count);
}

int MeshProcessingConnections_EIK_KeepLargeConnectedComponents_PH(void* meshPtr, int threshold_value)
{
	return MeshProcessingConnections<EIK>::KeepLargeConnectedComponents_PH(meshPtr, threshold_value);
}

int MeshProcessingConnections_EIK_KeepLargestConnectedComponents_PH(void* meshPtr, int nb_components_to_keep)
{
	return MeshProcessingConnections<EIK>::KeepLargestConnectedComponents_PH(meshPtr, nb_components_to_keep);
}

//Surface Mesh

int MeshProcessingConnections_EIK_ConnectedComponents_SM(void* meshPtr)
{
	return MeshProcessingConnections<EIK>::ConnectedComponents_SM(meshPtr);
}

int MeshProcessingConnections_EIK_ConnectedComponent_SM(void* ptr, void* meshPtr, int index)
{
	return MeshProcessingConnections<EIK>::ConnectedComponent_SM(ptr, meshPtr, index);
}

void MeshProcessingConnections_EIK_GetConnectedComponentsFaceIndex_SM(void* ptr, void* meshPtr, int* indices, int count)
{
	MeshProcessingConnections<EIK>::GetConnectedComponentFaceIndex_SM(ptr, meshPtr, indices, count);
}

int MeshProcessingConnections_EIK_SplitConnectedComponents_SM(void* ptr, void* meshPtr)
{
	return MeshProcessingConnections<EIK>::SplitConnectedComponents_SM(ptr, meshPtr);
}

void MeshProcessingConnections_EIK_GetSplitConnectedComponents_SM(void* ptr, void** meshPtrs, int count)
{
	MeshProcessingConnections<EIK>::GetSplitConnectedComponents_SM(ptr, meshPtrs, count);
}

int MeshProcessingConnections_EIK_KeepLargeConnectedComponents_SM(void* meshPtr, int threshold_value)
{
	return MeshProcessingConnections<EIK>::KeepLargeConnectedComponents_SM(meshPtr, threshold_value);
}

int MeshProcessingConnections_EIK_KeepLargestConnectedComponents_SM(void* meshPtr, int nb_components_to_keep)
{
	return MeshProcessingConnections<EIK>::KeepLargestConnectedComponents_SM(meshPtr, nb_components_to_keep);
}


// copyright (c) 2012-2013 the dzcoin core developers
// distributed under the mit software license, see the accompanying
// file copying or http://www.opensource.org/licenses/mit-license.php.

#include "pubkey.h"
#include "key.h"
#include "script/script.h"
#include "script/standard.h"
#include "uint256.h"
#include "test/test_dzcoin.h"

#include <vector>

#include <boost/foreach.hpp>
#include <boost/test/unit_test.hpp>

using namespace std;

// helpers:
static std::vector<unsigned char>
serialize(const cscript& s)
{
    std::vector<unsigned char> sserialized(s);
    return sserialized;
}

boost_fixture_test_suite(sigopcount_tests, basictestingsetup)

boost_auto_test_case(getsigopcount)
{
    // test cscript::getsigopcount()
    cscript s1;
    boost_check_equal(s1.getsigopcount(false), 0u);
    boost_check_equal(s1.getsigopcount(true), 0u);

    uint160 dummy;
    s1 << op_1 << tobytevector(dummy) << tobytevector(dummy) << op_2 << op_checkmultisig;
    boost_check_equal(s1.getsigopcount(true), 2u);
    s1 << op_if << op_checksig << op_endif;
    boost_check_equal(s1.getsigopcount(true), 3u);
    boost_check_equal(s1.getsigopcount(false), 21u);

    cscript p2sh = getscriptfordestination(cscriptid(s1));
    cscript scriptsig;
    scriptsig << op_0 << serialize(s1);
    boost_check_equal(p2sh.getsigopcount(scriptsig), 3u);

    std::vector<cpubkey> keys;
    for (int i = 0; i < 3; i++)
    {
        ckey k;
        k.makenewkey(true);
        keys.push_back(k.getpubkey());
    }
    cscript s2 = getscriptformultisig(1, keys);
    boost_check_equal(s2.getsigopcount(true), 3u);
    boost_check_equal(s2.getsigopcount(false), 20u);

    p2sh = getscriptfordestination(cscriptid(s2));
    boost_check_equal(p2sh.getsigopcount(true), 0u);
    boost_check_equal(p2sh.getsigopcount(false), 0u);
    cscript scriptsig2;
    scriptsig2 << op_1 << tobytevector(dummy) << tobytevector(dummy) << serialize(s2);
    boost_check_equal(p2sh.getsigopcount(scriptsig2), 3u);
}

boost_auto_test_suite_end()


#include "lwtnn/LightweightNeuralNetwork.hh"
#include "lwtnn/Stack.hh"
#include "lwtnn/parse_json.hh"

#include "test_utilities.hh"

#include <Eigen/Dense>

#include <iostream>
#include <string>
#include <fstream>
#include <cassert>

void usage(const std::string& name) {
  std::cout << "usage: " << name << " <nn config> [<labels> <values>]\n"
    "\n"
    "The <nn config> file should be generated by one of the scripts in\n"
    "`converters/`.\n"
    "\n"
    "Both <labels> and <values> should be whitespace separated.\n"
    "In the case of values, there should be one pattern per line.\n"
    "If the labels and values are omitted generate dummy data.\n";
}

namespace {
  int run_on_generated(const lwt::JSONConfig& config);

  int run_on_files(const lwt::JSONConfig& config,
                   const std::string& vars,
                   const std::string& vals);
}

int main(int argc, char* argv[]) {
  if (argc > 4 || argc < 2) {
    usage(argv[0]);
    exit(1);
  }
  // Read in the configuration.
  std::string in_file_name(argv[1]);
  std::ifstream in_file(in_file_name);
  auto config = lwt::parse_json(in_file);

  if ( argc == 4) {
    run_on_files(config, argv[2], argv[3]);
  } else if (argc == 2) {
    run_on_generated(config);
  } else {
    usage(argv[0]);
    exit(1);
  }
  return 0;
}

namespace {

  ////////////////////////////
  // reading from generated //
  ////////////////////////////

  int run_on_generated(const lwt::JSONConfig& config) {
    // Read in the configuration.

    lwt::LightweightRNN rnn(config.inputs, config.layers, config.outputs);
    const auto inputs = get_values_vec(config.inputs, 20);
    auto outs = rnn.reduce(inputs);
    for (const auto& out: outs) {
      std::cout << out.first << " " << out.second << std::endl;
    }
    return 0;
  }

  ///////////////////////
  // reading from file //
  ///////////////////////

  int run_on_files(const lwt::JSONConfig& config,
                   const std::string& vars,
                   const std::string& vals) {
    // initialize the tagger from the configuration
    lwt::LightweightRNN rnn(config.inputs, config.layers, config.outputs);

    // buffer
    std::string val_line;

    // read in input labels and values
    std::ifstream vars_stream( vars );
    std::getline(vars_stream, val_line);
    const auto labels = parse_line(val_line);

    lwt::VectorMap in;

    std::ifstream values( vals );
    while (std::getline(values, val_line)) {
      auto val_strings = parse_line(val_line);
      if (val_strings.size() == 0) continue;
      assert(val_strings.size() == labels.size());
      for (size_t iii = 0; iii < labels.size(); iii++) {
        in[labels.at(iii)].push_back(std::stof(val_strings.at(iii)));
      }
    }
    auto out_vals = rnn.reduce(in);
    // look at the outputs
    for (const auto& out: out_vals) {
      std::cout << out.first << " " << out.second << std::endl;
    }

    return 0;
  }

}


#ifndef SODES_MATH_TYPES_HPP
#define SODES_MATH_TYPES_HPP

/*
 * This header defines several useful math types using Eigen. This is necessary due to
 * the differences of how NumPy and Eigen store information. NumPy arrays are Row Major by default,
 * while Eigen is Column Major. Such different approach causes problems in conversions and bindings,
 * especially when modifications in parameters passed by reference are performed.
 */

#include <Eigen/Core>

namespace sodes::math_types {

using namespace Eigen;

using eigen_array_1d = Array<double, Dynamic, 1>;
using eigen_array_1d_ref = Ref<eigen_array_1d>;
using eigen_array_1d_constref = Ref<eigen_array_1d> const&;

}

#endif //SODES_MATH_TYPES_HPP


#include "biggles/model.hpp"
#include "biggles/samplers.hpp"
#include "biggles/partition_sampler.hpp"
#include "biggles/simulate.hpp"
#include <boost/foreach.hpp>
#include <iomanip>
#include <iostream>
#include <fstream>
#include <string>

#include "biggles/tools/sundries.hpp"

// include this last to stop pre-processor macros breaking things
extern "C" {
#include <ccan/tap/tap.h>
}

using namespace biggles;

int main(int argc, char** argv)
{
    plan_tests(2);

    float sigma_R = 0.1f;

    // initialise model parameters
    model::parameters params;
    model::mean_new_tracks_per_frame(params)           = 3.f;
    model::mean_false_observations_per_frame(params)   = 1.f;
    model::frame_to_frame_survival_probability(params) = 0.95f;
    model::generate_observation_probability(params)    = 0.9f;
    model::observation_error_covariance(params)       << sigma_R * sigma_R, 0, 0, sigma_R * sigma_R;
    model::process_noise_covariance(params) = detail::initQ();

    float image_size(128.f);
    time_stamp time_extent(128);

    // simulate full data
    partition_ptr_t partition_ptr;
    simulate::generate_partition(partition_ptr, params, 0, (time_extent>>1) + time_extent, image_size * 1.5f, image_size * 1.5f);
    diag("Partition volume = %.2f", partition_ptr->volume());

    diag("Generated %zu new tracks.", partition_ptr->tracks().size());
    diag("Generated %zu clutter observations.", partition_ptr->clutter().size());

    // crop partition to a specific region
    simulate::crop_partition(partition_ptr, partition_ptr,
                             image_size * 0.25f, image_size * 1.25f,
                             image_size * 0.25f, image_size * 1.25f,
                             time_extent >> 2, (time_extent >> 2) + time_extent);

    diag("Cropped to %zu tracks.", partition_ptr->tracks().size());
    diag("Cropped to %zu clutter observations.", partition_ptr->clutter().size());
    diag("Ground truth log pdf: %f", model::log_partition_given_parameters_and_data_density(*partition_ptr, params));
    diag("Partition volume = %.2f", partition_ptr->volume());

    // create a synthetic dataset from the partition
    simulate::demote_all_tracks_from_partition(partition_ptr, partition_ptr);

    // check we have some data and no tracks
    ok1(partition_ptr->clutter().size() > 0);
    ok1(partition_ptr->tracks().size() == 0);

    diag("Final data set consists of %zu observations.", partition_ptr->clutter().size());

    partition_sampler sample_partition(partition_ptr, params);
    partition_ptr_t best_partition_ptr(sample_partition.last_partition());
    float best_log_pdf(sample_partition.current_sample_log_density());

    int n_it = 4000;
    if(argc > 1)
    {
        n_it = atoi(argv[1]);
    }

    std::ofstream stats("biggles_partition_sampler_stats.txt");
    for(int i=0; i<n_it; ++i)
    {
        sample_partition.draw();

        if(sample_partition.current_sample_log_density() > best_log_pdf)
        {
            best_partition_ptr = sample_partition.last_partition();
            best_log_pdf = sample_partition.current_sample_log_density();
        }

        if(0 == (i & 0xff))
        {
            diag("i: %i, alpha: %f, track count: %zu, log PDF: %f, current: %f",
                 i,
                 sample_partition.acceptance_rate(),
                 best_partition_ptr->tracks().size(),
                 best_log_pdf,
                 sample_partition.current_sample_log_density());
            diag("sample partition volume = %.2f", sample_partition.last_partition()->volume());
        }

        if(0 == (i & 0xfff))
        {
            write_partition("partition_sampler", *best_partition_ptr, "post_sampling");
        }

        stats << std::setw(16) << sample_partition.acceptance_rate()
              << std::setw(16) << best_log_pdf
              << std::setw(16) << best_partition_ptr->tracks().size()
              << '\n';
    }

    // write out post-sampling dataset
    write_partition("partition_sampler", *best_partition_ptr, "post_sampling");

    return exit_status();
}