#include <iostream>
#include <ros/ros.h>
#include <vector>
#include <math.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h> //for sensor_msgs::PointCloud2
#include <geometry_msgs/TransformStamped.h>

#include <pcl/point_types.h> // for pcl::PointXYZ
#include <pcl_conversions/pcl_conversions.h> // for pcl::fromROSMsg()

#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
// #include <pcl_ros/transforms.hpp>

#include <pcl/segmentation/sac_segmentation.h> //for SACSegmentation
#include <pcl/filters/extract_indices.h> //for ExtractIndices
#include <pcl/filters/statistical_outlier_removal.h>

#include <tf2_ros/transform_listener.h>
#include <tf/transform_listener.h>

#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

#include <Eigen/Dense>

#include "complete_pcl_set.h"

ros::Publisher lfrq_oc_pub;
ros::Publisher lfrq_fr_pub;

// with PointCloud2Ptr not working, but with PointCloud2::ConstPtr it works
// void sync_callback(const boost::shared_ptr<const sensor_msgs::PointCloud2>& pcl_in, const nav_msgs::Odometry::ConstPtr &pose_in)

void sync_callback(const  sensor_msgs::PointCloud2::ConstPtr &pcl_in, const nav_msgs::Odometry::ConstPtr &pose_in)
{
  // Solve all of perception here...
  ROS_INFO("\n\nrcvd msg ... ");

  ROS_INFO_STREAM("pcl  time stamp: "<< pcl_in->header.stamp);
  ROS_INFO_STREAM("pose time stamp: "<< pose_in->header.stamp);

  //convert the input into a pcl::PointCloud< pcl::PointXYZ> object
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloudPtr(new pcl::PointCloud<pcl::PointXYZI>);

  // variables  for filtered/transformed/low_frq  pcl
  pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloudPtr(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::fromROSMsg(*pcl_in, *cloudPtr);

  //*********** pcl ***********
  pcl::PointCloud<pcl::PointXYZI>  org_pcl, sph_pcl, occ_srfc_pcl;
  std::copy(cloudPtr->points.begin(), cloudPtr->points.end(),std::back_inserter(org_pcl));

  //*********** clustering ***********
  // next step

  int cnt = 0 ;
  std::vector<pcl::PointXYZI>  pcl_complete_set = complete_pcl_set();
  std::cout<< pcl_complete_set[0].x << ", " << pcl_complete_set[0].y << std::endl;

  //*********** project to different form ***********
  std::vector<pcl::PointXYZI> sph_pcl_vec, occ_srfc_pcl_vec;

  for(auto pt: org_pcl){
    // ROS_INFO_STREAM("pt: " << pt.x);
    float dst = sqrt(pt.x*pt.x + pt.y*pt.y + pt.z*pt.z );
    float th = atan2(pt.y , pt.x );
    float al = acos(pt.z / dst );

    // spherical form
    float oc_srfc_rds = 5.5; // occupancy surface radius
    pcl::PointXYZI shp_pt;
    shp_pt.x = th;
    shp_pt.y = al;
    shp_pt.z = dst;
    shp_pt.intensity = dst;
    sph_pcl_vec.push_back(shp_pt);

    cnt ++;
    if( cnt < 1 )
      std::cout<< th << ", " << al << std::endl;

    // occupancy surface
    pcl::PointXYZI oc_srfc_pt;
    oc_srfc_pt.x = oc_srfc_rds * sin(al) * cos(th);
    oc_srfc_pt.y = oc_srfc_rds * sin(al) * sin(th);
    oc_srfc_pt.z = oc_srfc_rds * cos(al);
    oc_srfc_pt.intensity = dst;
    occ_srfc_pcl_vec.push_back(oc_srfc_pt);
  }
  ROS_INFO_STREAM("sph_pcl_vec: "<< sph_pcl_vec.size() );
  ROS_INFO_STREAM("occ_srfc_pcl_vec: "<< occ_srfc_pcl_vec.size() );

  //*********** std vector to pcl ***********
  std::copy(sph_pcl_vec.begin(), sph_pcl_vec.end(),std::back_inserter(sph_pcl));
  sph_pcl.header.frame_id = "velodyne";

  std::copy(occ_srfc_pcl_vec.begin(), occ_srfc_pcl_vec.end(),std::back_inserter(occ_srfc_pcl));
  occ_srfc_pcl.header.frame_id = "velodyne";

  ROS_INFO_STREAM("sph_pcl: "<< sph_pcl.size());
  ROS_INFO_STREAM("occ_srfc_pcl: "<< occ_srfc_pcl.size());

  //*********** from velodyne to world frame ***********
  // Eigen::Quaternionf rotation(pose_in->pose.pose.orientation.w, pose_in->pose.pose.orientation.x, pose_in->pose.pose.orientation.y,
                              // pose_in->pose.pose.orientation.z);
  // Eigen::Vector3f origin(pose_in->pose.pose.position.x, pose_in->pose.pose.position.y, pose_in->pose.pose.position.z+0.322);

  // transformPointCloud(org_pcl, *transformed_cloudPtr, origin, rotation);
  // transformed_cloudPtr->header.frame_id = "world"; //should be after transform otherwise aha
  // ROS_INFO_STREAM("transformed_cloudPtr: "<< transformed_cloudPtr->points.size());

//*********** Publish msgs ***********
  sensor_msgs::PointCloud2 oc_srfc_msg;
  pcl::toROSMsg(sph_pcl, oc_srfc_msg);
  oc_srfc_msg.header.stamp = pcl_in->header.stamp; //.toSec();
  lfrq_oc_pub.publish(oc_srfc_msg);

  sensor_msgs::PointCloud2 fr_srfc_msg;
  pcl::toROSMsg(occ_srfc_pcl, fr_srfc_msg);
  fr_srfc_msg.header.stamp = pcl_in->header.stamp;//.toSec();
  lfrq_fr_pub.publish(fr_srfc_msg);

  ros::Duration(0.3).sleep();

}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "pcl_surface_projection");
  ROS_INFO("pcl_surface_projection node ... ");

  ros::NodeHandle nh;

  //create publisher object
  lfrq_oc_pub = nh.advertise<sensor_msgs::PointCloud2>("/lfrq_sph_oc_srfc", 1);
  lfrq_fr_pub = nh.advertise<sensor_msgs::PointCloud2>("/lfrq_sph_fr_srfc", 1);

  // message filter time synchronization
  message_filters::Subscriber<nav_msgs::Odometry> pose_sub(nh, "/ground_truth/state", 1);
  message_filters::Subscriber<sensor_msgs::PointCloud2> pcl_sub(nh, "/mid/points", 1);
  message_filters::TimeSynchronizer<sensor_msgs::PointCloud2, nav_msgs::Odometry> sync(pcl_sub, pose_sub, 10);

  sync.registerCallback(boost::bind(&sync_callback, _1, _2)); // try std instead boost not working as well
  ros::spin();

  return 0;
}


//  Copyright 2013 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_

#ifndef BOOST_MP_MATH_SETUP_HPP
#define BOOST_MP_MATH_SETUP_HPP

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error
#undef BOOST_MATH_SMALL_CONSTANT
#define BOOST_MATH_SMALL_CONSTANT(x) x

#if !defined(TEST_MPF_50) && !defined(TEST_BACKEND) && !defined(TEST_CPP_DEC_FLOAT) \
      && !defined(TEST_MPFR_50) && !defined(TEST_FLOAT128) && !defined(TEST_CPP_BIN_FLOAT)
#  define TEST_MPF_50
#  define TEST_MPFR_50
#  define TEST_CPP_DEC_FLOAT
#  define TEST_FLOAT128
#  define TEST_CPP_BIN_FLOAT

#ifdef _MSC_VER
#pragma message("CAUTION!!: No backend type specified so testing everything.... this will take some time!!")
#endif
#ifdef __GNUC__
#pragma warning "CAUTION!!: No backend type specified so testing everything.... this will take some time!!"
#endif

#endif

#if defined(TEST_MPF_50)
#include <boost/multiprecision/gmp.hpp>
#include <boost/multiprecision/debug_adaptor.hpp>

#define MPF_TESTS    /*test(number<gmp_float<18> >(), "number<gmp_float<18> >");*/\
   mpf_float::default_precision(20);\
   test(mpf_float(), "number<gmp_float<0> > (20 digit precision)");\
   mpf_float::default_precision(35);\
   test(mpf_float(), "number<gmp_float<0> > (35 digit precision)");\
   test(number<gmp_float<30> >(), "number<gmp_float<30> >");\
   test(number<gmp_float<35> >(), "number<gmp_float<35> >");\
   /* there should be at least one test with expression templates off: */ \
   test(number<gmp_float<35>, et_off>(), "number<gmp_float<35>, et_off>");
#define MPF_SMALL_TESTS    /*test(number<gmp_float<18> >(), "number<gmp_float<18> >");*/\
   test(number<gmp_float<30> >(), "number<gmp_float<30> >");\
   test(number<gmp_float<35> >(), "number<gmp_float<35> >");\
   /* there should be at least one test with expression templates off: */ \
   test(number<gmp_float<35>, et_off>(), "number<gmp_float<35>, et_off>");\
   mpf_float::default_precision(20); \
   test(mpf_float(), "number<gmp_float<0> > (20 digit precision)"); \
   mpf_float::default_precision(35); \
   test(mpf_float(), "number<gmp_float<0> > (35 digit precision)"); \

typedef boost::multiprecision::number<boost::multiprecision::gmp_float<18> > test_type_1;
typedef boost::multiprecision::number<boost::multiprecision::gmp_float<30> > test_type_2;
typedef boost::multiprecision::number<boost::multiprecision::gmp_float<35> > test_type_3;
typedef boost::multiprecision::number<boost::multiprecision::gmp_float<35>, boost::multiprecision::et_off> test_type_4;
typedef boost::multiprecision::mpf_float test_type_5;

#else

#define MPF_TESTS
#define MPF_SMALL_TESTS

#endif

#if defined(TEST_MPFR_50)
#include <boost/multiprecision/mpfr.hpp>

#define MPFR_TESTS    test(number<mpfr_float_backend<18> >(), "number<mpfr_float_backend<18> >");\
   test(number<mpfr_float_backend<30> >(), "number<mpfr_float_backend<30> >");\
   test(number<mpfr_float_backend<35> >(), "number<mpfr_float_backend<35> >");\
  /* Test variable precision at 2 different precisions - checks our ability to handle dynamic changes in precision */\
  mpfr_float::default_precision(20);\
  test(mpfr_float(), "number<mpfr_float_backend<0> > (20-digit precision)");\
  mpfr_float::default_precision(35);\
  test(mpfr_float(), "number<mpfr_float_backend<0> > (35-digit precision)");

typedef boost::multiprecision::number<boost::multiprecision::mpfr_float_backend<18> > test_type_1;
typedef boost::multiprecision::number<boost::multiprecision::mpfr_float_backend<30> > test_type_2;
typedef boost::multiprecision::number<boost::multiprecision::mpfr_float_backend<35> > test_type_3;
typedef boost::multiprecision::mpfr_float test_type_4;

#else

#define MPFR_TESTS

#endif

#ifdef TEST_BACKEND
#include <boost/multiprecision/concepts/mp_number_archetypes.hpp>
#endif
#ifdef TEST_CPP_DEC_FLOAT
#include <boost/multiprecision/cpp_dec_float.hpp>

#define CPP_DEC_FLOAT_TESTS    test(number<cpp_dec_float<18> >(), "number<cpp_dec_float<18> >");\
   test(number<cpp_dec_float<30> >(), "number<cpp_dec_float<30> >");\
   test(number<cpp_dec_float<35, long long, std::allocator<char> > >(), "number<cpp_dec_float<35, long long, std::allocator<char> > >");

typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<18> > test_type_1;
typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<30> > test_type_2;
typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<35, long long, std::allocator<char> > > test_type_3;

#else

#define CPP_DEC_FLOAT_TESTS

#endif

#ifdef TEST_CPP_BIN_FLOAT
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/debug_adaptor.hpp>

//#define CPP_BIN_FLOAT_TESTS test(number<debug_adaptor<cpp_bin_float_quad::backend_type>, et_off>(), "cpp_bin_float_quad");
#define CPP_BIN_FLOAT_TESTS test(cpp_bin_float_quad(), "cpp_bin_float_quad");

//typedef boost::multiprecision::number<boost::multiprecision::debug_adaptor<boost::multiprecision::cpp_bin_float_quad::backend_type>, boost::multiprecision::et_off> test_type_1;
typedef boost::multiprecision::cpp_bin_float_quad test_type_1;

#else

#define CPP_BIN_FLOAT_TESTS

#endif

#ifdef TEST_FLOAT128
#include <boost/multiprecision/float128.hpp>

#define FLOAT128_TESTS test(float128(), "float128");

typedef boost::multiprecision::float128 test_type_1;

#else

#define FLOAT128_TESTS

#endif

#ifndef BOOST_MATH_TEST_TYPE
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#endif

#define ALL_TESTS \
 MPF_TESTS\
 MPFR_TESTS\
 CPP_DEC_FLOAT_TESTS\
 FLOAT128_TESTS\
 CPP_BIN_FLOAT_TESTS

#define ALL_SMALL_TESTS\
 MPF_SMALL_TESTS\
 MPFR_TESTS\
 CPP_DEC_FLOAT_TESTS\
 FLOAT128_TESTS\
 CPP_BIN_FLOAT_TESTS

#endif


//=======================================================================
// Copyright 2000 University of Notre Dame.
// Authors: Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee
//
// 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)
//=======================================================================

#ifndef BOOST_GRAPH_EDMONDS_KARP_MAX_FLOW_HPP
#define BOOST_GRAPH_EDMONDS_KARP_MAX_FLOW_HPP

#include <boost/config.hpp>
#include <vector>
#include <algorithm> // for std::min and std::max
#include <boost/config.hpp>
#include <boost/pending/queue.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/breadth_first_search.hpp>

namespace boost
{

// The "labeling" algorithm from "Network Flows" by Ahuja, Magnanti,
// Orlin.  I think this is the same as or very similar to the original
// Edmonds-Karp algorithm.  This solves the maximum flow problem.

namespace detail
{

    template < class Graph, class ResCapMap >
    filtered_graph< Graph, is_residual_edge< ResCapMap > > residual_graph(
        Graph& g, ResCapMap residual_capacity)
    {
        return filtered_graph< Graph, is_residual_edge< ResCapMap > >(
            g, is_residual_edge< ResCapMap >(residual_capacity));
    }

    template < class Graph, class PredEdgeMap, class ResCapMap,
        class RevEdgeMap >
    inline void augment(Graph& g,
        typename graph_traits< Graph >::vertex_descriptor src,
        typename graph_traits< Graph >::vertex_descriptor sink, PredEdgeMap p,
        ResCapMap residual_capacity, RevEdgeMap reverse_edge)
    {
        typename graph_traits< Graph >::edge_descriptor e;
        typename graph_traits< Graph >::vertex_descriptor u;
        typedef typename property_traits< ResCapMap >::value_type FlowValue;

        // find minimum residual capacity along the augmenting path
        FlowValue delta = (std::numeric_limits< FlowValue >::max)();
        e = get(p, sink);
        do
        {
            BOOST_USING_STD_MIN();
            delta = min BOOST_PREVENT_MACRO_SUBSTITUTION(
                delta, get(residual_capacity, e));
            u = source(e, g);
            e = get(p, u);
        } while (u != src);

        // push delta units of flow along the augmenting path
        e = get(p, sink);
        do
        {
            put(residual_capacity, e, get(residual_capacity, e) - delta);
            put(residual_capacity, get(reverse_edge, e),
                get(residual_capacity, get(reverse_edge, e)) + delta);
            u = source(e, g);
            e = get(p, u);
        } while (u != src);
    }

} // namespace detail

template < class Graph, class CapacityEdgeMap, class ResidualCapacityEdgeMap,
    class ReverseEdgeMap, class ColorMap, class PredEdgeMap >
typename property_traits< CapacityEdgeMap >::value_type edmonds_karp_max_flow(
    Graph& g, typename graph_traits< Graph >::vertex_descriptor src,
    typename graph_traits< Graph >::vertex_descriptor sink, CapacityEdgeMap cap,
    ResidualCapacityEdgeMap res, ReverseEdgeMap rev, ColorMap color,
    PredEdgeMap pred)
{
    typedef typename graph_traits< Graph >::vertex_descriptor vertex_t;
    typedef typename property_traits< ColorMap >::value_type ColorValue;
    typedef color_traits< ColorValue > Color;

    typename graph_traits< Graph >::vertex_iterator u_iter, u_end;
    typename graph_traits< Graph >::out_edge_iterator ei, e_end;
    for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter)
        for (boost::tie(ei, e_end) = out_edges(*u_iter, g); ei != e_end; ++ei)
            put(res, *ei, get(cap, *ei));

    put(color, sink, Color::gray());
    while (get(color, sink) != Color::white())
    {
        boost::queue< vertex_t > Q;
        breadth_first_search(detail::residual_graph(g, res), src, Q,
            make_bfs_visitor(record_edge_predecessors(pred, on_tree_edge())),
            color);
        if (get(color, sink) != Color::white())
            detail::augment(g, src, sink, pred, res, rev);
    } // while

    typename property_traits< CapacityEdgeMap >::value_type flow = 0;
    for (boost::tie(ei, e_end) = out_edges(src, g); ei != e_end; ++ei)
        flow += (get(cap, *ei) - get(res, *ei));
    return flow;
} // edmonds_karp_max_flow()

namespace detail
{
    //-------------------------------------------------------------------------
    // Handle default for color property map

    // use of class here is a VC++ workaround
    template < class ColorMap > struct edmonds_karp_dispatch2
    {
        template < class Graph, class PredMap, class P, class T, class R >
        static typename edge_capacity_value< Graph, P, T, R >::type apply(
            Graph& g, typename graph_traits< Graph >::vertex_descriptor src,
            typename graph_traits< Graph >::vertex_descriptor sink,
            PredMap pred, const bgl_named_params< P, T, R >& params,
            ColorMap color)
        {
            return edmonds_karp_max_flow(g, src, sink,
                choose_const_pmap(
                    get_param(params, edge_capacity), g, edge_capacity),
                choose_pmap(get_param(params, edge_residual_capacity), g,
                    edge_residual_capacity),
                choose_const_pmap(
                    get_param(params, edge_reverse), g, edge_reverse),
                color, pred);
        }
    };
    template <> struct edmonds_karp_dispatch2< param_not_found >
    {
        template < class Graph, class PredMap, class P, class T, class R >
        static typename edge_capacity_value< Graph, P, T, R >::type apply(
            Graph& g, typename graph_traits< Graph >::vertex_descriptor src,
            typename graph_traits< Graph >::vertex_descriptor sink,
            PredMap pred, const bgl_named_params< P, T, R >& params,
            param_not_found)
        {
            typedef
                typename graph_traits< Graph >::vertices_size_type size_type;
            size_type n = is_default_param(get_param(params, vertex_color))
                ? num_vertices(g)
                : 1;
            std::vector< default_color_type > color_vec(n);
            return edmonds_karp_max_flow(g, src, sink,
                choose_const_pmap(
                    get_param(params, edge_capacity), g, edge_capacity),
                choose_pmap(get_param(params, edge_residual_capacity), g,
                    edge_residual_capacity),
                choose_const_pmap(
                    get_param(params, edge_reverse), g, edge_reverse),
                make_iterator_property_map(color_vec.begin(),
                    choose_const_pmap(
                        get_param(params, vertex_index), g, vertex_index),
                    color_vec[0]),
                pred);
        }
    };

    //-------------------------------------------------------------------------
    // Handle default for predecessor property map

    // use of class here is a VC++ workaround
    template < class PredMap > struct edmonds_karp_dispatch1
    {
        template < class Graph, class P, class T, class R >
        static typename edge_capacity_value< Graph, P, T, R >::type apply(
            Graph& g, typename graph_traits< Graph >::vertex_descriptor src,
            typename graph_traits< Graph >::vertex_descriptor sink,
            const bgl_named_params< P, T, R >& params, PredMap pred)
        {
            typedef typename get_param_type< vertex_color_t,
                bgl_named_params< P, T, R > >::type C;
            return edmonds_karp_dispatch2< C >::apply(
                g, src, sink, pred, params, get_param(params, vertex_color));
        }
    };
    template <> struct edmonds_karp_dispatch1< param_not_found >
    {

        template < class Graph, class P, class T, class R >
        static typename edge_capacity_value< Graph, P, T, R >::type apply(
            Graph& g, typename graph_traits< Graph >::vertex_descriptor src,
            typename graph_traits< Graph >::vertex_descriptor sink,
            const bgl_named_params< P, T, R >& params, param_not_found)
        {
            typedef
                typename graph_traits< Graph >::edge_descriptor edge_descriptor;
            typedef
                typename graph_traits< Graph >::vertices_size_type size_type;
            size_type n
                = is_default_param(get_param(params, vertex_predecessor))
                ? num_vertices(g)
                : 1;
            std::vector< edge_descriptor > pred_vec(n);

            typedef typename get_param_type< vertex_color_t,
                bgl_named_params< P, T, R > >::type C;
            return edmonds_karp_dispatch2< C >::apply(g, src, sink,
                make_iterator_property_map(pred_vec.begin(),
                    choose_const_pmap(
                        get_param(params, vertex_index), g, vertex_index),
                    pred_vec[0]),
                params, get_param(params, vertex_color));
        }
    };

} // namespace detail

template < class Graph, class P, class T, class R >
typename detail::edge_capacity_value< Graph, P, T, R >::type
edmonds_karp_max_flow(Graph& g,
    typename graph_traits< Graph >::vertex_descriptor src,
    typename graph_traits< Graph >::vertex_descriptor sink,
    const bgl_named_params< P, T, R >& params)
{
    typedef typename get_param_type< vertex_predecessor_t,
        bgl_named_params< P, T, R > >::type Pred;
    return detail::edmonds_karp_dispatch1< Pred >::apply(
        g, src, sink, params, get_param(params, vertex_predecessor));
}

template < class Graph >
typename property_traits<
    typename property_map< Graph, edge_capacity_t >::const_type >::value_type
edmonds_karp_max_flow(Graph& g,
    typename graph_traits< Graph >::vertex_descriptor src,
    typename graph_traits< Graph >::vertex_descriptor sink)
{
    bgl_named_params< int, buffer_param_t > params(0);
    return edmonds_karp_max_flow(g, src, sink, params);
}

} // namespace boost

#endif // BOOST_GRAPH_EDMONDS_KARP_MAX_FLOW_HPP


#ifndef MESH_2D_HPP
#define MESH_2D_HPP

#include <polyfem/Common.hpp>
#include <polyfem/Mesh.hpp>
#include <polyfem/Navigation.hpp>
#include <polyfem/Types.hpp>

#include <Eigen/Dense>
#include <geogram/mesh/mesh.h>

namespace polyfem
{
	class Mesh2D : public Mesh
	{
	public:
		Mesh2D() = default;
		virtual ~Mesh2D() = default;
		POLYFEM_DEFAULT_MOVE_COPY(Mesh2D)

		void refine(const int n_refiniment, const double t, std::vector<int> &parent_nodes) override;

		bool is_volume() const override { return false; }

		int n_cells() const override { return 0; }
		int n_faces() const override { return mesh_.facets.nb(); }
		int n_edges() const override { return mesh_.edges.nb(); }
		int n_vertices() const override { return mesh_.vertices.nb(); }

		inline int n_face_vertices(const int f_id) const {return mesh_.facets.nb_vertices(f_id); }

		inline int face_vertex(const int f_id, const int lv_id) const { return mesh_.facets.vertex(f_id, lv_id); }
		inline int edge_vertex(const int e_id, const int lv_id) const { return mesh_.edges.vertex(e_id, lv_id); }

		bool is_boundary_vertex(const int vertex_global_id) const override {
			// GEO::Attribute<bool> boundary_vertices(mesh_.vertices.attributes(), "boundary_vertex");
			return (*boundary_vertices_)[vertex_global_id];
		}
		bool is_boundary_edge(const int edge_global_id) const override {
			// GEO::Attribute<bool> boundary_edges(mesh_.edges.attributes(), "boundary_edge");
			return (*boundary_edges_)[edge_global_id];
		}
		bool is_boundary_face(const int face_global_id) const override {
			assert(false);
			return false;
		}

		bool is_boundary_element(const int element_global_id) const override;

		bool build_from_matrices(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F) override;
		bool save(const std::string &path) const override;

		void attach_higher_order_nodes(const Eigen::MatrixXd &V, const std::vector<std::vector<int>> &nodes) override;
		RowVectorNd edge_node(const Navigation::Index &index, const int n_new_nodes, const int i) const;
		RowVectorNd face_node(const Navigation::Index &index, const int n_new_nodes, const int i, const int j) const;

		void normalize() override;

		double edge_length(const int gid) const override;

		void compute_elements_tag() override;
		virtual void update_elements_tag() override;

		void set_point(const int global_index, const RowVectorNd &p);

		virtual RowVectorNd point(const int global_index) const override;
		virtual RowVectorNd edge_barycenter(const int index) const override;
		virtual RowVectorNd face_barycenter(const int index) const override;
		virtual RowVectorNd cell_barycenter(const int index) const override { assert(false); return RowVectorNd(2); }

		virtual void bounding_box(RowVectorNd &min, RowVectorNd &max) const override;

		// Navigation wrapper
		inline Navigation::Index get_index_from_face(int f, int lv = 0) const { return Navigation::get_index_from_face(mesh_, *c2e_, f, lv); }

		// Navigation in a surface mesh
		inline Navigation::Index switch_vertex(Navigation::Index idx) const { return Navigation::switch_vertex(mesh_, idx); }
		inline Navigation::Index switch_edge(Navigation::Index idx) const { return Navigation::switch_edge(mesh_, *c2e_, idx); }
		inline Navigation::Index switch_face(Navigation::Index idx) const { return Navigation::switch_face(mesh_, *c2e_, idx); }

		// Iterate in a mesh
		inline Navigation::Index next_around_face(Navigation::Index idx) const { return switch_edge(switch_vertex(idx)); }
		inline Navigation::Index next_around_vertex(Navigation::Index idx) const { return switch_face(switch_edge(idx)); }

		void compute_boundary_ids(const double eps) override;
		void compute_boundary_ids(const std::function<int(const RowVectorNd&)> &marker) override;
		void compute_boundary_ids(const std::function<int(const RowVectorNd&, bool)> &marker) override;
		void compute_boundary_ids(const std::function<int(const std::vector<int>&, bool)> &marker) override;

		void compute_element_barycenters(Eigen::MatrixXd &barycenters) const override { face_barycenters(barycenters); }
		void triangulate_faces(Eigen::MatrixXi &tris, Eigen::MatrixXd &pts, std::vector<int> &ranges) const override;
		void get_edges(Eigen::MatrixXd &p0, Eigen::MatrixXd &p1) const override;
		void get_edges(Eigen::MatrixXd &p0, Eigen::MatrixXd &p1, const std::vector<bool> &valid_elements) const override;

	protected:
		bool load(const std::string &path) override;
		bool load(const GEO::Mesh &mesh) override;

	private:
		GEO::Mesh mesh_;
		std::unique_ptr<GEO::Attribute<GEO::index_t>> c2e_;
		std::unique_ptr<GEO::Attribute<bool>> boundary_vertices_;
		std::unique_ptr<GEO::Attribute<bool>> boundary_edges_;
	};
}

#endif //MESH_2D_HPP


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

#include <gtest/gtest.h>

#include <boost/numeric/odeint.hpp>

#include "smooth/compat/odeint.hpp"
#include "smooth/so3.hpp"

using namespace smooth;

TEST(OdeInt, Construct)
{
  ASSERT_NO_THROW(smooth::BoostOdeintOps::scale_sum2<double>(1., 2.));
  ASSERT_THROW(smooth::BoostOdeintOps::scale_sum2<double>(2., 2.), std::runtime_error);
}

TEST(OdeInt, Euler)
{
  using state_t = smooth::SO3d;
  using deriv_t = typename state_t::Tangent;

  auto system = [] (const state_t &, deriv_t & d, double) {
    d.setConstant(1);
  };

 boost::numeric::odeint::euler<state_t, double, deriv_t, double, boost::numeric::odeint::vector_space_algebra> stepper;

 state_t s;
 s.setIdentity();

 stepper.do_step(system, s, 0, 1);

 ASSERT_TRUE(s.isApprox(smooth::SO3d::exp(Eigen::Vector3d::Ones())));
}

TEST(OdeInt, RunkeKutta4)
{
  using state_t = smooth::SO3d;
  using deriv_t = typename state_t::Tangent;

  auto system = [] (const state_t &, deriv_t & d, double) {
    d.setConstant(1);
  };

 boost::numeric::odeint::runge_kutta4<state_t, double, deriv_t, double, boost::numeric::odeint::vector_space_algebra> stepper;

 state_t s;
 s.setIdentity();

 stepper.do_step(system, s, 0, 1);

 ASSERT_TRUE(s.isApprox(smooth::SO3d::exp(Eigen::Vector3d::Ones())));
}

TEST(OdeInt, CashKarp54)
{
  using state_t = smooth::SO3d;
  using deriv_t = typename state_t::Tangent;

  auto system = [] (const state_t &, deriv_t & d, double) {
    d.setConstant(1);
  };

 boost::numeric::odeint::runge_kutta_cash_karp54<state_t, double, deriv_t, double, boost::numeric::odeint::vector_space_algebra> stepper;

 state_t s;
 s.setIdentity();

 stepper.do_step(system, s, 0, 1);

 ASSERT_TRUE(s.isApprox(smooth::SO3d::exp(Eigen::Vector3d::Ones())));
}

TEST(OdeInt, Fehlberg78)
{
  using state_t = smooth::SO3d;
  using deriv_t = typename state_t::Tangent;

  auto system = [] (const state_t &, deriv_t & d, double) {
    d.setConstant(1);
  };

 boost::numeric::odeint::runge_kutta_fehlberg78<state_t, double, deriv_t, double, boost::numeric::odeint::vector_space_algebra> stepper;

 state_t s;
 s.setIdentity();

 stepper.do_step(system, s, 0, 1);

 ASSERT_TRUE(s.isApprox(smooth::SO3d::exp(Eigen::Vector3d::Ones())));
}


#pragma warning (disable : 4250)

#include "spectra_di_sg.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Spectra/SymGEigsSolver.h>
#include <Spectra/SymGEigsShiftSolver.h>
#include <Spectra/MatOp/SymShiftInvert.h>
#include <Spectra/MatOp/SparseSymMatProd.h>
#include <Spectra/MatOp/SparseCholesky.h>

using namespace Spectra;

using MapVec = Eigen::Map<Eigen::VectorXd>;
using MapMat = Eigen::Map<Eigen::MatrixXd>;
using SpMatrix = Eigen::SparseMatrix<double>;

template <typename TypeA, typename TypeB, typename Solver>
int solve_di_sg(const TypeA& A, const TypeB& B, Solver& eigs, int maxit, double tol, SortRule selection, spectra_result* result)
{
    eigs.init();

    int nconv = eigs.compute(selection, maxit, tol, SortRule::SmallestAlge);

    CompInfo info = eigs.info();

    if (info == CompInfo::Successful)
    {
        auto evals = static_cast<double*>(result->eigval);
        MapVec v(evals, nconv, 1);
        v.noalias() = eigs.eigenvalues();

        if (result->eigvec != NULL)
        {
            auto evecs = static_cast<double*>(result->eigvec);
            MapMat m(evecs, A.rows(), nconv);
            m.noalias() = eigs.eigenvectors();
        }
    }

    result->iterations = eigs.num_iterations();
    result->info = static_cast<int>(info);

    return nconv;
}

int spectra_di_sg(int which, int k, int ncv, int maxit, double tol,
    spectra_spmat* A, spectra_spmat* B, spectra_result* result)
{
    try
    {
        SortRule selection;

        SELECTION_RULE_FROM_INT(which)

        // We are going to calculate the eigenvalues of M
        Eigen::Map<const SpMatrix> M(A->n, A->n, A->nnz, A->p, A->i, (double*)A->x);
        Eigen::Map<const SpMatrix> N(A->n, A->n, B->nnz, B->p, B->i, (double*)B->x);

        SparseSymMatProd<double> op(M);
        SparseCholesky<double> Bop(N);

        SymGEigsSolver<double, SparseSymMatProd<double>, SparseCholesky<double>, GEigsMode::Cholesky> eigs(op, Bop, k, ncv);

        return solve_di_sg(M, N, eigs, maxit, tol, selection, result);
    }
    catch (std::exception& e)
    {
        result->info = -1001;
    }
    catch (...)
    {
        result->info = -1000;
    }

    return 0;
}

int spectra_di_sg_shift(int which, char mode, int k, int ncv, int maxit, double tol, double sigma,
    spectra_spmat* A, spectra_spmat* B, spectra_result* result)
{
    try
    {
        SortRule selection;

        SELECTION_RULE_FROM_INT(which)

        // We are going to calculate the eigenvalues of M
        Eigen::Map<const SpMatrix> M(A->n, A->n, A->nnz, A->p, A->i, (double*)A->x);
        Eigen::Map<const SpMatrix> N(A->n, A->n, B->nnz, B->p, B->i, (double*)B->x);

        using OpType = SymShiftInvert<double, Eigen::Sparse, Eigen::Sparse>;
        using BOpType = SparseSymMatProd<double>;

        OpType op(M, N);

        if (mode == 'S')
        {
            BOpType Bop(N);
            SymGEigsShiftSolver<double, OpType, BOpType, GEigsMode::ShiftInvert> eigs(op, Bop, k, ncv, sigma);

            return solve_di_sg(M, N, eigs, maxit, tol, selection, result);
        }
        else if (mode == 'B')
        {
            BOpType Bop(M);
            SymGEigsShiftSolver<double, OpType, BOpType, GEigsMode::Buckling> eigs(op, Bop, k, ncv, sigma);

            return solve_di_sg(M, N, eigs, maxit, tol, selection, result);
        }
        else if (mode == 'C')
        {
            BOpType Bop(N);
            SymGEigsShiftSolver<double, OpType, BOpType, GEigsMode::Cayley> eigs(op, Bop, k, ncv, sigma);

            return solve_di_sg(M, N, eigs, maxit, tol, selection, result);
        }

    }
    catch (std::exception& e)
    {
        result->info = -1001;
    }
    catch (...)
    {
        result->info = -1000;
    }

    return 0;
}

#undef AR_TYPE


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

#include <iostream>

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

#include <boost/program_options.hpp>
#include <boost/thread.hpp>
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <BMS.h>
#include <BMS360.h>
#include "../libgbvs360/ShiftImage.hpp"

// ------------------------------------------------------------------------------------------------------------------------------------------------------
// equatorial prior

void applyEquatorialPrior(cv::Mat& image, bool adaptive = false) {
	// find the latitude where most of the saliency is located

	float equatorialLine = 0;
	if(adaptive) {
		float sum = 0;
		for(int i = 0 ; i < image.rows ; ++i) {
			float lat = (.5f - static_cast<float>(i)/image.rows) * 180.f;
			float weight = 0;
			for(int j = 0 ; j < image.cols ; ++j) {
				weight += image.at<float>(i,j);
				sum += image.at<float>(i,j);;
			}
			equatorialLine += lat * weight;
		}
		equatorialLine /= sum;

		equatorialLine = std::max(std::min(equatorialLine, 8.f), -8.f);
	}

	// apply the equatorial prior
	for(int i = 0 ; i < image.rows ; ++i) {
		float lat = (.5f - static_cast<float>(i)/image.rows) * 180.f - equatorialLine;
		float prior =  (.01 + .40 * exp(-(lat*lat)/(700)) + .75 * exp(-((lat-100)*(lat-100))/(200)) + .75 * exp(-((lat+90)*(lat+90))/(200))) / .41f;
		prior = std::min(1.f, prior);

		for(int j = 0 ; j < image.cols ; ++j) {
			image.at<float>(i,j) = prior * image.at<float>(i,j);
		}
	}
}

// ------------------------------------------------------------------------------------------------------------------------------------------------------
// apply BMS on one frame

void process(const cv::Mat &input, cv::Mat &output, int maxDim, int dilatationWidth1, int dilatationWidth2, int normalize, int handleBorder, int colorSpace, bool whitening, int sampleStep, float blurStd, bool equirectangular) {

	cv::Mat src_small;
	float w = (float)input.cols, h = (float)input.rows;
	float maxD = fmax(w, h);
	cv::resize(input, src_small, cv::Size((int)(maxDim*w / maxD), (int)(maxDim*h / maxD)), 0.0, 0.0, cv::INTER_AREA);

	boost::shared_ptr<BMS> bms;
	if(equirectangular)
		bms = boost::shared_ptr<BMS>(new BMS360(src_small, dilatationWidth1, normalize == 1, handleBorder == 1, colorSpace, whitening));
	else
		bms = boost::shared_ptr<BMS>(new BMS(src_small, dilatationWidth1, normalize == 1, handleBorder == 1, colorSpace, whitening));

	bms->computeSaliency((double)sampleStep);

	cv::Mat result = bms->getSaliencyMap(false);

	if (dilatationWidth2 > 0)
		dilate(result, result, cv::Mat(), cv::Point(-1, -1), dilatationWidth2);

	// if (blurStd > 0) {
	// 	int blur_width = (int)MIN(floor(blurStd) * 4 + 1, 51);
	// 	cv::GaussianBlur(result, result, cv::Size(blur_width, blur_width), blurStd, blurStd);
	// }

	output = result;
	// cv::resize(result, output, input.size());
}

// ------------------------------------------------------------------------------------------------------------------------------------------------------
// work on one projection

struct Configuration {
	int sampleStep;
	int dilatationWidth1;
	int dilatationWidth2;
	float blurStd;
	int normalize;
	int handleBorder;
	int colorSpace;
	bool whitening;
	int maxDim;
	bool equatorialPrior;
	bool equirectangular;
};

void processJob(int workerID, int nb_shift, cv::Mat &input, std::vector<cv::Mat> &outputs, Configuration &conf) {
	cv::Mat inputImage = shiftImage<unsigned char>(input, workerID * input.cols / nb_shift, 0);

	process(inputImage, outputs[workerID], conf.maxDim, conf.dilatationWidth1, conf.dilatationWidth2, conf.normalize, conf.handleBorder, conf.colorSpace, conf.whitening, conf.sampleStep, conf.blurStd, conf.equirectangular);
}

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

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

	// ------------------------------------------------------------------------------------------------------------------------------------------
	// parse parameters

	namespace po = boost::program_options;

	po::positional_options_description p;
	p.add("input-file", -1);

	po::options_description desc("Allowed options");
	desc.add_options()
			("help", "produce help message")
			("input-file,i", po::value< std::string >(), "Input image to process.")
			("output-file,o", po::value< std::string >(), "Output image.")
	;

	po::options_description sl("Per-projection saliency estimation options");
	sl.add_options()
			("step-size", po::value< int >(), "Steps in the generation of the boolean maps. [default]: 8")
			("max-dim", po::value< int >(), "Maximum dimension of the image used for estimating Saliency. [default]: 400 ")
			("dilatation-width-1", po::value< int >(), "Size of the dilatation kernel performed before L2 normalization: [default]: max(round(7*max_dim/400),1); ")
			("dilatation-width-2", po::value< int >(), "Size of the dilatation kernel performed before the Gaussian blurring kernel: [default]: round(9*max_dim/400); sigma.")
			("blur-std", po::value< float >(), "Standard deviation final overall Gaussian blur processing. ")
			("normalize", po::value< int >(), "Normalize the saliency map. ")
			("handle-border", po::value< int >(), "Handle the border of the images (convolution issues). ")
			("color-space", po::value< int >(), "1) RGB, 2) Lab, 4) Luv [default]: 2")
			("whitening", po::value< int >(), "Do color whitening. [default]: 1")
			("equatorial-prior", "Apply an equatorial-prior to the images. This was designed for equirectangular images. It may not be a good idea for rectilinear images.")
			("apply-fms", po::value< int >(), "Compute the FMS model: apply the saliency algorithm on several shifted images. The provided parameter is the number of projection (4 recommended)")
			("equirectangular-mode", "Consider an equirectangular image as input")
	;

	desc.add(sl);

	po::variables_map vm;
	try {
		po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
		po::notify(vm);
	} catch(boost::exception &) {
		std::cerr << "Error incorect program options. See --help... \n";
		return 0;
	}

	if (vm.count("help")) {
		std::cout << "--------------------------------------------------------------------------------\n";
		std::cout << "\t\tTest saliency estimation on 2D images\n";
		std::cout << "--------------------------------------------------------------------------------\n";
		std::cout << "\n\n";
		std::cout << desc << "\n";
		return 1;
	}

	std::string inputPath;
	std::string outputPath;

	int sampleStep = 8;
	int dilatationWidth1 = 3;
	int dilatationWidth2 = 11;
	float blurStd = 20;
	int normalize = 1;
	int handleBorder = 0;
	int colorSpace = CL_Lab;
	bool whitening = true;
	int maxDim = 400;
	bool equatorialPrior = false;

	if (vm.count("input-file")) {
		inputPath = vm["input-file"].as< std::string >();
	} else {
		std::cerr << "It is required to provide the an input image. See --help\n";
		return 0;
	}

	if (vm.count("output-file")) {
		outputPath = vm["output-file"].as< std::string >();
	}

	// ---------------------------------------------------------------------------------------------------
	// Saliency BMS settings

	if (vm.count("step-size")) {
		sampleStep = vm["step-size"].as< int >();
	}

	if (vm.count("max-dim")) {
		maxDim = vm["max-dim"].as< int >();
		dilatationWidth1 = static_cast<int>(fmax(round(7*maxDim/400.f),1.f));
		dilatationWidth2 = static_cast<int>(fmax(round(9*maxDim/400.f),1.f));
		blurStd = round(9*maxDim/400.f);
	}

	if (vm.count("dilatation-width-1")) {
		dilatationWidth1 = vm["dilatation-width-1"].as< int >();
	}

	if (vm.count("dilatation-width-2")) {
		dilatationWidth2 = vm["dilatation-width-2"].as< int >();
	}

	if (vm.count("blur-std")) {
		blurStd = vm["blur-std"].as< float >();
	}

	if (vm.count("normalize")) {
		normalize = vm["normalize"].as< int >() == 1;
	}

	if (vm.count("handle-border")) {
		handleBorder = vm["handle-border"].as< int >() == 1;
	}

	if (vm.count("color-space")) {
		colorSpace = vm["color-space"].as< int >();
	}

	if (vm.count("whitening")) {
		whitening = vm["whitening"].as< int >() == 1;
	}

	if(vm.count("equatorial-prior")) {
		equatorialPrior = true;
	}

	int nb_projections = 1;
	if(vm.count("apply-fms")) {
		nb_projections = vm["apply-fms"].as< int >();
	}

	bool equirectangularMode = false;
	if(vm.count("equirectangular-mode")){
		equirectangularMode = true;
	}

	// ---------------------------------------------------------------------------------------------------
	// start program

	cv::Mat inputImage = cv::imread(inputPath);
	cv::Mat sMap;

    if(inputImage.empty()) {
        std::cerr << "cannot open: "<< inputPath << std::endl;
        return 0;
    }

    Configuration conf;

    conf.sampleStep = sampleStep;
	conf.dilatationWidth1 = dilatationWidth1;
	conf.dilatationWidth2 = dilatationWidth2;
	conf.blurStd = blurStd;
	conf.normalize = normalize;
	conf.handleBorder = handleBorder;
	conf.colorSpace = colorSpace;
	conf.whitening = whitening;
	conf.maxDim = maxDim;
	conf.equatorialPrior = equatorialPrior;
	conf.equirectangular = equirectangularMode;

    std::vector<cv::Mat> outputs(nb_projections);
	if(nb_projections > 1) {
		boost::thread_group g;
		for(int i = 0 ; i < nb_projections ; ++i) {
			g.create_thread(boost::bind(processJob, i, nb_projections, boost::ref(inputImage), boost::ref(outputs), boost::ref(conf)));
		}
		g.join_all();
	} else {
		processJob(0, nb_projections, inputImage, outputs, conf);
	}

    sMap = outputs[0];
    for(int i = 1 ; i < nb_projections ; ++i) {
    	sMap = sMap + shiftImage<float>(outputs[i], -i * outputs[i].cols / nb_projections, 0);
    }

	if(normalize) {
		if (blurStd > 0) {
			int blur_width = (int)MIN(floor(blurStd) * 4 + 1, 51);
			cv::GaussianBlur(sMap, sMap, cv::Size(blur_width, blur_width), blurStd, blurStd);
		}

		if(nb_projections > 0) {
			double mn, mx;
			cv::minMaxLoc(sMap, &mn, &mx);
			sMap = ((sMap-mn) / (mx - mn)) ;
		}
	}

	cv::resize(sMap, sMap, inputImage.size());

	if(equatorialPrior)
		applyEquatorialPrior(sMap);

	if(!outputPath.empty()) {
		cv::Mat tmp;
		cv::cvtColor(sMap, tmp, cv::COLOR_GRAY2BGR);
		tmp *= 255;
		tmp.convertTo(tmp, CV_8UC3);
		cv::imwrite(outputPath, tmp);

	} else {
		imshow("saliency", sMap);
		cv::waitKey();
	}

	return 0;
}


/*
 * Rasterizer header
 *
 * This file is part of the "SoftPixel Engine" (Copyright (c) 2008 by Lukas Hermanns)
 * See "SoftPixelEngine.hpp" for license information.
 */

#ifndef __SP_MATH_RASTERIZER_H__
#define __SP_MATH_RASTERIZER_H__

#include "Base/spStandard.hpp"
#include "Base/spMath.hpp"

#include <boost/function.hpp>

namespace sp
{
namespace math
{

//! Callback function interface for rasterization without vertices.
typedef boost::function<void (s32 x, s32 y, void* UserData)> RenderPixelCallback;

/**
Rasterizer vertex class interface. Write your own vertex class and use the same interface.
You do not need to inherit from this class but this is also allowed.
Then use your vertex class to rasterize triangles or other primitives.
\see Rasterizer
*/
class SP_EXPORT RasterizerVertex
{

    public:

        RasterizerVertex()
        {
        }
        virtual ~RasterizerVertex()
        {
        }

        /* Operators */

        virtual RasterizerVertex& operator = (const RasterizerVertex &Other) = 0;

        virtual RasterizerVertex& operator += (const RasterizerVertex &Other) = 0;
        virtual RasterizerVertex& operator -= (const RasterizerVertex &Other) = 0;

        virtual RasterizerVertex& operator *= (f32 Factor) = 0;
        virtual RasterizerVertex& operator /= (f32 Factor) = 0;

        /* Functions */

        //! Returns the X screen coordinate.
        virtual s32 getScreenCoordX() const = 0;
        //! Returns the Y screen coordinate.
        virtual s32 getScreenCoordY() const = 0;

};

//! Namespace for rasterization templates.
namespace Rasterizer
{

//! Only used internally for the rasterizeTriangle template function.
template <class VtxT> void computeRasterScanline(
    const VtxT* (&v)[3], s32 y, s32 yStart, s32 yMiddle, VtxT &a, VtxT &b)
{
    /* Get delta-y as float */
    f32 dy = static_cast<f32>(y - yStart);

    /* Compute interpolation factor */
    f32 Factor = dy / (v[2]->getScreenCoordY() - v[0]->getScreenCoordY());

    /* Interpolate left vertex: a = v0 + (v2 - v0) * Factor */
    math::lerp(a, *v[0], *v[2], Factor);

    if (y < yMiddle)
    {
        /* Compute interpolation factor */
        Factor = dy / (v[1]->getScreenCoordY() - v[0]->getScreenCoordY());

        /* Interpolate right vertex: b = v0 + (v1 - v0) * Factor */
        math::lerp(b, *v[0], *v[1], Factor);
    }
    else
    {
        /* Update delta-y */
        dy = static_cast<f32>(y - yMiddle);

        /* Compute interpolation factor */
        Factor = dy / (v[2]->getScreenCoordY() - v[1]->getScreenCoordY());

        /* Interpolate right vertex: b = v1 + (v2 - v1) * Factor */
        math::lerp(b, *v[1], *v[2], Factor);
    }
}

/**
Rasterizes a triangle onto a 2D screen.
\tparam VtxT Specifies your own vertex class. See the RasterizerVertex class and implement your
own vertex class with the same interface.
\param[in] RenderPixelCallback Specifies the pixel rendering callback. Without this callback
the function renders nothing. Use this to draw a pixel onto a screen or image buffer with the given x and y coordainte.
\param[in] VertexA First vertex for the triangle.
\param[in] VertexB Second vertex for the triangle.
\param[in] VertexC Third vertex for the triangle.
\param[in] UserData Any user data which will be passed to the render callback.
\see RasterizerVertex
*/
template <class VtxT> void rasterizeTriangle(
    const boost::function<void (s32 x, s32 y, const VtxT &Vertex, void* UserData)> &RenderPixelCallback,
    const VtxT &VertexA, const VtxT &VertexB, const VtxT &VertexC, void* UserData = 0)
{
    /* Store vertices as pointers in array */
    const VtxT* v[3] = { &VertexA, &VertexB, &VertexC };

    /* Sort the vertices in dependet of the y axis */
    if (v[0]->getScreenCoordY() > v[1]->getScreenCoordY())
        std::swap(v[0], v[1]);
    if (v[0]->getScreenCoordY() > v[2]->getScreenCoordY())
        std::swap(v[0], v[2]);
    if (v[1]->getScreenCoordY() > v[2]->getScreenCoordY())
        std::swap(v[1], v[2]);

    /* Get screen coordinates */
    s32 v0x = v[0]->getScreenCoordX();
    s32 v1x = v[1]->getScreenCoordX();
    s32 v2x = v[2]->getScreenCoordX();

    /* Set the vertices position */
    s32 yStart  = v[0]->getScreenCoordY();
    s32 yMiddle = v[1]->getScreenCoordY();
    s32 yEnd    = v[2]->getScreenCoordY();

    /* Compute the dimensions */
    s32 yMiddleStart    = yMiddle - yStart;
    s32 yEndMiddle      = yEnd - yMiddle;
    s32 yEndStart       = yEnd - yStart;

    /* Iteration variables */
    s32 x, y, xStart, xEnd;
    VtxT lside, rside, step, cur;

    /* Rater each scanline from top to bottom */
    for (y = yStart; y < yEnd; ++y)
    {
        /* Compute the scanline dimension */
        if (y < yMiddle)
            xStart = v0x + ( v1x - v0x ) * ( y - yStart ) / yMiddleStart;
        else if (y > yMiddle)
            xStart = v1x + ( v2x - v1x ) * ( y - yMiddle ) / yEndMiddle;
        else
            xStart = v1x;

        xEnd = v0x + ( v2x - v0x ) * ( y - yStart ) / yEndStart;

        /* Compute the polygon sides */
        if (xStart > xEnd)
        {
            std::swap(xStart, xEnd);
            computeRasterScanline(v, y, yStart, yMiddle, lside, rside);
        }
        else
            computeRasterScanline(v, y, yStart, yMiddle, rside, lside);

        /* Compute the steps */
        f32 dx = static_cast<f32>(xEnd - xStart);

        step = rside;
        step -= lside;
        step /= dx;

        cur = lside;

        /* Render each pixel in scanline */
        for (x = xStart; x < xEnd; ++x)
        {
            RenderPixelCallback(x, y, cur, UserData);

            /* Interpolate vertex between left- and right side of scanline */
            cur += step;
        }
    }
}

/**
Rasterizes a line onto a 2D screen using the "Bresenham" algorithm.
\tparam VtxT Specifies your own vertex class. See the RasterizerVertex class and implement your
own vertex class with the same interface.
\param[in] RenderPixelCallback Specifies the pixel rendering callback. Without this callback
the function renders nothing. Use this to draw a pixel onto a screen or image buffer with the given x and y coordainte.
\param[in] VertexA First vertex for the line.
\param[in] VertexB Second vertex for the line.
\param[in] UserData Any user data which will be passed to the render callback.
\see RasterizerVertex
*/
template <class VtxT> void rasterizeLine(
    const boost::function<void (s32 x, s32 y, const VtxT &Vertex, void* UserData)> &RenderPixelCallback,
    const VtxT &VertexA, const VtxT &VertexB, void* UserData = 0)
{
    /* Store screen coordinates */
    s32 x1 = VertexA.getScreenCoordX();
    s32 y1 = VertexA.getScreenCoordY();

    s32 x2 = VertexB.getScreenCoordX();
    s32 y2 = VertexB.getScreenCoordY();

    /* Pre-Compuations of the "Bresenham" algorithm */
    s32 dx = x2 - x1;
    s32 dy = y2 - y1;

    s32 incx = (dx > 0) ? 1 : (dx < 0) ? -1 : 0;
    s32 incy = (dy > 0) ? 1 : (dy < 0) ? -1 : 0;

    if (dx < 0) dx = -dx;
    if (dy < 0) dy = -dy;

    s32 pdx, pdy, ddx, ddy, es, el;

    if (dx > dy)
    {
        pdx = incx;
        pdy = 0;
        ddx = incx;
        ddy = incy;
        es  = dy;
        el  = dx;
    }
    else
    {
        pdx = 0;
        pdy = incy;
        ddx = incx;
        ddy = incy;
        es  = dx;
        el  = dy;
    }

    if (el == 0)
        return;

    s32 x   = x1;
    s32 y   = y1;
    s32 err = el/2;

    /* Compute the steps */
    VtxT step, cur;

    step = VertexB;
    step -= VertexA;
    step /= static_cast<f32>(el);

    cur = VertexA;

    /* Render each pixel of the line */
    for (s32 t = 0; t < el; ++t)
    {
        /* Render pixel */
        RenderPixelCallback(x, y, cur, UserData);

        cur += step;

        /* Move to next pixel */
        err -= es;
        if (err < 0)
        {
            err += el;
            x += ddx;
            y += ddy;
        }
        else
        {
            x += pdx;
            y += pdy;
        }
    }
}

/**
Rasterizes a triangle onto a 2D screen.
\param[in] RenderCallback Specifies the pixel rendering callback. Without this callback
the function renders nothing. Use this to draw a pixel onto a screen or image buffer with the given x and y coordainte.
\param[in] PointA Specifies the first 2D point of the triangle.
\param[in] PointB Specifies the second 2D point of the triangle.
\param[in] PointC Specifies the third 2D point of the triangle.
\param[in] UserData Any user data which will be passed to the render callback.
\note This function does not interpolate any values between the triangle points.
For this feature use the template variant.
*/
SP_EXPORT void rasterizeTriangle(
    const RenderPixelCallback &RenderCallback,
    dim::point2di PointA, dim::point2di PointB, dim::point2di PointC,
    void* UserData = 0
);

/**
Rasterizes a circle onto a 2D screen using the "Bresenham" algorithm.
\param[in] RenderCallback Specifies the pixel rendering callback. Without this callback
the function renders nothing. Use this to draw a pixel onto a screen or image buffer with the given x and y coordainte.
\param[in] Position Specifies the 2D position of the circle.
\param[in] Radius Specifies the radius of the circle.
\param[in] UserData Any user data which will be passed to the render callback.
*/
SP_EXPORT void rasterizeCircle(
    const RenderPixelCallback &RenderCallback, const dim::point2di &Position, s32 Radius, void* UserData = 0
);

/**
Rasterizes an ellipse onto a 2D screen using the "Bresenham" algorithm.
\param[in] RenderCallback Specifies the pixel rendering callback. Without this callback
the function renders nothing. Use this to draw a pixel onto a screen or image buffer with the given x and y coordainte.
\param[in] Position Specifies the 2D position of the ellipse.
\param[in] Radius Specifies the radius of the ellipse.
\param[in] UserData Any user data which will be passed to the render callback.
*/
SP_EXPORT void rasterizeEllipse(
    const RenderPixelCallback &RenderCallback, const dim::point2di &Position, const dim::size2di &Radius, void* UserData = 0
);

} // /namespace Rasterizer

} // /namespace math

} // /namespace sp

#endif

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


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef ITL_PC_ILU_INCLUDE
#define ITL_PC_ILU_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/operation/adjoint.hpp>
#include <boost/numeric/mtl/operation/lower_trisolve.hpp>
#include <boost/numeric/mtl/operation/upper_trisolve.hpp>
#include <boost/numeric/mtl/operation/lu.hpp>
#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/numeric/itl/pc/solver.hpp>

namespace itl { namespace pc {

/// Incomplete LU factorization of a \p Matrix into matrices of type \p Value using a \p Factorizer (e.g. ILU(0) or ILUT)
template <typename Matrix, typename Factorizer, typename Value= typename mtl::Collection<Matrix>::value_type>
class ilu
{
  public:
    typedef Value                                         value_type;
    typedef typename mtl::Collection<Matrix>::size_type   size_type;
    typedef ilu                                           self;
    typedef Factorizer                                    factorizer_type;

    typedef mtl::matrix::parameters<mtl::row_major, mtl::index::c_index, mtl::non_fixed::dimensions, false, size_type> para;
    typedef mtl::matrix::compressed2D<value_type, para>                     L_type;
    typedef mtl::matrix::compressed2D<value_type, para>                     U_type;
    typedef typename mtl::matrix::traits::adjoint<L_type>::type     adjoint_L_type;
    typedef typename mtl::matrix::traits::adjoint<U_type>::type     adjoint_U_type;

    typedef mtl::matrix::detail::lower_trisolve_t<L_type, mtl::tag::unit_diagonal, true>    lower_solver_t;
    typedef mtl::matrix::detail::upper_trisolve_t<U_type, mtl::tag::inverse_diagonal, true> upper_solver_t;

    typedef mtl::matrix::detail::lower_trisolve_t<adjoint_U_type, mtl::tag::inverse_diagonal, true> adjoint_lower_solver_t;
    typedef mtl::matrix::detail::upper_trisolve_t<adjoint_L_type, mtl::tag::unit_diagonal, true>    adjoint_upper_solver_t;

    /// Factorization adapted from Saad
    explicit ilu(const Matrix& A)
      : f(A, L, U), lower_solver(L), upper_solver(U), adjoint_L(adjoint(L)), adjoint_U(adjoint(U)),
	adjoint_lower_solver(adjoint_U), adjoint_upper_solver(adjoint_L) {}

    template <typename FactPara>
    ilu(const Matrix& A, const FactPara& p)
      : f(A, p, L, U), lower_solver(L), upper_solver(U), adjoint_L(adjoint(L)), adjoint_U(adjoint(U)),
	adjoint_lower_solver(adjoint_U), adjoint_upper_solver(adjoint_L) {}

    /// Solve  LU y = x --> y= U^{-1} L^{-1} x
    template <typename Vector>
    Vector solve(const Vector& x) const
    {
	Vector y;
	solve(x, y);
	return y;
    }

    // solve x = L y --> y0= L^{-1} x
    template <typename VectorIn, typename VectorOut>
    const VectorOut& solve_lower(const VectorIn& x, VectorOut&) const
    {
	static VectorOut y0;
	y0.change_resource(resource(x));
	lower_solver(x, y0);
	return y0;
    }

    // Solve  LU y = x --> y= U^{-1} L^{-1} x
    template <typename VectorIn, typename VectorOut>
    void solve(const VectorIn& x, VectorOut& y) const
    {
	mtl::vampir_trace<5039> tracer;
	const VectorOut& y0= solve_lower(x, y);

	y.checked_change_resource(x);
	upper_solver(y0, y);
    }

    /// Solve (LU)^H y = x --> y= L^{-H} U^{-H} x
    template <typename Vector>
    Vector adjoint_solve(const Vector& x) const
    {
	mtl::vampir_trace<5040> tracer;
	Vector y(resource(x));
	adjoint_solve(x, y);
	return y;
    }

    /// Solve (LU)^H y = x --> y= L^{-H} U^{-H} x
    template <typename VectorIn, typename VectorOut>
    void adjoint_solve(const VectorIn& x, VectorOut& y) const
    {
	mtl::vampir_trace<5040> tracer;
	y.checked_change_resource(x);
	// y= unit_upper_trisolve(adjoint(L), inverse_lower_trisolve(adjoint(U), x));
	static VectorOut y0;
	y0.change_resource(resource(x));
	adjoint_lower_solver(x, y0);
	adjoint_upper_solver(y0, y);
    }

    L_type get_L() { return L; }
    U_type get_U() { return U; }

  public:
    L_type                      L;
    U_type                      U;
  private:
    Factorizer                  f;
    lower_solver_t              lower_solver;
    upper_solver_t              upper_solver;
    adjoint_L_type              adjoint_L;
    adjoint_U_type              adjoint_U;
    adjoint_lower_solver_t      adjoint_lower_solver;
    adjoint_upper_solver_t      adjoint_upper_solver;

};

template <typename Value, typename Factorizer, typename V2>
class ilu<mtl::matrix::dense2D<Value, mtl::matrix::parameters<> >, Factorizer, V2> // last 2 arguments are dummies
{
  public:
    typedef mtl::matrix::dense2D<Value, mtl::matrix::parameters<> >    Matrix;
    typedef typename mtl::Collection<Matrix>::value_type  value_type;
    typedef typename mtl::Collection<Matrix>::size_type   size_type;
    typedef ilu                                           self;
    typedef Matrix                                        LU_type;

    ilu(const Matrix& A) : LU(A) { lu(LU, P); std::cout << "LU is\n" << LU << "P is " << P << "\n"; }

    // Solve  P^{-1}LU x = b --> x= U^{-1} L^{-1} P b
    template <typename Vector>
    Vector solve(const Vector& b) const { return lu_apply(LU, P, b); }

    // Solve  P^{-1}LU x = b --> x= U^{-1} L^{-1} P b
    template <typename VectorIn, typename VectorOut>
    void solve(const VectorIn& b, VectorOut& x) const { x= lu_apply(LU, P, b); }

    // Solve (P^{-1}LU)^H x = b --> x= P^{-1}L^{-H} U^{-H} b // P^{-1}^{-1}^H = P^{-1})
    template <typename Vector>
    Vector adjoint_solve(const Vector& b) const { return lu_adjoint_apply(LU, P, b); }

    // Solve (P^{-1}LU)^H x = b --> x= P^{-1}L^{-H} U^{-H} b // P^{-1}^{-1}^H = P^{-1})
    template <typename VectorIn, typename VectorOut>
    void adjoint_solve(const VectorIn& b, VectorOut& x) const { x= lu_adjoint_apply(LU, P, b); }

  private:
    LU_type                        LU;
    mtl::vector::dense_vector<size_type, mtl::vector::parameters<> >   P;
};

#if 0
template <typename Matrix, typename Factorizer, typename Value, typename Vector>
struct ilu_solver
  : mtl::vector::assigner<ilu_solver<Matrix, Factorizer, Value, Vector> >
{
    typedef ilu<Matrix, Factorizer, Value> pc_type;

    ilu_solver(const pc_type& P, const Vector& x) : P(P), x(x) {}

    template <typename VectorOut>
    void assign_to(VectorOut& y) const
    {	P.solve(x, y);    }

    const pc_type&       P;
    const Vector&        x;
};
#endif

/// Solve LU x = b --> x= U^{-1} L^{-1} b
template <typename Matrix, typename Factorizer, typename Value, typename Vector>
// ilu_solver<Matrix, Factorizer, Value, Vector>
solver<ilu<Matrix, Factorizer, Value>, Vector, false>
inline solve(const ilu<Matrix, Factorizer, Value>& P, const Vector& x)
{
    return solver<ilu<Matrix, Factorizer, Value>, Vector, false>(P, x);
}

/// Solve (LU)^H x = b --> x= L^{-H} U^{-H} b
template <typename Matrix, typename Factorizer, typename Value, typename Vector>
// Vector
solver<ilu<Matrix, Factorizer, Value>, Vector, true>
inline adjoint_solve(const ilu<Matrix, Factorizer, Value>& P, const Vector& b)
{
    return solver<ilu<Matrix, Factorizer, Value>, Vector, true>(P, b);
}

// ic_0_evaluator not needed IC(0) and ILU(0) do the same at the upper triangle ;-)

}} // namespace itl::pc

#endif // ITL_PC_ILU_INCLUDE