// MIT License
//
// Copyright (c) 2019 Ivan Eichhardt
//
// 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.
//
// Please contact the author of this library if you have any questions.
// Author: Ivan Eichhardt (ivan.eichhardt@sztaki.mta.hu)

#include <iostream>
#include <unordered_map>

#include <Eigen/Dense>
#include <third_party/cmdLine/cmdLine.h>
#include <third_party/progress/progress_display.hpp>
#include <third_party/stlplus3/filesystemSimplified/file_system.hpp>

#include <openMVG/multiview/essential.hpp>
#include <openMVG/sfm/sfm_data.hpp>
#include <openMVG/sfm/sfm_data_io.hpp>

#include <higher_order/refinement.hpp>

using namespace openMVG;
using namespace openMVG::sfm;
using namespace openMVG::geometry;

void RefineLAFsForLandmark(
  const SfM_Data& sfm_data,
  Landmark& landmark,
  bool use_observed_region_centers = false);

void RefineAllLAFs(SfM_Data& sfm_data);

int main(int argc, char** argv) {
  CmdLine cmd;

  std::string sSfM_Data_Filename;
  std::string sOutDir = "";

  cmd.add(make_option('i', sSfM_Data_Filename, "sfmdata"));
  cmd.add(make_option('o', sOutDir, "outdir"));

  try {
    if (argc == 1) {
      throw std::string("Invalid command line parameter.");
    }
    cmd.process(argc, argv);
  }
  catch (const std::string& s) {
    std::cerr << "Usage: " << argv[0] << '\n'
      << "[-i|--sfmdata] filename, the SfM_Data file\n"
      << "[-o|--outdir] path where refined SfM_Data with refined LAFs\n"
      << std::endl;

    std::cerr << s << std::endl;
    return EXIT_FAILURE;
  }

  if (!stlplus::folder_exists(sOutDir)) {
    stlplus::folder_create(sOutDir);
  }

  // Read the input SfM scene
  SfM_Data sfm_data;
  if (!Load(sfm_data, sSfM_Data_Filename, ESfM_Data(ALL))) {
    std::cerr << std::endl
      << "The input SfM_Data file \"" << sSfM_Data_Filename
      << "\" cannot be read." << std::endl;
    return EXIT_FAILURE;
  }

  // Perform refinement for all landmarks
  RefineAllLAFs(sfm_data);

  // Save the output scene with the refined observations
  std::string output_sSfM_Data_Filename = stlplus::create_filespec(
    sOutDir, "refined_" + stlplus::basename_part(sSfM_Data_Filename), "bin");
  if (!Save(sfm_data, output_sSfM_Data_Filename, ESfM_Data(ALL))) {
    std::cerr << std::endl
      << "Cannot write to output SfM_Data file \"" << output_sSfM_Data_Filename
      << "\"." << std::endl;
    return EXIT_FAILURE;
  }
}

void RefineLAFsForLandmark(
  const SfM_Data& sfm_data,
  Landmark& landmark,
  bool use_observed_region_centers) {
  using namespace higher_order;

  auto& obs = landmark.obs;

  EpipolarConstraints constraints;
  constraints.reserve((obs.size() * (obs.size() - 1)) / 2);

  ObservedLAFs LAFs(obs.size());

  struct CachedView {
    openMVG::Vec3 bearing;
    openMVG::Mat32 gradient;
    openMVG::Mat2 distortion_gradient;

    inline Mat2 distort(const Mat2& M) const {
      return
         distortion_gradient * M;
    }

    inline Mat2 undistort(const Mat2& M) const {
      return
         distortion_gradient.inverse() * M;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

  std::unordered_map<ViewID, CachedView> cached_views;

  // populate LAFs and pre-evaluate bearing vectors and gradients, etc
  auto index = 0;
  for (const auto& observation : obs) {
    auto viewID = observation.first;
    auto& ob = observation.second;
    const Mat2 M = ob.M;

    const auto view = sfm_data.views.at(viewID).get();
    const auto intrinsic = sfm_data.intrinsics.at(view->id_intrinsic).get();
    const auto& pose = sfm_data.GetPoseOrDie(view);

    const Vec2 x = use_observed_region_centers
      ? intrinsic->get_ud_pixel(ob.x)
      : intrinsic->project(pose(landmark.X), true);

    cached_views[viewID] = {
      /*bearing*/ intrinsic->operator()(x),
      /*gradient*/ intrinsic->gradient(x),
      intrinsic->get_d_pixel_gradient(x)
    };

    LAFs[index].viewID = viewID;
    LAFs[index].M = cached_views[viewID].undistort(M);
    ++index;
  }

  for (auto it = obs.begin(); it != obs.end(); ++it) {
    const auto& viewID_i = it->first;
    const auto view_i = sfm_data.views.at(viewID_i).get();
    const auto& pose_i = sfm_data.GetPoseOrDie(view_i);

    // bearing vector and gradient corresponding to viewID_i
    const auto& [q_i, q_grad_i, _3] = cached_views[viewID_i];

    auto tmp = it;
    std::advance(tmp, 1);
    for (auto jt = tmp; jt != obs.end(); ++jt) {
      const auto& viewID_j = jt->first;
      const auto view_j = sfm_data.views.at(viewID_j).get();
      const auto& pose_j = sfm_data.poses.at(view_j->id_pose);

      // bearing vector and gradient corresponding to viewID_j
      const auto& [q_j, q_grad_j, _3] = cached_views[viewID_j];

      Mat3 E;
      // TODO: this can be made a bit more efficient.
      EssentialFromRt(pose_i.rotation(), pose_i.translation(),
        pose_j.rotation(), pose_j.translation(), &E);

      EpipolarConstraintOnLAFs constraint;
      constraint.view_pair = { viewID_i, viewID_j };

      stereo::CreateFirstOrderConstraint(E,
        q_i, q_j, q_grad_i, q_grad_j,
        constraint.a, constraint.b);

      constraints.emplace_back(constraint);
    }
  }

  multiview::RefineLocalAffineFrames(
    constraints,
    LAFs
  );

  for (const auto& laf : LAFs) {
    auto& viewID = laf.viewID;
    auto& M = laf.M;
    obs[viewID].M = cached_views[viewID].distort(M);
  }
}

void RefineAllLAFs(SfM_Data& sfm_data) {
  C_Progress_display my_progress_bar(sfm_data.GetLandmarks().size(), std::cout,
    "\n- PROCESSING LANDMARKS (Refining Local Affine Frames) -\n");

  for (auto& landmark : sfm_data.structure) {
    auto& obs = landmark.second.obs;

    RefineLAFsForLandmark(sfm_data, landmark.second);

    ++my_progress_bar;
  }
}


// Copyright (c) Darrell Wright
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/beached/daw_json_link
//

#include "defines.h"

#include "daw/json/daw_json_link.h"

#include <daw/daw_read_file.h>

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <string_view>

namespace daw::json {
	template<typename Backend, boost::multiprecision::expression_template_option
	                             ExpressionTemplates>
	struct json_data_contract<
	  boost::multiprecision::number<Backend, ExpressionTemplates>> {

		using type = json_class_map<std::string_view>;

		static inline auto to_json_data(
		  boost::multiprecision::number<Backend, ExpressionTemplates> const
		    &value ) {
			return value.str( );
		}
	};
} // namespace daw::json

int main( int argc, char **argv )
#ifdef DAW_USE_EXCEPTIONS
  try
#endif
{
	if( argc <= 1 ) {
		puts( "Must supply path to cookbook_numbers3.json file\n" );
		exit( EXIT_FAILURE );
	}
	auto data = daw::read_file( argv[1] );
	assert( data and not data->empty( ) );

	auto numbers =
	  daw::json::from_json_array<boost::multiprecision::cpp_dec_float_100>(
	    *data );

	assert( not numbers.empty( ) );
	auto new_json_data = daw::json::to_json( numbers );
	puts( new_json_data.c_str( ) );

	auto const numbers2 =
	  daw::json::from_json_array<boost::multiprecision::cpp_dec_float_100>(
	    new_json_data );
	assert( not numbers2.empty( ) );

	test_assert( numbers.front( ) == numbers2.front( ),
	             "Unexpected round trip error" );
	test_assert( numbers.size( ) == numbers2.size( ),
	             "Unexpected round trip error" );
}
#ifdef DAW_USE_EXCEPTIONS
catch( daw::json::json_exception const &jex ) {
	std::cerr << "Exception thrown by parser: " << jex.reason( ) << std::endl;
	exit( 1 );
}
#endif


// The MIT License (MIT)

// Copyright (c) 2019 Caili Li

//  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.

/* code */

#ifndef __NAVIGATIONFUNCTION__
#define __NAVIGATIONFUNCTION__

#include <vector>
#include <memory>
#include <utility>

#include <Eigen/Core>

#include "navigation_function/math/StarObject.hpp"
#include "navigation_function/math/ScalarObject.hpp"

namespace roboflow
{
namespace navigation_function
{

enum class DestinationFunctionType
{
    DestPoint,
    DestSphere
};

class NavigationFunction
{
public:
    // point
    virtual std::pair<double, bool> evaluate(Eigen::Vector2d) = 0;
    // point: x, y
    virtual std::pair<double, bool> evaluate(double, double) = 0;
    // get all obstacles, including the zeroth obsatcle
    virtual std::vector<std::shared_ptr<math::StarObject>> getObstacles() const = 0;
    // get destination function and its type
    virtual std::pair<std::shared_ptr<math::ScalarObject>, DestinationFunctionType>
    getDestinationFunction() const = 0;
};

} // namespace navigation_function
} // namespace roboflow

#endif


/*
   Copyright (C) 2014 - 2015 by Chris Beck <render787@gmail.com>
   Part of the Battle for Wesnoth Project http://www.wesnoth.org/

   This program 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; either version 2 of the License, or
   (at your option) any later version.
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY.

   See the COPYING file for more details.
*/

/* This file selects a seed source -- "nondeterministic" random number
   generator in boost documentation. It should be a wrapper for
   boost::random_device on platforms where this is available, otherwise
   it should most likely be the system time.
*/

#include "seed_rng.hpp"
#include <boost/version.hpp>

/*****
  Use preprocessor tests to decide whether to try to include and
  use boost random device, or fallback to system time.
 *****/

//Boost does not support random device on windows before v 1.43.0
//http://www.boost.org/users/history/version_1_43_0.html
#if (defined(_WIN32) && (BOOST_VERSION < 104300))
#define NO_BOOST_RANDOM_DEVICE
#endif

/*****
  End preprocessor checks
 *****/

#ifndef NO_BOOST_RANDOM_DEVICE
#include <boost/nondet_random.hpp>
#else
#include <ctime>
#endif

#include <sstream>
#include <iomanip>

namespace seed_rng {

	uint32_t next_seed() {
	#ifndef NO_BOOST_RANDOM_DEVICE
		static boost::random_device rnd_;
		return rnd_();
	#else
		return static_cast<uint32_t> (std::time(0));
	#endif
	}

	std::string next_seed_str() {
		uint32_t random_seed_ = next_seed();
		std::stringstream stream;
		stream << std::setfill('0') << std::setw(sizeof(uint32_t)*2) << std::hex << random_seed_;

		return stream.str();
	}

} //ends seed_rng namespace


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test Helper

// Copyright (c) 2010-2019 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2016-2020.
// Modifications copyright (c) 2016-2020, 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)

#ifndef BOOST_GEOMETRY_TEST_BUFFER_HPP
#define BOOST_GEOMETRY_TEST_BUFFER_HPP

#if defined(TEST_WITH_SVG)
    // Define before including any buffer headerfile
    #define BOOST_GEOMETRY_BUFFER_USE_HELPER_POINTS
#endif

#include <iostream>
#include <fstream>
#include <iomanip>

#include <boost/foreach.hpp>
#include <geometry_test_common.hpp>
#include <expectation_limits.hpp>

#include <boost/geometry/algorithms/envelope.hpp>
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/algorithms/buffer.hpp>
#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
#include <boost/geometry/algorithms/is_empty.hpp>
#include <boost/geometry/algorithms/is_valid.hpp>
#include <boost/geometry/algorithms/num_interior_rings.hpp>
#include <boost/geometry/algorithms/union.hpp>

#include <boost/geometry/algorithms/detail/overlay/debug_turn_info.hpp>
#include <boost/geometry/algorithms/detail/overlay/self_turn_points.hpp>

#include <boost/geometry/geometries/geometries.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/strategies/buffer.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>

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

const double same_distance = -999;

#if defined(TEST_WITH_SVG)
#  include "test_buffer_svg.hpp"
#  include "test_buffer_svg_per_turn.hpp"
#endif

//-----------------------------------------------------------------------------
template <typename JoinStrategy>
struct JoinTestProperties
{
    static std::string name() { return "joinunknown"; }
};

template<> struct JoinTestProperties<boost::geometry::strategy::buffer::join_round>
{
    static std::string name() { return "round"; }
};

template<> struct JoinTestProperties<boost::geometry::strategy::buffer::join_miter>
{
    static std::string name() { return "miter"; }
};

template<> struct JoinTestProperties<boost::geometry::strategy::buffer::join_round_by_divide>
{
    static std::string name() { return "divide"; }
};

//-----------------------------------------------------------------------------
template <typename EndStrategy>
struct EndTestProperties { };

template<> struct EndTestProperties<boost::geometry::strategy::buffer::end_round>
{
    static std::string name() { return "round"; }
};

template<> struct EndTestProperties<boost::geometry::strategy::buffer::end_flat>
{
    static std::string name() { return "flat"; }
};

struct ut_settings : public ut_base_settings
{
    explicit ut_settings(double tol = 0.01, bool val = true, int points = 88)
        : ut_base_settings(val)
        , tolerance(tol)
        , test_area(true)
        , use_ln_area(false)
        , points_per_circle(points)
    {}

    static inline ut_settings ignore_validity()
    {
        ut_settings result;
        result.set_test_validity(false);
        return result;
    }

    static inline ut_settings assertions_only()
    {
        ut_settings result;
        result.test_area = false;
        result.set_test_validity(false);
        return result;
    }

    static inline double ignore_area() { return 9999.9; }

    double tolerance;
    bool test_area;
    bool use_ln_area;
    int points_per_circle;
};

template
<
    typename GeometryOut,
    typename JoinStrategy,
    typename EndStrategy,
    typename DistanceStrategy,
    typename SideStrategy,
    typename PointStrategy,
    typename Strategy,
    typename Geometry
>
void test_buffer(std::string const& caseid,
            bg::model::multi_polygon<GeometryOut>& buffered,
            Geometry const& geometry,
            JoinStrategy const& join_strategy,
            EndStrategy const& end_strategy,
            DistanceStrategy const& distance_strategy,
            SideStrategy const& side_strategy,
            PointStrategy const& point_strategy,
            Strategy const& strategy,
            int expected_count,
            int expected_holes_count,
            expectation_limits const& expected_area,
            ut_settings const& settings)
{
    namespace bg = boost::geometry;

    typedef typename bg::coordinate_type<Geometry>::type coordinate_type;
    typedef typename bg::point_type<Geometry>::type point_type;

    typedef typename bg::tag<Geometry>::type tag;
    // TODO use something different here:
    std::string type = boost::is_same<tag, bg::polygon_tag>::value ? "poly"
        : boost::is_same<tag, bg::linestring_tag>::value ? "line"
        : boost::is_same<tag, bg::point_tag>::value ? "point"
        : boost::is_same<tag, bg::multi_polygon_tag>::value ? "multipoly"
        : boost::is_same<tag, bg::multi_linestring_tag>::value ? "multiline"
        : boost::is_same<tag, bg::multi_point_tag>::value ? "multipoint"
        : ""
        ;

    bg::model::box<point_type> envelope;
    if (bg::is_empty(geometry))
    {
        bg::assign_values(envelope, 0, 0, 1,  1);
    }
    else
    {
        bg::envelope(geometry, envelope, strategy);
    }

    std::string join_name = JoinTestProperties<JoinStrategy>::name();
    std::string end_name = EndTestProperties<EndStrategy>::name();

    if ( BOOST_GEOMETRY_CONDITION((
            boost::is_same<tag, bg::point_tag>::value
         || boost::is_same<tag, bg::multi_point_tag>::value )) )
    {
        join_name.clear();
    }

    std::ostringstream complete;
    complete
        << type << "_"
        << caseid << "_"
        << string_from_type<coordinate_type>::name()
        << "_" << join_name
        << (end_name.empty() ? "" : "_") << end_name
        << (distance_strategy.negative() ? "_deflate" : "")
        << (bg::point_order<GeometryOut>::value == bg::counterclockwise ? "_ccw" : "")
#if defined(BOOST_GEOMETRY_USE_RESCALING)
        << "_rescaled"
#endif
         // << "_" << point_buffer_count
        ;

    //std::cout << complete.str() << std::endl;

#if defined(TEST_WITH_SVG_PER_TURN)
    save_turns_visitor<point_type> visitor;
#elif defined(TEST_WITH_SVG)

    buffer_svg_mapper<point_type> buffer_mapper(complete.str());

    std::ostringstream filename;
    filename << "buffer_" << complete.str() << ".svg";
    std::ofstream svg(filename.str().c_str());
    typedef bg::svg_mapper<point_type> mapper_type;
    mapper_type mapper(svg, 1000, 800);

    svg_visitor<mapper_type, bg::model::box<point_type> > visitor(mapper);

    buffer_mapper.prepare(mapper, visitor, envelope,
            distance_strategy.negative()
            ? 1.0
            : 1.1 * distance_strategy.max_distance(join_strategy, end_strategy)
        );
#else
    bg::detail::buffer::visit_pieces_default_policy visitor;
#endif

    typedef typename bg::point_type<Geometry>::type point_type;
    typedef typename bg::rescale_policy_type<point_type>::type
        rescale_policy_type;

    // Enlarge the box to get a proper rescale policy
    bg::buffer(envelope, envelope, distance_strategy.max_distance(join_strategy, end_strategy));

    rescale_policy_type rescale_policy
            = bg::get_rescale_policy<rescale_policy_type>(envelope, strategy);

    buffered.clear();
    bg::detail::buffer::buffer_inserter<GeometryOut>(geometry,
                        std::back_inserter(buffered),
                        distance_strategy,
                        side_strategy,
                        join_strategy,
                        end_strategy,
                        point_strategy,
                        strategy,
                        rescale_policy,
                        visitor);

#if defined(TEST_WITH_SVG)
    buffer_mapper.map_input_output(mapper, geometry, buffered, distance_strategy.negative());
#endif

    //Uncomment to create simple CSV to compare/use in tests - adapt precision if necessary
    //std::cout << complete.str() << "," << std::fixed << std::setprecision(0) << area << std::endl;
    //return;

    if (bg::is_empty(buffered) && expected_area.is_zero())
    {
        // As expected - don't get rescale policy for output (will be invalid)
        return;
    }

    if (settings.test_area)
    {
        BOOST_CHECK_MESSAGE
            (
                ! bg::is_empty(buffered),
                complete.str() << " output is empty (unexpected)."
            );
    }

    bg::model::box<point_type> envelope_output;
    bg::assign_values(envelope_output, 0, 0, 1,  1);
    bg::envelope(buffered, envelope_output, strategy);

    //    std::cout << caseid << std::endl;
    //    std::cout << "INPUT: " << bg::wkt(geometry) << std::endl;
    //    std::cout << "OUTPUT: " << area << std::endl;
    //    std::cout << "OUTPUT env: " << bg::wkt(envelope_output) << std::endl;
    //    std::cout << bg::wkt(buffered) << std::endl;

    if (expected_count >= 0)
    {
        BOOST_CHECK_MESSAGE
            (
                int(buffered.size()) == expected_count,
                "#outputs not as expected."
                << " Expected: " << expected_count
                << " Detected: " << buffered.size()
            );
    }

    if (expected_holes_count >= 0)
    {
        std::size_t nholes = bg::num_interior_rings(buffered);
        BOOST_CHECK_MESSAGE
        (
            int(nholes) == expected_holes_count,
            complete.str() << " #holes not as expected."
            << " Expected: " << expected_holes_count
            << " Detected: " << nholes
        );
    }

    if (settings.test_area)
    {
        auto const area = bg::area(buffered, strategy);
        BOOST_CHECK_MESSAGE(expected_area.contains(area, settings.tolerance, settings.use_ln_area),
              "difference: " << caseid << std::setprecision(20)
              << " #area expected: " << expected_area
              << " detected: " << area
              << " type: " << (type_for_assert_message<Geometry, GeometryOut>())
              );
    }

    if (settings.test_validity() && ! bg::is_valid(buffered))
    {
        BOOST_CHECK_MESSAGE(bg::is_valid(buffered), complete.str() <<  " is not valid");
    }

#if defined(TEST_WITH_SVG_PER_TURN)
    {
        // Create a per turn visitor to map per turn, and buffer again with it
        per_turn_visitor<point_type> ptv(complete.str(), visitor.get_points());
        bg::detail::buffer::buffer_inserter<GeometryOut>(geometry,
                            std::back_inserter(buffered),
                            distance_strategy,
                            side_strategy,
                            join_strategy,
                            end_strategy,
                            point_strategy,
                            rescale_policy,
                            ptv);
        ptv.map_input_output(geometry, buffered, distance_strategy.negative());
        // self_ips NYI here
    }
#elif defined(TEST_WITH_SVG)
    rescale_policy_type rescale_policy_output
            = bg::get_rescale_policy<rescale_policy_type>(envelope_output);
    buffer_mapper.map_self_ips(mapper, buffered, strategy, rescale_policy_output);
#endif

}

template
<
    typename GeometryOut,
    typename JoinStrategy,
    typename EndStrategy,
    typename DistanceStrategy,
    typename SideStrategy,
    typename PointStrategy,
    typename Geometry
>
void test_buffer(std::string const& caseid, bg::model::multi_polygon<GeometryOut>& buffered, Geometry const& geometry,
            JoinStrategy const& join_strategy,
            EndStrategy const& end_strategy,
            DistanceStrategy const& distance_strategy,
            SideStrategy const& side_strategy,
            PointStrategy const& point_strategy,
            expectation_limits const& expected_area,
            ut_settings const& settings = ut_settings())
{
    typename bg::strategies::relate::services::default_strategy
        <
            Geometry, Geometry
        >::type strategy;

    test_buffer<GeometryOut>(caseid, buffered, geometry,
        join_strategy, end_strategy, distance_strategy, side_strategy, point_strategy,
        strategy,
        -1, -1, expected_area, settings);
}

#ifdef BOOST_GEOMETRY_CHECK_WITH_POSTGIS
static int counter = 0;
#endif

template
<
    typename Geometry,
    typename GeometryOut,
    typename JoinStrategy,
    typename EndStrategy
>
void test_one(std::string const& caseid, std::string const& wkt,
        JoinStrategy const& join_strategy, EndStrategy const& end_strategy,
        int expected_count, int expected_holes_count, expectation_limits const& expected_area,
        double distance_left, ut_settings const& settings = ut_settings(),
        double distance_right = same_distance)
{
    namespace bg = boost::geometry;
    Geometry g;
    bg::read_wkt(wkt, g);
    bg::correct(g);

#ifdef BOOST_GEOMETRY_CHECK_WITH_POSTGIS
    std::cout
        << (counter > 0 ? "union " : "")
        << "select " << counter++
        << ", '" << caseid << "' as caseid"
        << ", ST_Area(ST_Buffer(ST_GeomFromText('" << wkt << "'), "
        << distance_left
        << ", 'endcap=" << end_name << " join=" << join_name << "'))"
        << ", "  << expected_area
        << std::endl;
#endif

    bg::strategy::buffer::side_straight side_strategy;
    bg::strategy::buffer::point_circle circle_strategy(settings.points_per_circle);

    bg::strategy::buffer::distance_asymmetric
    <
        typename bg::coordinate_type<Geometry>::type
    > distance_strategy(distance_left,
                        bg::math::equals(distance_right, same_distance)
                        ? distance_left : distance_right);

    typename bg::strategies::relate::services::default_strategy
        <
            Geometry, Geometry
        >::type strategy;

    bg::model::multi_polygon<GeometryOut> buffered;
    test_buffer<GeometryOut>
            (caseid, buffered, g,
            join_strategy, end_strategy,
            distance_strategy, side_strategy, circle_strategy,
            strategy,
            expected_count, expected_holes_count, expected_area,
            settings);

#if !defined(BOOST_GEOMETRY_COMPILER_MODE_DEBUG) \
    && !defined(BOOST_GEOMETRY_TEST_ONLY_ONE_ORDER) \
    && defined(BOOST_GEOMETRY_COMPILER_MODE_RELEASE)

    // Also test symmetric distance strategy if right-distance is not specified
    // (only in release mode, not if "one order" if speficied)
    if (bg::math::equals(distance_right, same_distance))
    {
        bg::strategy::buffer::distance_symmetric
        <
            typename bg::coordinate_type<Geometry>::type
        > sym_distance_strategy(distance_left);

        test_buffer<GeometryOut>
                (caseid + "_sym", buffered, g,
                join_strategy, end_strategy,
                sym_distance_strategy, side_strategy, circle_strategy,
                strategy,
                expected_count, expected_holes_count, expected_area,
                settings);

    }
#endif
}

template
<
    typename Geometry,
    typename GeometryOut,
    typename JoinStrategy,
    typename EndStrategy
>
void test_one(std::string const& caseid, std::string const& wkt,
        JoinStrategy const& join_strategy, EndStrategy const& end_strategy,
        expectation_limits const& expected_area,
        double distance_left, ut_settings const& settings = ut_settings(),
        double distance_right = same_distance)
{
    test_one<Geometry, GeometryOut>(caseid, wkt, join_strategy, end_strategy,
        -1 ,-1, expected_area,
        distance_left, settings, distance_right);
}

template
<
    typename Geometry,
    typename GeometryOut,
    typename JoinStrategy,
    typename EndStrategy,
    typename DistanceStrategy,
    typename SideStrategy,
    typename PointStrategy
>
void test_with_custom_strategies(std::string const& caseid,
        std::string const& wkt,
        JoinStrategy const& join_strategy,
        EndStrategy const& end_strategy,
        DistanceStrategy const& distance_strategy,
        SideStrategy const& side_strategy,
        PointStrategy const& point_strategy,
        expectation_limits const& expected_area,
        ut_settings const& settings = ut_settings())
{
    namespace bg = boost::geometry;
    Geometry g;
    bg::read_wkt(wkt, g);
    bg::correct(g);

    bg::model::multi_polygon<GeometryOut> buffered;

    test_buffer<GeometryOut>
            (caseid, buffered, g,
            join_strategy, end_strategy,
            distance_strategy, side_strategy, point_strategy,
            expected_area, settings);
}

#endif


#include <ros/ros.h>
#include <ros/package.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <std_msgs/Int32MultiArray.h>

#include <moveit/move_group_interface/move_group.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>

#include <yaml-cpp/yaml.h>
#include <string.h>
#include <sstream>
#include "path.h"

bool executePath( moveit::planning_interface::MoveGroup &arm, moveit::planning_interface::MoveGroup::Plan &plan, std::vector<geometry_msgs::Pose>& waypoints, std::string& moveit_group){
  moveit_msgs::RobotTrajectory trajectory_msg;

  double fraction = arm.computeCartesianPath(waypoints,
                                               0.01,  // eef_step
                                               0.0,   // jump_threshold
                                               trajectory_msg, false);
  // The trajectory needs to be modified so it will include velocities as well.
  // First to create a RobotTrajectory object
  robot_trajectory::RobotTrajectory rt(arm.getCurrentState()->getRobotModel(), "manipulator");

  // Second get a RobotTrajectory from trajectory
  rt.setRobotTrajectoryMsg(*arm.getCurrentState(), trajectory_msg);

  // Thrid create a IterativeParabolicTimeParameterization object
  trajectory_processing::IterativeParabolicTimeParameterization iptp;
  // Fourth compute computeTimeStamps
  bool success = iptp.computeTimeStamps(rt);
  ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
  // Get RobotTrajectory_msg from RobotTrajectory
  rt.getRobotTrajectoryMsg(trajectory_msg);
  // Check trajectory_msg for velocities not empty
  //std::cout << trajectory_msg << std::endl;

  plan.trajectory_ = trajectory_msg;
  ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);

  if(fraction == 1){
    ROS_INFO("EXECUTING....");
      arm.execute(plan);
  }
}

int main(int argc, char **argv)
{
  Path path;
  bool success;
  std::string package_path = ros::package::getPath("p3_uvic");

  YAML::Node yaml_config  = YAML::LoadFile(package_path+"/config/trajectory.yaml");

  int trajectories_size = yaml_config["Totalsize"].as<int>();

  std::vector<geometry_msgs::Pose> waypoints;

  ros::init(argc, argv, "p3_uvic");
  ros::NodeHandle nh;

  ros::AsyncSpinner spinner(1);
  spinner.start();

  std::string moveit_group = yaml_config["MoveItGroup"].as<std::string>();
  moveit::planning_interface::MoveGroup group(yaml_config["MoveItGroup"].as<std::string>());
  moveit::planning_interface::MoveGroup::Plan my_plan;
  group.setPlannerId("RRTConnectkConfigDefault");
  group.setPlanningTime(10);
  group.setNumPlanningAttempts(5);
  group.setEndEffectorLink("ee_link");

  group.setStartStateToCurrentState();
  geometry_msgs::Pose startingPose = group.getCurrentPose().pose;
  ROS_INFO_STREAM("Starting POSE: " << startingPose.position.x << "  " << startingPose.position.y << "  " << startingPose.position.z);
  waypoints.clear();

  for(int j=1; j <= trajectories_size; j++){

    YAML::Node trajectory_node = yaml_config["Trajectory" + std::to_string(j)];
    int path_size = trajectory_node["size"].as<int>();
    bool isPose = false;

    for(int k = 1; k <= path_size; k++){

      auto path_node = trajectory_node["Path" + std::to_string(k)];
      std::string path_type = path_node["Type"].as<std::string>();

      if(path_type.find("Circle") != std::string::npos){

        double radius = path_node["Radius"].as<double>();
        double start_angle = path_node["StartAngle"].as<double>();
        double end_angle = path_node["EndAngle"].as<double>();
        double step = path_node["AngleStep"].as<double>();
        int clockwise = (start_angle >= end_angle ) ? -1: 1;

        ROS_INFO_STREAM("CIRCLE: " << radius << "  " << start_angle << "  " << end_angle << "  " << step << " " << clockwise);
        path.getCircleTrajectoryEEPlane(startingPose, radius, start_angle, end_angle, step, clockwise,  waypoints);

        startingPose = waypoints.back();

      }else if (path_type.find("Line") != std::string::npos){

        double lenght = path_node["Lenght"].as<double>();
        int direction = path_node["Direction"].as<double>();             //   1,-1 -> x,-x      2,-2 -> y,-y      3,-3 -> z,-z
        double step = path_node["LenghtStep"].as<double>();

        ROS_INFO_STREAM("LINE : " << lenght << "  " << direction << "  " << step );
        path.getLineTrajectoryEEPlane(startingPose, lenght, direction, step, waypoints);

        startingPose = waypoints.back();

      }else if (path_type.find("Pose") != std::string::npos){

        group.setNamedTarget(path_node["Name"].as<std::string>());
        group.move();
        isPose = true;
      }
    }

    if(!isPose) executePath(group, my_plan, waypoints, moveit_group);

    group.setStartStateToCurrentState();
    startingPose = group.getCurrentPose().pose;
    waypoints.clear();
    ROS_INFO_STREAM("Starting POSE: " << startingPose.position.x << "  " << startingPose.position.y << "  " << startingPose.position.z);
  }

  return 0;
}


#pragma once
#ifndef SFMTAVXEQMPI_HPP
#define SFMTAVXEQMPI_HPP
/**
 * @file SFMTAVXeqmpi.hpp
 */
#include "devavxprng.h"
#include "AlgorithmSIMDEquidistribution.hpp"
//#include <MTToolBox/AlgorithmEquidistribution.hpp>
//#include <MTToolBox/AlgorithmReducibleRecursionSearch.hpp>
//#include <MTToolBox/period.hpp>
//#include <NTL/GF2X.h>
#include "Annihilate.hpp"
#include "EQOptions.hpp"

namespace MTToolBox {

    template<typename U, typename G, typename P, int bitWidth>
    int sfmtavxmpi_equidist(EQOptions<P>& opt, int rank, int num_process)
    {
        using namespace std;
        G sf(opt.params);
        U wseed;
        wseed.u64[0] = opt.seed;
        sf.seed(wseed);
        Annihilate<G, U, uint32_t> annihilate;
        if (!annihilate.anni(sf)) {
            return -1;
        }

        SIMDInfo info;
        info.bitSize = bitWidth;
        info.fastMode = false;
        sf.reset_reverse_bit();
        info.bitMode = 64;
        info.elementNo = bitWidth / 64;
        if (opt.verbose) {
            cout << "64bit dimension of equidistribution at v-bit accuracy k(v)"
                 << endl;
        }
        for (int v = 1; v <= 64; v++) {
            if (v % num_process == rank) {
                int veq = calc_SIMD_equidist<U, G>(v, sf, info,
                                                   opt.params.mexp);
                int d = opt.params.mexp / v - veq;
                if (opt.verbose) {
                    cout << "k(" << dec << v << ") = " << dec << veq;
                    cout << "\td(" << dec << v << ") = " << dec << d << endl;
                }
            }
        }
        info.bitMode = 32;
        info.elementNo = bitWidth / 32;
        if (opt.verbose) {
            cout << "32bit dimension of equidistribution at v-bit accuracy k(v)"
                 << endl;
        }
        for (int v = 1; v <= 32; v++) {
            if (v % num_process == rank) {
                int veq = calc_SIMD_equidist<U, G>
                    (v, sf, info, opt.params.mexp);
                int d = opt.params.mexp / v - veq;
                if (opt.verbose) {
                    cout << "k(" << dec << v << ") = " << dec << veq;
                    cout << "\td(" << dec << v << ") = " << dec << d << endl;
                }
            }
        }
        return 0;
    }
}
#endif // SFMTAVXEQMPI_HPP


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2016 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/multiprecision/cpp_bin_float.hpp>

int main()
{
   typedef boost::multiprecision::number<boost::multiprecision::cpp_bin_float<8, boost::multiprecision::backends::digit_base_2> > quarter_float;

   quarter_float qf(256);

   unsigned int ui = qf.convert_to<unsigned int>();
   if (ui != 256)
      return 1;

   boost::uintmax_t m(1), n;
   m <<= std::numeric_limits<boost::uintmax_t>::digits - 1;
   qf = m;
   n = qf.convert_to<boost::uintmax_t>();
   if (m != n)
      return 2;
   qf *= 2;
   n = qf.convert_to<boost::uintmax_t>();
   m = (std::numeric_limits<boost::uintmax_t>::max)();
   if (m != n)
      return 3;

   qf = 256;
   int si = qf.convert_to<int>();
   if (si != 256)
      return 4;
   boost::intmax_t sm(1), sn;
   sm <<= std::numeric_limits<boost::intmax_t>::digits - 1;
   qf = sm;
   sn = qf.convert_to<boost::intmax_t>();
   if (sm != sn)
      return 5;
   qf *= 2;
   sn = qf.convert_to<boost::intmax_t>();
   sm = (std::numeric_limits<boost::intmax_t>::max)();
   if (sm != sn)
      return 6;

   // Again with negative numbers:
   qf = -256;
   si = qf.convert_to<int>();
   if (si != -256)
      return 7;
   sm = 1;
   sm <<= std::numeric_limits<boost::intmax_t>::digits - 1;
   sm = -sm;
   qf = sm;
   sn = qf.convert_to<boost::intmax_t>();
   if (sm != sn)
      return 8;
   qf *= 2;
   sn = qf.convert_to<boost::intmax_t>();
   sm = (std::numeric_limits<boost::intmax_t>::min)();
   if (sm != sn)
      return 9;

   // Now try conversion to cpp_int:
   qf = 256;
   boost::multiprecision::cpp_int i = qf.convert_to<boost::multiprecision::cpp_int>(), j;
   if (i != 256)
      return 10;
   qf = ldexp(qf, 126);
   i = qf.convert_to<boost::multiprecision::cpp_int>();
   j = 256;
   j <<= 126;
   if (i != j)
      return 11;

   return 0;
}


#include <NTL/ZZ_p.h>
#include <NTL/FFT.h>

NTL_START_IMPL

NTL_TLS_GLOBAL_DECL(SmartPtr<ZZ_pInfoT>, ZZ_pInfo_stg)
NTL_TLS_GLOBAL_DECL(SmartPtr<ZZ_pTmpSpaceT>, ZZ_pTmpSpace_stg)

NTL_CHEAP_THREAD_LOCAL ZZ_pInfoT *ZZ_pInfo = 0;
NTL_CHEAP_THREAD_LOCAL ZZ_pTmpSpaceT *ZZ_pTmpSpace = 0;
NTL_CHEAP_THREAD_LOCAL bool ZZ_pInstalled = false;

ZZ_pInfoT::ZZ_pInfoT(const ZZ& NewP)
{
   if (NewP <= 1) LogicError("ZZ_pContext: p must be > 1");

   p = NewP;
   size = p.size();

   ExtendedModulusSize = 2*size +
                 (NTL_BITS_PER_LONG + NTL_ZZ_NBITS - 1)/NTL_ZZ_NBITS;

}

// we use a lazy strategy for initializing and installing
// FFTInfo and TmpSpace related to a ZZ_p modulus.
// The routines GetFFTInfo and GetTmpSpace make sure this process
// is complete.

void ZZ_p::DoInstall()
{
   SmartPtr<ZZ_pTmpSpaceT> tmps;

   do { // NOTE: thread safe lazy init
      Lazy<ZZ_pFFTInfoT>::Builder builder(ZZ_pInfo->FFTInfo);
      if (!builder()) break;

      UniquePtr<ZZ_pFFTInfoT> FFTInfo;
      FFTInfo.make();

      ZZ B, M, M1, M2, M3;
      long n, i;
      long q, t;
      mulmod_t qinv;

      sqr(B, ZZ_pInfo->p);

      LeftShift(B, B, NTL_FFTMaxRoot+NTL_FFTFudge);

      // FIXME: the following is quadratic time...would
      // be nice to get a faster solution...
      // One could estimate the # of primes by summing logs,
      // then multiply using a tree-based multiply, then
      // adjust up or down...

      // Assuming IEEE floating point, the worst case estimate
      // for error guarantees a correct answer +/- 1 for
      // numprimes up to 2^25...for sure we won't be
      // using that many primes...we can certainly put in
      // a sanity check, though.

      // If I want a more accuaruate summation (with using Kahan,
      // which has some portability issues), I could represent
      // numbers as x = a + f, where a is integer and f is the fractional
      // part.  Summing in this representation introduces an *absolute*
      // error of 2 epsilon n, which is just as good as Kahan
      // for this application.

      // same strategy could also be used in the ZZX HomMul routine,
      // if we ever want to make that subquadratic

      set(M);
      n = 0;
      while (M <= B) {
         UseFFTPrime(n);
         q = GetFFTPrime(n);
         n++;
         mul(M, M, q);
      }

      FFTInfo->NumPrimes = n;
      FFTInfo->MaxRoot = CalcMaxRoot(q);

      double fn = double(n);

      // NOTE: the following checks is somewhat academic,
      // but the implementation relies on it

      if (8.0*fn*(fn+48) > NTL_FDOUBLE_PRECISION)
         ResourceError("modulus too big");

      FFTInfo->rem_struct.init(n, ZZ_pInfo->p, GetFFTPrime);
      FFTInfo->crt_struct.init(n, ZZ_pInfo->p, GetFFTPrime);

      if (!FFTInfo->crt_struct.special()) {
         FFTInfo->prime.SetLength(n);
         FFTInfo->prime_recip.SetLength(n);
         FFTInfo->u.SetLength(n);
         FFTInfo->uqinv.SetLength(n);

         // montgomery
         FFTInfo->reduce_struct.init(ZZ_pInfo->p, ZZ(n) << NTL_SP_NBITS);

         ZZ qq, rr;

         DivRem(qq, rr, M, ZZ_pInfo->p);

         NegateMod(FFTInfo->MinusMModP, rr, ZZ_pInfo->p);

         // montgomery
         FFTInfo->reduce_struct.adjust(FFTInfo->MinusMModP);

         for (i = 0; i < n; i++) {
            q = GetFFTPrime(i);
            qinv = GetFFTPrimeInv(i);

            long tt = rem(qq, q);

            mul(M2, ZZ_pInfo->p, tt);
            add(M2, M2, rr);
            div(M2, M2, q);  // = (M/q) rem p

            div(M1, M, q);
            t = rem(M1, q);
            t = InvMod(t, q);

            // montgomery
            FFTInfo->reduce_struct.adjust(M2);

            FFTInfo->crt_struct.insert(i, M2);

            FFTInfo->prime[i] = q;
            FFTInfo->prime_recip[i] = 1/double(q);
            FFTInfo->u[i] = t;
            FFTInfo->uqinv[i] = PrepMulModPrecon(FFTInfo->u[i], q, qinv);
         }

      }

      tmps = MakeSmart<ZZ_pTmpSpaceT>();
      tmps->crt_tmp_vec.fetch(FFTInfo->crt_struct);
      tmps->rem_tmp_vec.fetch(FFTInfo->rem_struct);

      builder.move(FFTInfo);
   } while (0);

   if (!tmps) {
      const ZZ_pFFTInfoT *FFTInfo = ZZ_pInfo->FFTInfo.get();
      tmps = MakeSmart<ZZ_pTmpSpaceT>();
      tmps->crt_tmp_vec.fetch(FFTInfo->crt_struct);
      tmps->rem_tmp_vec.fetch(FFTInfo->rem_struct);
   }

   NTL_TLS_GLOBAL_ACCESS(ZZ_pTmpSpace_stg);
   ZZ_pTmpSpace_stg = tmps;
   ZZ_pTmpSpace = ZZ_pTmpSpace_stg.get();
}

void ZZ_p::init(const ZZ& p)
{
   ZZ_pContext c(p);
   c.restore();
}

void ZZ_pContext::save()
{
   NTL_TLS_GLOBAL_ACCESS(ZZ_pInfo_stg);
   ptr = ZZ_pInfo_stg;
}

void ZZ_pContext::restore() const
{
   if (ZZ_pInfo == ptr.get()) return;
   // NOTE: this simple optimization could be useful in some situations,
   //    for example, a worker thread re-setting the current modulus
   //    in a multi-threaded build

   NTL_TLS_GLOBAL_ACCESS(ZZ_pInfo_stg);
   ZZ_pInfo_stg = ptr;
   ZZ_pInfo = ZZ_pInfo_stg.get();

   NTL_TLS_GLOBAL_ACCESS(ZZ_pTmpSpace_stg);
   ZZ_pTmpSpace_stg = 0;
   ZZ_pTmpSpace = 0;

   ZZ_pInstalled = false;
}

ZZ_pBak::~ZZ_pBak()
{
   if (MustRestore) c.restore();
}

void ZZ_pBak::save()
{
   c.save();
   MustRestore = true;
}

void ZZ_pBak::restore()
{
   c.restore();
   MustRestore = false;
}

const ZZ_p& ZZ_p::zero()
{
   static const ZZ_p z(INIT_NO_ALLOC); // GLOBAL (assumes C++11 thread-safe init)
   return z;
}

NTL_CHEAP_THREAD_LOCAL
ZZ_p::DivHandlerPtr ZZ_p::DivHandler = 0;

ZZ_p::ZZ_p(INIT_VAL_TYPE, const ZZ& a)  // NO_ALLOC
{
   conv(*this, a);
}

ZZ_p::ZZ_p(INIT_VAL_TYPE, long a) // NO_ALLOC
{
   conv(*this, a);
}

void conv(ZZ_p& x, long a)
{
   if (a == 0)
      clear(x);
   else if (a == 1)
      set(x);
   else {
      NTL_ZZRegister(y);

      conv(y, a);
      conv(x, y);
   }
}

istream& operator>>(istream& s, ZZ_p& x)
{
   NTL_ZZRegister(y);

   NTL_INPUT_CHECK_RET(s, s >> y);
   conv(x, y);

   return s;
}

void div(ZZ_p& x, const ZZ_p& a, const ZZ_p& b)
{
   NTL_ZZ_pRegister(T);

   inv(T, b);
   mul(x, a, T);
}

void inv(ZZ_p& x, const ZZ_p& a)
{
   NTL_ZZRegister(T);

   if (InvModStatus(T, a._ZZ_p__rep, ZZ_p::modulus())) {
      if (!IsZero(a._ZZ_p__rep) && ZZ_p::DivHandler)
         (*ZZ_p::DivHandler)(a);

      InvModError("ZZ_p: division by non-invertible element",
                   a._ZZ_p__rep, ZZ_p::modulus());
   }

   x._ZZ_p__rep = T;
}

long operator==(const ZZ_p& a, long b)
{
   if (b == 0)
      return IsZero(a);

   if (b == 1)
      return IsOne(a);

   NTL_ZZ_pRegister(T);
   conv(T, b);
   return a == T;
}

void add(ZZ_p& x, const ZZ_p& a, long b)
{
   NTL_ZZ_pRegister(T);
   conv(T, b);
   add(x, a, T);
}

void sub(ZZ_p& x, const ZZ_p& a, long b)
{
   NTL_ZZ_pRegister(T);
   conv(T, b);
   sub(x, a, T);
}

void sub(ZZ_p& x, long a, const ZZ_p& b)
{
   NTL_ZZ_pRegister(T);
   conv(T, a);
   sub(x, T, b);
}

void mul(ZZ_p& x, const ZZ_p& a, long b)
{
   NTL_ZZ_pRegister(T);
   conv(T, b);
   mul(x, a, T);
}

void div(ZZ_p& x, const ZZ_p& a, long b)
{
   NTL_ZZ_pRegister(T);
   conv(T, b);
   div(x, a, T);
}

void div(ZZ_p& x, long a, const ZZ_p& b)
{
   if (a == 1) {
      inv(x, b);
   }
   else {
      NTL_ZZ_pRegister(T);
      conv(T, a);
      div(x, T, b);
   }
}

NTL_END_IMPL


#ifndef LUCCA_VIEW_EVENT_HPP
#define LUCCA_VIEW_EVENT_HPP

#include "lucca/config.hpp"

#include "lucca/event/event.hpp"

#include <boost/optional.hpp>

namespace ayla {
	class Ray;
}

namespace lucca {

class View;
class Camera;

/**
 * Event generated on a lucca::View.
 *
 * @author O Programador
 */
class LUCCA_API ViewEvent : public Event {
public:
	ViewEvent(Float x, Float y, const View* sourceView, KeyboardModifierFlags keyboardModifier);
	virtual ~ViewEvent() = 0;

public:
	/**
	 * @return The position of this event in view absolute coordinates.
	 */
	glm::vec2 getPosition() const;

	/**
	 * @return The position of this event in view normalized coordinates.
	 */
	boost::optional<glm::vec2> getNormalizedPosition() const;

	/**
	 * @return The position of this event in normalized device coordinates (NDC).
	 */
	boost::optional<glm::vec2> getNDCPosition(bool invertY = true) const;

public:
	boost::optional<ayla::Ray> getCameraToViewportRay() const;
	boost::optional<glm::vec3> getCameraDirection() const;
	boost::optional<glm::mat4> getViewMatrix() const;
	boost::optional<glm::mat4> getProjectionMatrix() const;

	const View* getSourceView() const;

private:
	const Camera* _getSourceCamera() const;

private:
	const Float _x;
	const Float _y;
	const View* _sourceView;
};

}

#endif // LUCCA_VIEW_EVENT_HPP


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

/*
 Copyright (C) 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <boost/make_shared.hpp>

#include <qlo/defaultbasket.hpp>

#include <ql/currencies/europe.hpp>

#include <ql/experimental/credit/basket.hpp>

namespace QuantLibAddin {

    /* \todo: Add amortizing version
    */
    Basket::Basket(
        const boost::shared_ptr<ObjectHandler::ValueObject>& properties,

        const std::vector<std::string>& names,
        const std::vector<boost::shared_ptr<QuantLib::Issuer> >& issuers,
        const std::vector<QuantLib::Real>& notionals,
        const QuantLib::Date& refDate,
        QuantLib::Real attachRatio,
        QuantLib::Real detachRatio,
        bool amortizes,

        bool permanent
        )
        : ObjectHandler::LibraryObject<QuantLib::Basket>(properties, permanent)
    {
        QL_REQUIRE(names.size() == issuers.size(),
            "Different number of names and issuers.");

        std::vector<QuantLib::DefaultProbKey> contractTriggers(
            names.size(),
            QuantLib::NorthAmericaCorpDefaultKey(QuantLib::EURCurrency(),
                QuantLib::SeniorSec,
                QuantLib::Period(),
                1. // amount threshold
            ));

        boost::shared_ptr<QuantLib::Pool> pool(new QuantLib::Pool());
        for(QuantLib::Size i=0; i<names.size(); i++)
                pool->add(names[i], *issuers[i], contractTriggers[i]);
        libraryObject_ =
            boost::make_shared<QuantLib::Basket>(
                QuantLib::Basket(refDate, names, notionals, pool,
                    attachRatio, detachRatio));
    }

}


#pragma once
// sin.hpp: vector sine function
//
// Copyright (C) 2017-2020 Stillwater Supercomputing, Inc.
//
// This file is part of the HPR-DSP project, which is released under an MIT Open Source License
#include <boost/numeric/mtl/mtl.hpp>

namespace sw {
namespace hprdsp {

	// TODO: we need to protect this to access a Vector concept (needs to have size() and operator[]
	// Vector = mtl::dense_vector<Real>

template<bool value>
using enableif = typename std::enable_if<value>::type;

	// is T a Vector?
template<typename T>
struct isVector { constexpr static bool value = false; };

template<typename R>
struct isVector<mtl::dense_vector<R> > { constexpr static bool value = true; };

	/*
	MTL already has vector-valued trigonometry functions

// vector valued sine function
template<typename Vector, enableif< isVector<Vector>::value >...>
Vector sin(const Vector& timeSamples) {
	Vector v(size(timeSamples));
	unsigned i = 0;
	for (auto t : timeSamples) {
		v[i++] = sin(t);
	}
	return v;
}
	*/

} // namespace hprdsp
} // namespace sw


// Note: this is C++ #3 program coming in at spot 1.9 (relative to the top entry as C++ #7).
// We use this one as both of the top 2 entries (#5 and #7) use different kind of parallelism
// with parallel execution within the iterations of sum-count.
// (it would be interesting to also create a Koka version of this but it requires a work-stealing implementation).
// Even #3 is a bit too specialized as it uses the Boost object pools for allocation
// which frees all objects at once instead of freeing per node (like Koka and the other languages do).

/* The Computer Language Benchmarks Game
 * https://salsa.debian.org/benchmarksgame-team/benchmarksgame/
 *
 * contributed by Jon Harrop
 * modified by Alex Mizrahi
 * modified by Andreas Schäfer
 * very minor omp tweak by The Anh Tran
 * Jeff Wofford: replaced omp dependency with C++11 threading and load-based job distribution.
 *  *reset*
 */

#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <thread>
#include <mutex>
#include <vector>

#include <boost/pool/object_pool.hpp>

const size_t   LINE_SIZE = 64;

struct Node
{
   Node *l, *r;

   Node() : l(0), r(0)
   {}
   Node(Node *l2, Node *r2) : l(l2), r(r2)
   {}

   int check() const
   {
      if (l)
         return l->check() + 1 + r->check();
      else return 1;
   }
};

typedef boost::object_pool<Node> NodePool;

Node *make(int d, NodePool &store)
{
   if (d > 0)
      return store.construct(   make(d-1, store),
                        make(d-1, store)   );
   return store.construct();
}

const unsigned THREADS_TO_USE = std::max( 1U, std::min( 4U, std::thread::hardware_concurrency() ));

int main(int argc, char *argv[])
{
   const int min_depth = 4;
   const int max_depth = std::max(min_depth+2,
                      (argc == 2 ? atoi(argv[1]) : 21));
   const int stretch_depth = max_depth+1;

   // Alloc then dealloc stretchdepth tree
   {
      NodePool store;
      Node *c = make(stretch_depth, store);
      std::cout << "stretch tree of depth " << stretch_depth << "\t "
              << "check: " << c->check() << std::endl;
   }

   NodePool long_lived_store;
   Node *long_lived_tree = make(max_depth, long_lived_store);

   // buffer to store output of each thread
   std::unique_ptr< char > outputstr{ new char[ LINE_SIZE * (max_depth + 1)] };

   std::mutex mutex;
   int nextDepthToProcess = min_depth;

   const auto work = [&]()
   {
      while(true)
      {
         std::unique_lock< decltype(mutex) > lock(mutex);
         const int d = nextDepthToProcess;
         if(d > max_depth)
         {
            return;
         }
         nextDepthToProcess += 2;
         lock.unlock();

         const int iterations = 1 << (max_depth - d + min_depth);
         int checksum = 0;

         for(int i = 1; i <= iterations; ++i)
         {
            NodePool store;
            Node *a = make(d, store);
            checksum += a->check();
         }

         // each thread write to separate location
         sprintf(outputstr.get() + LINE_SIZE * d, "%d\t trees of depth %d\t check: %d\n", iterations, d, checksum);
      }
   };

   std::vector< std::thread > threads(THREADS_TO_USE - 1);
   for(auto& thread : threads)
   {
      thread = std::thread{ work };
   }

   work();

   for(auto& thread : threads)
   {
      thread.join();
   }

   // print all results
   for (int d = min_depth; d <= max_depth; d += 2)
      printf("%s", outputstr.get() + (d * LINE_SIZE) );

   std::cout << "long lived tree of depth " << max_depth << "\t "
           << "check: " << (long_lived_tree->check()) << "\n";

   return 0;
}


#include "temperature.h"
#include <bit>
#include <boost/accumulators/statistics/stats.hpp>
#include <gsl/gsl-lite.hpp>

namespace angonoka::stun {
Temperature::Temperature(
    Beta beta,
    BetaScale beta_scale,
    StunWindow stun_window,
    RestartPeriod restart_period)
    : value{beta}
    , acc{tag::rolling_window::window_size
          = static_cast<std::int_fast32_t>(stun_window)}
    , beta_scale{beta_scale}
    , restart_period_mask{
          static_cast<std::size_t>(restart_period) - 1}
{
    Expects(beta > 0.F);
    Expects(beta_scale > 0.F);
    Expects(static_cast<std::int_fast32_t>(stun_window) > 0);
    Expects(static_cast<std::size_t>(restart_period) > 0);
    Expects(
        std::popcount(static_cast<std::size_t>(restart_period)) == 1);
}

[[nodiscard]] float Temperature::average_stun() const noexcept
{
    using boost::accumulators::rolling_mean;
    return rolling_mean(acc);
}

void Temperature::update(float stun) noexcept
{
    using boost::accumulators::count;
    Expects(stun >= 0.F);

    acc(stun);
    const auto dampening
        = static_cast<float>(count(acc) & restart_period_mask)
        / static_cast<float>(restart_period_mask + 1);
    const auto diff = average_stun() - 0.03F;
    const auto t = 1.F - dampening;
    value *= 1.F + diff * beta_scale * t * t;

    Ensures(value >= 0.F);
}

Temperature& Temperature::operator=(Temperature&& other) noexcept
{
    value = other.value;
    try {
        acc = other.acc;
    } catch (...) {
        // 99% certain that the missing noexcept on
        // accumulator's copy ctor is a bug in
        // a boost lib.
#ifdef __llvm__
        __builtin_unreachable();
#endif // __llvm__
    }
    beta_scale = other.beta_scale;
    restart_period_mask = other.restart_period_mask;
    return *this;
}

Temperature::Temperature(const Temperature& other) = default;
Temperature::Temperature(Temperature&& other) noexcept = default;
Temperature& Temperature::operator=(const Temperature& other)
    = default;
Temperature::~Temperature() noexcept = default;
} // namespace angonoka::stun


/*
 * Copyright 2022 Sean McBane
 *
 * 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 MESH_TRAITS_HPP
#define MESH_TRAITS_HPP

#include <type_traits>
#include <variant>

#include <Eigen/Core>

#include "Galerkin.hpp"
#include "TetMesh.hpp"

#ifndef ELASTICITY_MAX_ELEMENT_ORDER
#define ELASTICITY_MAX_ELEMENT_ORDER 2
#endif

namespace msh
{

template <class T>
struct IsTetMesh : public std::false_type
{
};

template <class CoordT, size_t... Others>
struct IsTetMesh<TetMesh<CoordT, Others...>> : public std::true_type
{
};

template <size_t NPF, size_t IN>
constexpr int which_poly_order() noexcept
{
    if constexpr (NPF == 0 && IN == 0)
    {
        return 1;
    }
    else if constexpr (NPF == 1 && IN == 0)
    {
        return 2;
    }
    else if constexpr (NPF == 2 && IN == 1)
    {
        return 3;
    }
    else if constexpr (NPF == 3 && IN == 3)
    {
        return 4;
    }
    else
    {
        return -1;
    }
}

template <class Mesh>
struct ElementOrder : public std::integral_constant<int, -1>
{
};

template <class CoordT, size_t MEA, size_t MNA, size_t NPF, size_t IN>
struct ElementOrder<TetMesh<CoordT, MEA, MNA, NPF, IN>>
    : public std::integral_constant<int, which_poly_order<NPF, IN>()>
{
};

template <class Mesh>
constexpr int element_order = ElementOrder<Mesh>::value;

} // namespace msh

namespace Elasticity
{

constexpr size_t max_node_adjacencies = 96;

constexpr size_t max_element_adjacencies = 16;

using Mesh1 = msh::TetMesh<double, max_element_adjacencies, max_node_adjacencies, 0, 0>;
using Mesh2 = msh::TetMesh<double, max_element_adjacencies, max_node_adjacencies, 1, 0>;
using Mesh3 = msh::TetMesh<double, max_element_adjacencies, max_node_adjacencies, 2, 1>;
using Mesh4 = msh::TetMesh<double, max_element_adjacencies, max_node_adjacencies, 3, 3>;

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
using MeshVariant = std::variant<Mesh1, Mesh2, Mesh3, Mesh4>;
#elif ELASTICITY_MAX_ELEMENT_ORDER > 2
using MeshVariant = std::variant<Mesh1, Mesh2, Mesh3>;
#else
using MeshVariant = std::variant<Mesh1, Mesh2>;
#endif

template <int order>
using MeshType = std::decay_t<decltype(std::get<order - 1>(std::declval<MeshVariant>()))>;

template <int Order>
struct CurveMesh
{
    CurveMesh(Eigen::Matrix2Xd &&coords) : m_coords(std::move(coords))
    {
        std::array<size_t, Order + 1> tmp;
        size_t d = 0;
        double curr_point = 0;
        for (int i = 0; i < m_coords.cols() - 1; ++i)
        {
            Eigen::Vector2d diff = m_coords.col(i + 1) - m_coords.col(i);
            double dist = std::sqrt(diff.dot(diff));
            m_elements.push_back(Galerkin::Elements::IntervalElement<Order, double>(
                curr_point, curr_point + dist));
            for (size_t j = 0; j <= Order; ++j)
            {
                tmp[j] = d + j;
            }
            m_dofs.push_back(tmp);
            d += Order;
        }
    }

    const auto &elements() const noexcept { return m_elements; }

    const auto &dofs() const noexcept { return m_dofs; }

    size_t num_nodes() const noexcept { return m_elements.size() * Order + 1; }

    Eigen::Matrix2d get_element_endpoints(int eli) const
    {
        Eigen::Matrix2d endpoints;
        endpoints.col(0) = m_coords.col(eli);
        endpoints.col(1) = m_coords.col(eli + 1);
        return endpoints;
    }

    constexpr static int order() noexcept { return Order; }

  private:
    Eigen::Matrix2Xd m_coords;
    std::vector<Galerkin::Elements::IntervalElement<Order, double>> m_elements;
    std::vector<std::array<size_t, Order + 1>> m_dofs;
};

#if ELASTICITY_MAX_ELEMENT_ORDER > 3
using CurveMeshVariant = std::variant<CurveMesh<1>, CurveMesh<2>, CurveMesh<3>, CurveMesh<4>>;
#elif ELASTICITY_MAX_ELEMENT_ORDER > 2
using CurveMeshVariant = std::variant<CurveMesh<1>, CurveMesh<2>, CurveMesh<3>>;
#else
using CurveMeshVariant = std::variant<CurveMesh<1>, CurveMesh<2>>;
#endif

} // namespace Elasticity

#endif // MESH_TRAITS_HPP


#include <Eigen/Core>
#include <Eigen/Sparse>
#include <algorithm>
#include <atomic>
#include <cstddef>
#include <future>
#include <iostream>
#include <string>
#include <unordered_set>
#include <vector>

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

#include "argcheck.hpp"

namespace irspack {

using CountVector = Eigen::Matrix<std::int64_t, Eigen::Dynamic, 1>;
struct Metrics {
  // This isn't necessary, but MSVC complains Metric is not default
  // constructible.
  inline Metrics() : Metrics(0) {}
  inline Metrics(size_t n_item) : n_item(n_item), item_cnt(n_item) {
    item_cnt.array() = 0;
  }

  inline void merge(const Metrics &other) {
    hit += other.hit;
    recall += other.recall;
    ndcg += other.ndcg;
    total_user += other.total_user;
    valid_user += other.valid_user;
    item_cnt += other.item_cnt;
    precision += other.precision;
    map += other.map;
  }

  std::map<std::string, double> as_dict() const {
    std::map<std::string, double> result;
    CountVector count_local(item_cnt);
    double total_item = item_cnt.sum();
    std::sort(count_local.data(), count_local.data() + item_cnt.rows());
    double apperere_item = 0;
    double entropy = 0;
    double gini = 0;

    for (int i = 0; i < item_cnt.rows(); i++) {
      int64_t cnt = count_local(i);
      if (cnt == 0) {
        continue;
      }
      double p = cnt / total_item;
      apperere_item++;
      entropy += -std::log(p) * p;
      gini += (2 * i - item_cnt.rows() + 1) * cnt;
    }
    gini /= (item_cnt.rows() * total_item);

    size_t denominator = valid_user > 0u ? valid_user : 1;
    result["total_user"] = total_user;
    result["valid_user"] = valid_user;
    result["n_items"] = n_item;
    result["hit"] = hit / denominator;
    result["ndcg"] = ndcg / denominator;
    result["recall"] = recall / denominator;
    result["map"] = map / denominator;
    result["precision"] = precision / denominator;
    result["appeared_item"] = apperere_item;
    result["entropy"] = entropy;
    result["gini_index"] = gini;
    return result;
  }

  size_t valid_user = 0;
  size_t total_user = 0;
  double hit = 0;
  double recall = 0;
  double ndcg = 0;
  double precision = 0;
  double map = 0;
  size_t n_item;
  CountVector item_cnt;
};

struct EvaluatorCore {
  using Real = double;
  using SparseMatrix = Eigen::SparseMatrix<Real, Eigen::RowMajor>;

  template <typename ScoreFloatType>
  using DenseMatrix = Eigen::Matrix<ScoreFloatType, Eigen::Dynamic,
                                    Eigen::Dynamic, Eigen::RowMajor>;

  EvaluatorCore(const SparseMatrix &X,
                const std::vector<std::vector<size_t>> &recommendable_items)
      : X_(X), n_users(X.rows()), n_items(X.cols()),
        recommendable_items(recommendable_items) {
    check_arg(recommendable_items.empty() ||
                  (recommendable_items.size() == 1) ||
                  static_cast<size_t>(X.rows()) == recommendable_items.size(),
              "recommendable.size.() must be in {0, 1, ground_truth.size()}");
    X_.makeCompressed();
    // sort & bound check
    for (auto &urec : this->recommendable_items) {
      std::sort(urec.begin(), urec.end());
      if (!urec.empty()) {
        check_arg(urec.back() < static_cast<size_t>(X_.cols()),
                  "recommendable items contain a index >= n_items.");
        auto prev = urec[0];
        for (size_t i = 1; i < urec.size(); i++) {
          auto current = urec[i];
          check_arg(current > prev, "duplicate recommendable items.");
          prev = current;
        }
      }
    }
  }

  inline void cache_X_map(size_t n_threads) {
    check_arg(n_threads > 0, "n_threads must be strictly positive.");
    if (!this->X_as_set.empty()) {
      return;
    }

    const int64_t n_rows = this->X_.rows();
    this->X_as_set.resize(this->X_.rows());
    std::atomic<std::int64_t> cursor(0);
    std::vector<std::future<void>> workers;
    for (size_t th = 0; th < n_threads; th++) {
      workers.emplace_back(std::async([this, &cursor, n_rows]() {
        while (true) {
          int64_t current = cursor.fetch_add(1);
          if (current >= n_rows) {
            break;
          }
          auto &target = this->X_as_set[current];
          for (SparseMatrix::InnerIterator iter(this->X_, current); iter;
               ++iter) {
            target.insert(iter.col());
          }
        }
      }));
    }
  }

  template <typename ScoreFloatType>
  inline Metrics
  get_metrics(const Eigen::Ref<DenseMatrix<ScoreFloatType>> &scores,
              size_t cutoff, size_t offset, size_t n_threads,
              bool recall_with_cutoff = false) {
    this->cache_X_map(n_threads);
    Metrics overall(n_items);
    check_arg(n_threads > 0, "n_threads == 0");
    check_arg(n_users > offset, "got offset >= n_users");
    check_arg(static_cast<size_t>(offset + scores.rows()) <= n_users,
              "offset + scores.shape[0] exceeds n_users");
    check_arg(cutoff > 0, "cutoff must be strictly greather than 0.");
    check_arg(cutoff <= n_items, "cutoff must not exeeed the number of items.");
    std::atomic<int64_t> current_index(0);

    std::vector<std::future<Metrics>> workers;
    for (size_t th = 0; th < n_threads; th++) {
      workers.emplace_back(std::async(
          std::launch::async, [this, &current_index, &scores, cutoff,
                               offset, recall_with_cutoff]() {
            return this->get_metrics_local(scores, current_index, cutoff,
                                           offset, recall_with_cutoff);
          }));
    }
    for (auto &metric : workers) {
      overall.merge(metric.get());
    }
    return overall;
  }

  inline SparseMatrix get_ground_truth() const { return this->X_; }
  inline std::vector<std::vector<size_t>> get_recommendable_items() const {
    return this->recommendable_items;
  }

private:
  template <typename ScoreFloatType>
  inline Metrics
  get_metrics_local(const Eigen::Ref<DenseMatrix<ScoreFloatType>> &scores,
                    std::atomic<int64_t> &current_index, size_t cutoff,
                    size_t offset, bool recall_with_cutoff = false) const {
    using StorageIndex = typename SparseMatrix::StorageIndex;

    Metrics metrics_local(n_items);

    const ScoreFloatType *score_begin = scores.data();
    std::vector<std::pair<ScoreFloatType, StorageIndex>> score_and_index;
    score_and_index.reserve(n_items);
    std::vector<StorageIndex> intersection(cutoff);
    std::vector<double> dcg_discount(cutoff);
    for (size_t i = 0; i < cutoff; i++) {
      dcg_discount[i] = 1 / std::log2(2 + i);
    }

    size_t n_recommendable_items = std::min(cutoff, n_items);
    while (true) {
      auto u = current_index.fetch_add(1);
      if (u >= scores.rows()) {
        break;
      }
      auto buffer = score_begin + n_items * u;
      int u_orig = u + offset;

      const auto &gt_indices = this->X_as_set.at(u_orig);
      metrics_local.total_user += 1;
      score_and_index.clear();

      size_t n_gt = 0;
      const auto n_items_signed = static_cast<StorageIndex>(n_items);
      if (this->recommendable_items.empty()) {
        auto score_loc = buffer;
        for (StorageIndex _ = 0; _ < n_items_signed; _++) {
          score_and_index.emplace_back(-*(score_loc++), _);
        }
        n_gt = gt_indices.size();
      } else if (this->recommendable_items.size() == 1u) {
        const auto &rec_items_global = recommendable_items[0];
        for (auto i : rec_items_global) {
          score_and_index.emplace_back(-buffer[i], i);
          if (gt_indices.find(i) != gt_indices.cend()) {
            n_gt++;
          }
        }
        n_recommendable_items = std::min(cutoff, recommendable_items[0].size());
      } else {
        const auto &item_local = this->recommendable_items[u_orig];
        for (auto i : item_local) {
          score_and_index.emplace_back(-buffer[i], i);
          if (gt_indices.find(i) != gt_indices.cend()) {
            n_gt++;
          }
        }
        n_recommendable_items = std::min(cutoff, item_local.size());
      }

      if ((n_gt == 0) || (n_recommendable_items == 0)) {
        continue;
      }

      std::partial_sort(score_and_index.begin(),
                        score_and_index.begin() + n_recommendable_items,
                        score_and_index.end());

      metrics_local.valid_user += 1;
      double dcg = 0;
      double idcg = std::accumulate(
          dcg_discount.begin(),
          dcg_discount.begin() + std::min(n_gt, n_recommendable_items), 0.);
      double average_precision = 0;

      size_t cum_hit = 0;
      for (size_t i = 0; i < n_recommendable_items; i++) {
        auto rec_index = score_and_index[i].second;
        metrics_local.item_cnt(rec_index) += 1;
        if (gt_indices.find(rec_index) == gt_indices.cend()) {
        } else {
          dcg += dcg_discount[i];
          cum_hit++;
          average_precision += (static_cast<Real>(cum_hit) / (i + 1));
        }
      }
      if (cum_hit > 0) {
        metrics_local.hit += 1;
      }
      metrics_local.precision +=
          cum_hit / static_cast<double>(n_recommendable_items);
      metrics_local.recall +=
          cum_hit / static_cast<double>(recall_with_cutoff
                                            ? (n_gt > cutoff ? cutoff : n_gt)
                                            : n_gt);
      metrics_local.ndcg += (dcg / idcg);
      metrics_local.map += average_precision / n_gt;
    }
    return metrics_local;
  }

  SparseMatrix X_;
  const size_t n_users;
  const size_t n_items;
  std::vector<std::vector<size_t>> recommendable_items;
  std::vector<std::unordered_set<int64_t>> X_as_set;
}; // namespace irspack
} // namespace irspack

namespace py = pybind11;
using namespace irspack;

PYBIND11_MODULE(_core, m) {
  py::class_<Metrics>(m, "Metrics")
      .def(py::init<size_t>())
      .def("merge",
           [](Metrics &this_, const Metrics &other) { this_.merge(other); })
      .def("as_dict", &Metrics::as_dict);

  py::class_<EvaluatorCore>(m, "EvaluatorCore")
      .def(py::init<const typename EvaluatorCore::SparseMatrix &,
                    const std::vector<std::vector<size_t>> &>(),
           py::arg("grount_truth"), py::arg("recommendable"))
      .def("get_metrics_f64",
           static_cast<Metrics (EvaluatorCore::*)(
               const Eigen::Ref<EvaluatorCore::DenseMatrix<double>> &, size_t,
               size_t, size_t, bool)>(&EvaluatorCore::get_metrics<double>),
           py::arg("score_array"), py::arg("cutoff"), py::arg("offset"),
           py::arg("n_threads"), py::arg("recall_with_cutoff") = false)
      .def("get_metrics_f32",
           static_cast<Metrics (EvaluatorCore::*)(
               const Eigen::Ref<EvaluatorCore::DenseMatrix<float>> &, size_t,
               size_t, size_t, bool)>(&EvaluatorCore::get_metrics<float>),
           py::arg("score_array"), py::arg("cutoff"), py::arg("offset"),
           py::arg("n_threads"), py::arg("recall_with_cutoff") = false)
      .def("get_ground_truth", &EvaluatorCore::get_ground_truth)
      .def("cache_X_as_set", &EvaluatorCore::cache_X_map)
      .def(py::pickle(
          [](const EvaluatorCore &evaluator) {
            return py::make_tuple(evaluator.get_ground_truth(),
                                  evaluator.get_recommendable_items());
          },
          [](py::tuple t) {
            if (t.size() != 2)
              throw std::runtime_error("invalid state");
            return EvaluatorCore(t[0].cast<EvaluatorCore::SparseMatrix>(),
                                 t[1].cast<std::vector<std::vector<size_t>>>());
          }));

  ;
}


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

#include "root_sift.hpp"

#include <cmath>

#include <glog/logging.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <boost/shared_ptr.hpp>

#include "cpp/third_party/vlfeat/vl/dsift.h"

namespace vision
{
namespace features
{

struct VlFree {
  void operator()(void* x){
    if (x != NULL){
      vl_free(x);
    }
  }
};

struct VlDsiftDelete{
  void operator()(void* x){
    if (x != NULL){
      vl_dsift_delete((VlDsiftFilter*)x);
    }
  }
};

RootSift::RootSift()
{
}

/*virtual*/RootSift::~RootSift()
{
}
/*virtual*/ unsigned int RootSift::descriptorLength() const
{
  return 128;
}

/*virtual*/ cv::Mat_<float> RootSift::denseExtract(
    const cv::Mat& image,
    std::vector<cv::Point>& locations,
    const cv::Mat_<uchar>& mask ) const
{

  const int step = 4;

  locations.clear();

  CHECK(mask.data == NULL) << "rootsift does not support masks";

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

  // convert to float image
  cv::Mat_<float> float_img;
  gray.convertTo(float_img, CV_32FC1, 1./255);

  // from https://github.com/vlfeat/vlfeat/blob/master/apps/recognition/getDenseSIFT.m
  VlDsiftDescriptorGeometry geom ;
  geom.numBinX = 4 ;
  geom.numBinY = 4 ;
  geom.numBinT = 8 ;
  geom.binSizeX = 8 ;
  geom.binSizeY = 8 ;

  boost::shared_ptr<VlDsiftFilter> dsift(
      vl_dsift_new(gray.cols, gray.rows),
      VlDsiftDelete());
  vl_dsift_set_geometry(dsift.get(), &geom) ;
  vl_dsift_set_steps(dsift.get(), step, step) ;
  vl_dsift_set_flat_window(dsift.get(), false) ; // opt_fast

  const int num_features = vl_dsift_get_keypoint_num (dsift.get());
  const int descriptor_dims = vl_dsift_get_descriptor_size(dsift.get());
  DCHECK_EQ(descriptorLength(), descriptor_dims);
  geom = *vl_dsift_get_geometry(dsift.get());

  vl_dsift_process(dsift.get(), float_img[0]) ;

  const VlDsiftKeypoint* frames = vl_dsift_get_keypoints(dsift.get());
  const float* descrs = vl_dsift_get_descriptors(dsift.get()) ;

  // copy
  locations.resize(num_features);
  for (int idx = 0; idx < num_features; ++idx){
    cv::Point& p = locations[idx];
    p.x = frames[idx].x;
    p.y = frames[idx].y;
  }
  cv::Mat_<float> descriptors(num_features, descriptor_dims);
  float* out_ptr = descriptors[0];
  float const* src_ptr = descrs;
  for (int i = 0; i < descriptor_dims * num_features; ++i){
    out_ptr[i] = std::sqrt(std::min(512.0f * src_ptr[i], 255.0f));
  }
  // rootsift
  for (int r = 0; r < descriptors.rows; ++r){
    const double l2_norm = cv::norm(descriptors.row(r), cv::NORM_L2);
    if (l2_norm != 0){
      descriptors.row(r) /= l2_norm;
    }
  }

  return descriptors;
}

/*virtual*/ void RootSift::extract_at_extremas(const cv::Mat& image,
    cv::Mat_<float>* descriptors,
    const cv::Mat_<uchar>& mask) const {

  CHECK(false);
}

//==============================================================================

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


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

#include <Eigen/Dense>

#include <stdio.h>
#include "net_impl.h"

namespace py = pybind11;

typedef Matrixf MatrixPy;

typedef MatrixPy::Scalar Scalar;
constexpr bool rowMajor = MatrixPy::Flags & Eigen::RowMajorBit;

class Vocoder {
    Model model;
    bool isLoaded;
public:
    Vocoder() { isLoaded = false; }
    void loadWeights( const std::string& fileName ){
        FILE* fd = fopen(fileName.c_str(), "rb");
        if( not fd ){
            throw std::runtime_error("Cannot open file.");
        }
        model.loadNext(fd);
        isLoaded = true;
    }

    Vectorf melToWav( Eigen::Ref<const MatrixPy> mels ){

        if( not isLoaded ){
            throw std::runtime_error("Model hasn't been loaded. Call loadWeights first.");
        }
        return model.apply(mels);
    }

};

PYBIND11_MODULE(WaveRNNVocoder, m){
    m.doc() = "WaveRNN Vocoder";

    py::class_<MatrixPy>( m, "Matrix", py::buffer_protocol() )
            .def("__init__", [](MatrixPy &m, py::buffer b) {
        typedef Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic> Strides;

        /* Request a buffer descriptor from Python */
        py::buffer_info info = b.request();

        /* Some sanity checks ... */
        if (info.format != py::format_descriptor<Scalar>::format())
            throw std::runtime_error("Incompatible format: expected a float32 array!");

        if (info.ndim != 2)
            throw std::runtime_error("Incompatible buffer dimension!");

        auto strides = Strides(
                    info.strides[rowMajor ? 0 : 1] / (py::ssize_t)sizeof(Scalar),
                info.strides[rowMajor ? 1 : 0] / (py::ssize_t)sizeof(Scalar));

        auto map = Eigen::Map<MatrixPy, 0, Strides>(
                    static_cast<Scalar *>(info.ptr), info.shape[0], info.shape[1], strides);

        new (&m) MatrixPy(map);
    });

    py::class_<Vocoder>( m, "Vocoder")
            .def(py::init())
            .def("loadWeights", &Vocoder::loadWeights )
            .def("melToWav", &Vocoder::melToWav )
            ;
}


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

#include <boost/simd/toolbox/arithmetic/functions/fma.hpp>
#include <boost/simd/toolbox/operator/functions/plus.hpp>
#include <boost/simd/toolbox/operator/functions/multiplies.hpp>
#include <boost/dispatch/dsl/category.hpp>
#include <boost/dispatch/functor/preprocessor/call.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::plus_ , tag::formal_, (D)(A0)(A1)
                                   , ((node_<A0, boost::simd::tag::multiplies_, mpl::long_<2> , D>))(unspecified_<A1>)
                                   )
  {
    BOOST_DISPATCH_RETURNS(2, (A0 const& a0, A1 const& a1),
      fma(boost::proto::child_c<0>(a0), boost::proto::child_c<1>(a0), a1)
    )
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::plus_ , tag::formal_, (D)(A0)(A1)
                                   , (unspecified_<A0>)((node_<A1, boost::simd::tag::multiplies_, mpl::long_<2> , D>))
                                   )
  {
    BOOST_DISPATCH_RETURNS(2, (A0 const& a0, A1 const& a1),
      fma(boost::proto::child_c<0>(a1), boost::proto::child_c<1>(a1), a0)
    )
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::plus_ , tag::formal_, (D)(A0)(A1)
                                   , ((node_<A0, boost::simd::tag::multiplies_, mpl::long_<2> , D>))((node_<A1, boost::simd::tag::multiplies_, mpl::long_<2> , D>))
                                   )
  {
    BOOST_DISPATCH_RETURNS(2, (A0 const& a0, A1 const& a1),
      fma(boost::proto::child_c<0>(a0), boost::proto::child_c<1>(a0), a1)
    )
  };
} } }

#endif


#ifndef PLANE_DETECTOR__
#define PLANE_DETECTOR__

#include <vector>
#include <random>
#include <queue>
#include "detected_plane.hpp"
#include <eigen3/Eigen/Geometry>
#include <eigen3/Eigen/Eigenvalues>
#include <boost/array.hpp>
#include <boost/thread.hpp>
#include <exception>
#include <opencv2/opencv.hpp>
#include <iostream>

#define __MIN_RANGE__ 0.3
#define __MAX_RANGE__ 10.0

struct UnknownDepthException:public std::exception {
  const char *what() const throw()
  {
    return "Unknown Depth Exception";
  }

};

enum PixelStatus {
  UNPROCESSED = -3, IN_R_PRIMA, IN_QUEUE
};

//! @class PlaneDetector
//! @brief Implements a fast plane detector on RGB-D images as presented in Poppinga IROS 2008
class PlaneDetector {
public:
  //! Recommended constructor
  PlaneDetector(double delta = 1.0, double epsilon = 1.0, double gamma = 10.0, int theta = 1000, double _std_dev = 0.03);

  //! @brief Detects the planes inside the image
  int detectPlanes(const cv::Mat &depth);

  inline std::vector<DetectedPlane> getPlanes() const {
    return _detected_planes;
  }

  void resetStatus();

  inline int getPos(int i, int j) const {
    return i + j * _width;
  }

  void setCameraParameters(const boost::array< double, 9 >& k_);

  inline bool isInitialized() {return _initialized;}

  inline bool setFloatImage(bool new_value) {_float_image = new_value;}

protected:
  double _delta, _epsilon, _gamma; // Dynamically reconfigurable
  int _theta;
  std::vector <DetectedPlane> _detected_planes; // Vector de IDs
  std::vector <int> _detected_ids; // Contiene todos los IDs que son considerados planos
  std::vector<int> _status_vec; // Relaciona los puntos de la imagen con una region
  std::queue<int> _q;
  int _available_pixels;
  bool _initialized;
  bool _downsample;
  double _std_dev;
  cv::Mat _image;

  std::vector<int> _curr_region; // Saves the region in coordinates --> i + j * _height
  DetectedPlane _curr_plane;

  // Internal stuff for update Matrices
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> _es;
  DetectedPlane _p;

  // Attributes of the image
  int _width, _height;
  bool _float_image;

  // Camera parameters
  Eigen::Matrix3d _K;
  float _kx, _cx, _ky , _cy; // Taken from camera parameters

  std::vector<Eigen::Vector3d> _color;

  int getRandomPixel(int region = (int)UNPROCESSED) const;
  //! @brief Gets the unprocessed nearest neighbor of the image
  int getNearestNeighbor(int index) const;
  //! @brief Gets the amount of available pixels in image
  int availablePixels() const;

  // For random numbers
  static std::default_random_engine generator;

  //! @brief Adds pixels to region and adds its nearest neighbors to the queue (modifying status_vec)
  void addPixelToRegion(int index, int _curr_region_id);

  Eigen::Vector3d get3DPoint(int i, int j) const;
  Eigen::Vector3d get3DPoint(int index) const;
  //! @brief Gets the depth of a depth image
  //! @return THe depth value or -1.0 if the data is not valid
  double getDepth(int i, int j) const;

  //! @brief updates s_g, m_k, p_k and MSE
  //! @retval true The conditions of step 8 of the matrix (see [1]) are met --> the matrices are updated
  bool  updateMatrices(const Eigen::Vector3d &v);

  //! @brief Initialize the color matrix
  void initializeColors();
};

std::default_random_engine PlaneDetector::generator;

PlaneDetector::PlaneDetector(double delta, double epsilon, double gamma, int theta, double _std_dev):
_delta(delta), _epsilon(epsilon), _gamma(gamma), _theta(theta),_initialized(false), _downsample(true),_std_dev(_std_dev)
{
  initializeColors();
}

void PlaneDetector::initializeColors() {
  // Different colors for planes
  Eigen::Vector3d v;
  v(0) = 0.0; v(1) = 1.0; v(2) = 0.0;
  _color.push_back(v);
  v(0) = 1.0; v(1) = 0.0; v(2) = 0.0;
  _color.push_back(v);
  v(0) = 0.0; v(1) = 0.0; v(2) = 1.0;
  _color.push_back(v);
  v(0) = 0.0; v(1) = 1.0; v(2) = 1.0;
  _color.push_back(v);
  v(0) = 1.0; v(1) = 0.0; v(2) = 1.0;
  _color.push_back(v);
  v(0) = 1.0; v(1) = 1.0; v(2) = 0.0;
  _color.push_back(v);

}

int PlaneDetector::detectPlanes(const cv::Mat& image)
{
  if (!_initialized) {
    // The camera parameters have to be set before detecting planes
    return 0;
  }
  // First: get parameters of the image (MUST BE AT THE FIRST)
  _width = image.cols;
  _height = image.rows;
  this->_image = image; // TODO: será lento?

  if (_downsample) {
    _width /= 2;
    _height /= 2;
  }

  // All internal status to zero or proper values
  resetStatus();
  _q.empty();
  int _curr_region_id = 0;

  _available_pixels = availablePixels();
  while (_available_pixels > _theta * 1.1)
  {
//     std::cout << "Available pixels: " << _available_pixels << std::endl;
    _curr_region.clear();
    // Initialization of the algorithm--> random point and nearest neighbor
    int candidate = getRandomPixel();
    int nearest = getNearestNeighbor(candidate);

    if (nearest > 0)
    {
      _status_vec[nearest] = _curr_region_id;
      addPixelToRegion(candidate, _curr_region_id);
      addPixelToRegion(nearest, _curr_region_id);

      // Initialize matrices and vectors
      Eigen::Vector3d r_1 = get3DPoint(candidate);
      Eigen::Vector3d r_2 = get3DPoint(nearest);
      _curr_plane.s_g = r_1 + r_2;
      _curr_plane.m_k = (r_1 - _curr_plane.s_g * 0.5)*(r_1 - _curr_plane.s_g * 0.5).transpose();
      _curr_plane.m_k += (r_2 - _curr_plane.s_g * 0.5)*(r_2 - _curr_plane.s_g * 0.5).transpose();
      _curr_plane.p_k = r_1 * r_1.transpose() + r_2*r_2.transpose();
      _curr_plane.n_points = 2;

      while (!_q.empty())
      {
        int new_point = _q.front();
        _q.pop();
        Eigen::Vector3d v = get3DPoint(new_point);
        if (updateMatrices(v)) {
          addPixelToRegion(new_point, _curr_region_id);
        } else {
          _available_pixels--;
        }
      }

      // The queue has been emptied --> clear possible QUEUE status and add the region to the detected planes if condition of step 12 (Algorithm 1)
      if (_curr_region.size() > _theta) {
        _curr_plane.makeDPositive();
        _curr_plane.calculateCovariance(_std_dev);
//         std::cout << "Detected plane: " << _curr_plane.toString() << std::endl;
        _detected_planes.push_back(_curr_plane);
        _detected_ids.push_back(_curr_region_id);
      }
      _curr_region_id++; // The next plane will have a new identificator
    } else {
      // No nearest neighbor available --> discard (to R_PRIMA)
      _status_vec[candidate] = (int)IN_R_PRIMA;
      _available_pixels--;
    }
  }

  return _detected_planes.size();
}

void PlaneDetector::resetStatus()
{
  if (_status_vec.size() != _width*_height) {
    _status_vec.resize(_width*_height);
  }
  for (size_t i = 0; i < _width; i++) {
    for (size_t j = 0; j < _height; j++) {
      if (getDepth(i, j) > 0.0) {
        _status_vec[getPos(i,j)] = (int)UNPROCESSED;
      } else {
        _status_vec[getPos(i,j)] = (int)IN_R_PRIMA;
      }
    }
  }

  _p.init();
  _curr_plane.init();

  _detected_planes.clear();
  _detected_ids.clear();
}

int PlaneDetector::getRandomPixel(int region) const
{
  std::uniform_int_distribution<int> distribution(0, _height * _width - 1);
  int ret_val;

  do {
    ret_val = distribution(generator);
  } while (_status_vec[ret_val] != region);

  return ret_val;
}

//! Gets the unprocessed nearest neighbor of a pixel in a window of size 9
int PlaneDetector::getNearestNeighbor(int index) const
{
  int near = -1;
  int aux;
  float min_dist = 1e10;

  Eigen::Vector3d v = get3DPoint(index);
  Eigen::Vector3d v_;
  for (int i = -1; i < 2; i++) {
    for (int j = -1; j < 2; j++) {
      if (i != 0 || j != 0) {
        aux = index + i + j * _width;

        if (aux > 0 && aux < _height * _width) { // Check bounds
          if (_status_vec[aux] == (int)UNPROCESSED) {
            v_ = get3DPoint(aux);
            double dist = (v - v_).norm();
            if (dist < min_dist) {
              near = aux;
              min_dist = dist;
            }
          }
        }
      }
    }
  }
  return near;
}

int PlaneDetector::availablePixels() const
{
  bool ret_val = false;
  int cont = 0;

  //TODO: Uncomment
  for (int i = 0; i < _width * _height /*&& cont < _theta*/; i++) {
    if (_status_vec[i] == (int)UNPROCESSED)
      cont++;
  }

//   std::cout << "availablePixels() --> cont = " << cont << std::endl;

  return cont;
}

void PlaneDetector::setCameraParameters(const boost::array< double, 9 >& k_)
{
  for (int i = 0; i < 3;i++)
  {
    for (int j = 0; j < 3; j++)
    {
      _K(i,j) = k_[i*3 + j];
    }
  }

  _kx = 1.0/_K(0,0);
  _cx = _K(0,2);
  _ky = 1.0/_K(1,1);
  _cy = _K(1,2);
  _initialized = true;
}

double PlaneDetector::getDepth(int i, int j) const
{
  double ret_val = -1.0;
  if (_downsample) {
    i *= 2;
    j *= 2;
  }

  if (i >= _image.cols || j >= _image.rows)
    return -1.0;
//   std::cout << "Data: " << cvbDepth->image.at<float>(i,j) << " ";
  if(_float_image)
    ret_val = _image.at<float>(j, i); /// CAUTION!!!! the indices in cv::Mat are row, cols (elsewhere cols, rows)
  else
    ret_val = _image.at<u_int16_t>(j, i)*0.001;
  if(ret_val < __MIN_RANGE__ || ret_val > __MAX_RANGE__)
  {
    ret_val = -1.0;
  }
  return ret_val;
}

Eigen::Vector3d PlaneDetector::get3DPoint(int i, int j) const
{
  Eigen::Vector3d pt;

  pt[2] = getDepth(i, j);
  if (_downsample) {
    i *= 2;
    j *= 2;
  }
  if(pt[2] > __MIN_RANGE__ && pt[2] < __MAX_RANGE__)
  {
    pt[0] = (float)(i - _cx) * pt[2] * _kx;
    pt[1] = (float)(j - _cy) * pt[2] * _ky;
  } else {
    throw UnknownDepthException();
  }

  return pt;
}

Eigen::Vector3d PlaneDetector::get3DPoint(int index) const
{
  return get3DPoint(index%_width, index/_width);
}

void PlaneDetector::addPixelToRegion(int index, int _curr_region_id)
{
  _status_vec[index] = _curr_region_id;
  _curr_region.push_back(index);
  Eigen::Vector3d v = get3DPoint(index);
  _available_pixels--;

  int neighbor = getNearestNeighbor(index);
  while (neighbor >= 0) {
    Eigen::Vector3d v_2 = get3DPoint(neighbor);
    if ( (v-v_2).norm() < _delta) { // First check --> the neighbor is sufficiently near to the point
      _status_vec[neighbor] = (int)IN_QUEUE;
      _q.push(neighbor);
      neighbor = getNearestNeighbor(index);
    } else {
      neighbor = -1;
    }
  }
}

bool PlaneDetector::updateMatrices(const Eigen::Vector3d& v)
{
  size_t k = _curr_region.size();
  double div_1 = 1.0 / (double)(k + 1);
  double div = 1.0 / (double)(k);

  _p.s_g = _curr_plane.s_g + v;
  _p.m_k = _curr_plane.m_k + v*v.transpose() - (_p.s_g * div_1)*_p.s_g.transpose() + (_curr_plane.s_g * div) * _curr_plane.s_g.transpose();
  _p.p_k = _curr_plane.p_k + v*v.transpose();

  // Calculate d and n (n is the eigenvector related to the lowest eigenvalue)

  _es.compute(_p.m_k);
  double min_eigenval = 1e10;
  int min_i;
  for (int i = 0; i < 3; i++) {
    double curr_eigen = fabs(_es.eigenvalues()(i));
    if (curr_eigen < min_eigenval) {
      min_eigenval = curr_eigen;
      min_i = i;
    }

  }
  _p.v = _es.eigenvectors().col(min_i);
  _p.d = div_1 * _p.v.dot(_p.s_g);

  // Update the MSE (Eq 3)
  _p.mse = div_1 * _p.v.transpose() * _p.p_k * _p.v - 2 * _p.v.dot(_p.s_g) * _p.d * div_1 + _p.d * _p.d;
  _p.n_points = k + 1;

  // Check if the new plane meets the constraint of algorithm 1 step 8. If so, update the values of the matrices of the class
  if (_p.mse < _epsilon && _p.distance(v) < _gamma)
  {
    // Meets the constraints --> Actualize the plane
    _p.r_g = _p.s_g * div_1;
    _curr_plane = _p;

    return true;
  }
  return false;
}

#endif