//
//  Copyright (c) 2000-2002
//  Joerg Walter, Mathias Koch
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  GeNeSys mbH & Co. KG in producing this work.
//

#ifndef _BOOST_UBLAS_TRAITS_
#define _BOOST_UBLAS_TRAITS_

#include <iterator>
#include <complex>
#include <boost/config/no_tr1/cmath.hpp>

#include <boost/numeric/ublas/detail/config.hpp>
#include <boost/numeric/ublas/detail/iterator.hpp>
#include <boost/numeric/ublas/detail/returntype_deduction.hpp>

#include <boost/type_traits.hpp>
#include <complex>
#include <boost/typeof/typeof.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/mpl/and.hpp>

// anonymous namespace to avoid ADL issues
namespace {
  template<class T> T boost_numeric_ublas_sqrt (const T& t) {
    using namespace std;
    // we'll find either std::sqrt or else another version via ADL:
    return sqrt (t);
  }
  template<class T> T boost_numeric_ublas_abs (const T& t) {
    using namespace std;
    // we'll find either std::abs or else another version via ADL:
    return abs (t);
  }
}

namespace boost { namespace numeric { namespace ublas {

    // Use Joel de Guzman's return type deduction
    // uBLAS assumes a common return type for all binary arithmetic operators
    template<class X, class Y>
    struct promote_traits {
        typedef type_deduction_detail::base_result_of<X, Y> base_type;
        static typename base_type::x_type x;
        static typename base_type::y_type y;
        static const std::size_t size = sizeof (
                type_deduction_detail::test<
                    typename base_type::x_type
                  , typename base_type::y_type
                >(x + y)     // Use x+y to stand of all the arithmetic actions
            );

        static const std::size_t index = (size / sizeof (char)) - 1;
        typedef typename mpl::at_c<
            typename base_type::types, index>::type id;
        typedef typename id::type promote_type;
    };

      template<typename R, typename I>
      typename boost::enable_if<
        mpl::and_<
          boost::is_float<R>,
          boost::is_integral<I>
          >,
        std::complex<R> >::type inline operator+ (I in1, std::complex<R> const& in2 ) {
        return R (in1) + in2;
      }

      template<typename R, typename I>
      typename boost::enable_if<
        mpl::and_<
          boost::is_float<R>,
          boost::is_integral<I>
          >,
        std::complex<R> >::type inline operator+ (std::complex<R> const& in1, I in2) {
        return in1 + R (in2);
      }

      template<typename R, typename I>
      typename boost::enable_if<
        mpl::and_<
          boost::is_float<R>,
          boost::is_integral<I>
          >,
        std::complex<R> >::type inline operator- (I in1, std::complex<R> const& in2) {
        return R (in1) - in2;
      }

      template<typename R, typename I>
      typename boost::enable_if<
        mpl::and_<
          boost::is_float<R>,
          boost::is_integral<I>
          >,
        std::complex<R> >::type inline operator- (std::complex<R> const& in1, I in2) {
        return in1 - R (in2);
      }

      template<typename R, typename I>
      typename boost::enable_if<
        mpl::and_<
          boost::is_float<R>,
          boost::is_integral<I>
          >,
        std::complex<R> >::type inline operator* (I in1, std::complex<R> const& in2) {
        return R (in1) * in2;
      }

      template<typename R, typename I>
      typename boost::enable_if<
        mpl::and_<
          boost::is_float<R>,
          boost::is_integral<I>
          >,
        std::complex<R> >::type inline operator* (std::complex<R> const& in1, I in2) {
        return in1 * R(in2);
      }

      template<typename R, typename I>
      typename boost::enable_if<
        mpl::and_<
          boost::is_float<R>,
          boost::is_integral<I>
          >,
        std::complex<R> >::type inline operator/ (I in1, std::complex<R> const& in2) {
        return R(in1) / in2;
      }

      template<typename R, typename I>
      typename boost::enable_if<
        mpl::and_<
          boost::is_float<R>,
          boost::is_integral<I>
          >,
        std::complex<R> >::type inline operator/ (std::complex<R> const& in1, I in2) {
        return in1 / R (in2);
      }

    // Type traits - generic numeric properties and functions
    template<class T>
    struct type_traits;

    // Define properties for a generic scalar type
    template<class T>
    struct scalar_traits {
        typedef scalar_traits<T> self_type;
        typedef T value_type;
        typedef const T &const_reference;
        typedef T &reference;

        typedef T real_type;
        typedef real_type precision_type;       // we do not know what type has more precision then the real_type

        static const unsigned plus_complexity = 1;
        static const unsigned multiplies_complexity = 1;

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                return t;
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference /*t*/) {
                return 0;
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                return t;
        }

        static
        BOOST_UBLAS_INLINE
        real_type type_abs (const_reference t) {
            return boost_numeric_ublas_abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type type_sqrt (const_reference t) {
            // force a type conversion back to value_type for intgral types
            return value_type (boost_numeric_ublas_sqrt (t));
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            return self_type::type_abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::type_abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            return self_type::type_abs (t);
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            return self_type::norm_inf (t1 - t2) < BOOST_UBLAS_TYPE_CHECK_EPSILON *
                   (std::max) ((std::max) (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             BOOST_UBLAS_TYPE_CHECK_MIN);
        }
    };

    // Define default type traits, assume T is a scalar type
    template<class T>
    struct type_traits : scalar_traits <T> {
        typedef type_traits<T> self_type;
        typedef T value_type;
        typedef const T &const_reference;
        typedef T &reference;

        typedef T real_type;
        typedef real_type precision_type;
        static const unsigned multiplies_complexity = 1;

    };

    // Define real type traits
    template<>
    struct type_traits<float> : scalar_traits<float> {
        typedef type_traits<float> self_type;
        typedef float value_type;
        typedef const value_type &const_reference;
        typedef value_type &reference;
        typedef value_type real_type;
        typedef double precision_type;
    };
    template<>
    struct type_traits<double> : scalar_traits<double> {
        typedef type_traits<double> self_type;
        typedef double value_type;
        typedef const value_type &const_reference;
        typedef value_type &reference;
        typedef value_type real_type;
        typedef long double precision_type;
    };
    template<>
    struct type_traits<long double>  : scalar_traits<long double> {
        typedef type_traits<long double> self_type;
        typedef long double value_type;
        typedef const value_type &const_reference;
        typedef value_type &reference;
        typedef value_type real_type;
        typedef value_type precision_type;
    };

    // Define properties for a generic complex type
    template<class T>
    struct complex_traits {
        typedef complex_traits<T> self_type;
        typedef T value_type;
        typedef const T &const_reference;
        typedef T &reference;

        typedef typename T::value_type real_type;
        typedef real_type precision_type;       // we do not know what type has more precision then the real_type

        static const unsigned plus_complexity = 2;
        static const unsigned multiplies_complexity = 6;

        static
        BOOST_UBLAS_INLINE
        real_type real (const_reference t) {
                return std::real (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type imag (const_reference t) {
                return std::imag (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type conj (const_reference t) {
                return std::conj (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type type_abs (const_reference t) {
                return abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        value_type type_sqrt (const_reference t) {
                return sqrt (t);
        }

        static
        BOOST_UBLAS_INLINE
        real_type norm_1 (const_reference t) {
            return self_type::type_abs (t);
            // original computation has been replaced because a complex number should behave like a scalar type
            // return type_traits<real_type>::type_abs (self_type::real (t)) +
            //       type_traits<real_type>::type_abs (self_type::imag (t));
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_2 (const_reference t) {
            return self_type::type_abs (t);
        }
        static
        BOOST_UBLAS_INLINE
        real_type norm_inf (const_reference t) {
            return self_type::type_abs (t);
            // original computation has been replaced because a complex number should behave like a scalar type
            // return (std::max) (type_traits<real_type>::type_abs (self_type::real (t)),
            //                 type_traits<real_type>::type_abs (self_type::imag (t)));
        }

        static
        BOOST_UBLAS_INLINE
        bool equals (const_reference t1, const_reference t2) {
            return self_type::norm_inf (t1 - t2) < BOOST_UBLAS_TYPE_CHECK_EPSILON *
                   (std::max) ((std::max) (self_type::norm_inf (t1),
                                       self_type::norm_inf (t2)),
                             BOOST_UBLAS_TYPE_CHECK_MIN);
        }
    };

    // Define complex type traits
    template<>
    struct type_traits<std::complex<float> > : complex_traits<std::complex<float> >{
        typedef type_traits<std::complex<float> > self_type;
        typedef std::complex<float> value_type;
        typedef const value_type &const_reference;
        typedef value_type &reference;
        typedef float real_type;
        typedef std::complex<double> precision_type;

    };
    template<>
    struct type_traits<std::complex<double> > : complex_traits<std::complex<double> >{
        typedef type_traits<std::complex<double> > self_type;
        typedef std::complex<double> value_type;
        typedef const value_type &const_reference;
        typedef value_type &reference;
        typedef double real_type;
        typedef std::complex<long double> precision_type;
    };
    template<>
    struct type_traits<std::complex<long double> > : complex_traits<std::complex<long double> > {
        typedef type_traits<std::complex<long double> > self_type;
        typedef std::complex<long double> value_type;
        typedef const value_type &const_reference;
        typedef value_type &reference;
        typedef long double real_type;
        typedef value_type precision_type;
    };

#ifdef BOOST_UBLAS_USE_INTERVAL
    // Define scalar interval type traits
    template<>
    struct type_traits<boost::numeric::interval<float> > : scalar_traits<boost::numeric::interval<float> > {
        typedef type_traits<boost::numeric::interval<float> > self_type;
        typedef boost::numeric::interval<float> value_type;
        typedef const value_type &const_reference;
        typedef value_type &reference;
        typedef value_type real_type;
        typedef boost::numeric::interval<double> precision_type;

    };
    template<>
    struct type_traits<boost::numeric::interval<double> > : scalar_traits<boost::numeric::interval<double> > {
        typedef type_traits<boost::numeric::interval<double> > self_type;
        typedef boost::numeric::interval<double> value_type;
        typedef const value_type &const_reference;
        typedef value_type &reference;
        typedef value_type real_type;
        typedef boost::numeric::interval<long double> precision_type;
    };
    template<>
    struct type_traits<boost::numeric::interval<long double> > : scalar_traits<boost::numeric::interval<long double> > {
        typedef type_traits<boost::numeric::interval<long double> > self_type;
        typedef boost::numeric::interval<long double> value_type;
        typedef const value_type &const_reference;
        typedef value_type &reference;
        typedef value_type real_type;
        typedef value_type precision_type;
    };
#endif

    // Storage tags -- hierarchical definition of storage characteristics

    struct unknown_storage_tag {};
    struct sparse_proxy_tag: public unknown_storage_tag {};
    struct sparse_tag: public sparse_proxy_tag {};
    struct packed_proxy_tag: public sparse_proxy_tag {};
    struct packed_tag: public packed_proxy_tag {};
    struct dense_proxy_tag: public packed_proxy_tag {};
    struct dense_tag: public dense_proxy_tag {};

    template<class S1, class S2>
    struct storage_restrict_traits {
        typedef S1 storage_category;
    };

    template<>
    struct storage_restrict_traits<sparse_tag, dense_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<sparse_tag, packed_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<sparse_tag, sparse_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    template<>
    struct storage_restrict_traits<packed_tag, dense_proxy_tag> {
        typedef packed_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<packed_tag, packed_proxy_tag> {
        typedef packed_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<packed_tag, sparse_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    template<>
    struct storage_restrict_traits<packed_proxy_tag, sparse_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    template<>
    struct storage_restrict_traits<dense_tag, dense_proxy_tag> {
        typedef dense_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<dense_tag, packed_proxy_tag> {
        typedef packed_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<dense_tag, sparse_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    template<>
    struct storage_restrict_traits<dense_proxy_tag, packed_proxy_tag> {
        typedef packed_proxy_tag storage_category;
    };
    template<>
    struct storage_restrict_traits<dense_proxy_tag, sparse_proxy_tag> {
        typedef sparse_proxy_tag storage_category;
    };

    // Iterator tags -- hierarchical definition of storage characteristics

    struct sparse_bidirectional_iterator_tag : public std::bidirectional_iterator_tag {};
    struct packed_random_access_iterator_tag : public std::random_access_iterator_tag {};
    struct dense_random_access_iterator_tag : public packed_random_access_iterator_tag {};

    // Thanks to Kresimir Fresl for convincing Comeau with iterator_base_traits ;-)
    template<class IC>
    struct iterator_base_traits {};

    template<>
    struct iterator_base_traits<std::forward_iterator_tag> {
        template<class I, class T>
        struct iterator_base {
            typedef forward_iterator_base<std::forward_iterator_tag, I, T> type;
        };
    };

    template<>
    struct iterator_base_traits<std::bidirectional_iterator_tag> {
        template<class I, class T>
        struct iterator_base {
            typedef bidirectional_iterator_base<std::bidirectional_iterator_tag, I, T> type;
        };
    };
    template<>
    struct iterator_base_traits<sparse_bidirectional_iterator_tag> {
        template<class I, class T>
        struct iterator_base {
            typedef bidirectional_iterator_base<sparse_bidirectional_iterator_tag, I, T> type;
        };
    };

    template<>
    struct iterator_base_traits<std::random_access_iterator_tag> {
        template<class I, class T>
        struct iterator_base {
            typedef random_access_iterator_base<std::random_access_iterator_tag, I, T> type;
        };
    };
    template<>
    struct iterator_base_traits<packed_random_access_iterator_tag> {
        template<class I, class T>
        struct iterator_base {
            typedef random_access_iterator_base<packed_random_access_iterator_tag, I, T> type;
        };
    };
    template<>
    struct iterator_base_traits<dense_random_access_iterator_tag> {
        template<class I, class T>
        struct iterator_base {
            typedef random_access_iterator_base<dense_random_access_iterator_tag, I, T> type;
        };
    };

    template<class I1, class I2>
    struct iterator_restrict_traits {
        typedef I1 iterator_category;
    };

    template<>
    struct iterator_restrict_traits<packed_random_access_iterator_tag, sparse_bidirectional_iterator_tag> {
        typedef sparse_bidirectional_iterator_tag iterator_category;
    };
    template<>
    struct iterator_restrict_traits<sparse_bidirectional_iterator_tag, packed_random_access_iterator_tag> {
        typedef sparse_bidirectional_iterator_tag iterator_category;
    };

    template<>
    struct iterator_restrict_traits<dense_random_access_iterator_tag, sparse_bidirectional_iterator_tag> {
        typedef sparse_bidirectional_iterator_tag iterator_category;
    };
    template<>
    struct iterator_restrict_traits<sparse_bidirectional_iterator_tag, dense_random_access_iterator_tag> {
        typedef sparse_bidirectional_iterator_tag iterator_category;
    };

    template<>
    struct iterator_restrict_traits<dense_random_access_iterator_tag, packed_random_access_iterator_tag> {
        typedef packed_random_access_iterator_tag iterator_category;
    };
    template<>
    struct iterator_restrict_traits<packed_random_access_iterator_tag, dense_random_access_iterator_tag> {
        typedef packed_random_access_iterator_tag iterator_category;
    };

    template<class I>
    BOOST_UBLAS_INLINE
    void increment (I &it, const I &it_end, typename I::difference_type compare, packed_random_access_iterator_tag) {
        it += (std::min) (compare, it_end - it);
    }
    template<class I>
    BOOST_UBLAS_INLINE
    void increment (I &it, const I &/* it_end */, typename I::difference_type /* compare */, sparse_bidirectional_iterator_tag) {
        ++ it;
    }
    template<class I>
    BOOST_UBLAS_INLINE
    void increment (I &it, const I &it_end, typename I::difference_type compare) {
        increment (it, it_end, compare, typename I::iterator_category ());
    }

    template<class I>
    BOOST_UBLAS_INLINE
    void increment (I &it, const I &it_end) {
#if BOOST_UBLAS_TYPE_CHECK
        I cit (it);
        while (cit != it_end) {
            BOOST_UBLAS_CHECK (*cit == typename I::value_type/*zero*/(), internal_logic ());
            ++ cit;
        }
#endif
        it = it_end;
    }

    namespace detail {

        // specialisation which define whether a type has a trivial constructor
        // or not. This is used by array types.
        template<typename T>
        struct has_trivial_constructor : public boost::has_trivial_constructor<T> {};

        template<typename T>
        struct has_trivial_destructor : public boost::has_trivial_destructor<T> {};

        template<typename FLT>
        struct has_trivial_constructor<std::complex<FLT> > : public boost::true_type {};

        template<typename FLT>
        struct has_trivial_destructor<std::complex<FLT> > : public boost::true_type {};

    }

    /**  \brief Traits class to extract type information from a constant matrix or vector CONTAINER.
     *
     */
    template < class E >
    struct container_view_traits {
        /// type of indices
        typedef typename E::size_type             size_type;
        /// type of differences of indices
        typedef typename E::difference_type       difference_type;

        /// storage category: \c unknown_storage_tag, \c dense_tag, \c packed_tag, ...
        typedef typename E::storage_category      storage_category;

        /// type of elements
        typedef typename E::value_type            value_type;
        /// const reference to an element
        typedef typename E::const_reference       const_reference;

        /// type used in expressions to mark a reference to this class (usually a const container_reference<const E> or the class itself)
        typedef typename E::const_closure_type    const_closure_type;
    };

    /**  \brief Traits class to extract additional type information from a mutable matrix or vector CONTAINER.
     *
     */
    template < class E >
    struct mutable_container_traits {
        /// reference to an element
        typedef typename E::reference             reference;

        /// type used in expressions to mark a reference to this class (usually a container_reference<E> or the class itself)
        typedef typename E::closure_type          closure_type;
    };

    /**  \brief Traits class to extract type information from a matrix or vector CONTAINER.
     *
     */
    template < class E >
    struct container_traits
        : container_view_traits<E>, mutable_container_traits<E> {

    };

    /**  \brief Traits class to extract type information from a constant MATRIX.
     *
     */
    template < class MATRIX >
    struct matrix_view_traits : container_view_traits <MATRIX> {

        /// orientation of the matrix, either \c row_major_tag, \c column_major_tag or \c unknown_orientation_tag
        typedef typename MATRIX::orientation_category  orientation_category;

        /// row iterator for the matrix
        typedef typename MATRIX::const_iterator1  const_iterator1;

        /// column iterator for the matrix
        typedef typename MATRIX::const_iterator2  const_iterator2;
    };

    /**  \brief Traits class to extract additional type information from a mutable MATRIX.
     *
     */
    template < class MATRIX >
    struct mutable_matrix_traits
        : mutable_container_traits <MATRIX> {

        /// row iterator for the matrix
        typedef typename MATRIX::iterator1  iterator1;

        /// column iterator for the matrix
        typedef typename MATRIX::iterator2  iterator2;
    };

    /**  \brief Traits class to extract type information from a MATRIX.
     *
     */
    template < class MATRIX >
    struct matrix_traits
        : matrix_view_traits <MATRIX>, mutable_matrix_traits <MATRIX> {
    };

    /**  \brief Traits class to extract type information from a VECTOR.
     *
     */
    template < class VECTOR >
    struct vector_view_traits : container_view_traits <VECTOR> {

        /// iterator for the VECTOR
        typedef typename VECTOR::const_iterator  const_iterator;

        /// iterator pointing to the first element
        static
        const_iterator begin(const VECTOR & v) {
            return v.begin();
        }
        /// iterator pointing behind the last element
        static
        const_iterator end(const VECTOR & v) {
            return v.end();
        }

    };

    /**  \brief Traits class to extract type information from a VECTOR.
     *
     */
    template < class VECTOR >
    struct mutable_vector_traits : mutable_container_traits <VECTOR> {
        /// iterator for the VECTOR
        typedef typename VECTOR::iterator  iterator;

        /// iterator pointing to the first element
        static
        iterator begin(VECTOR & v) {
            return v.begin();
        }

        /// iterator pointing behind the last element
        static
        iterator end(VECTOR & v) {
            return v.end();
        }
    };

    /**  \brief Traits class to extract type information from a VECTOR.
     *
     */
    template < class VECTOR >
    struct vector_traits
        : vector_view_traits <VECTOR>, mutable_vector_traits <VECTOR> {
    };

    // Note: specializations for T[N] and T[M][N] have been moved to traits/c_array.hpp

}}}

#endif


/**
 * This file is part of YaPLY (Yet Another PLY library).
 *
 * Copyright (C) 2016 Alex Locher <alocher at ethz dot ch> (ETH Zuerich)
 * For more information see <https://github.com/alexlocher/yaply>
 *
 */

#include <yaply/YaPLY.hpp>
#include <Eigen/Dense>

int main() {

	{
		// generate 1000 random 3D points and RGB colors
		Eigen::Matrix<double, Eigen::Dynamic, 3> randPoints(1000, 3);
		randPoints << Eigen::Matrix<double, Eigen::Dynamic, 3>::Random(1000, 3);
		Eigen::Matrix<unsigned char, Eigen::Dynamic, 3> randColors(1000, 3);
		randColors << Eigen::Matrix<unsigned char, Eigen::Dynamic, 3>::Random(1000, 3);

		// generate a file with random 1000 3D points with colors
		yaply::PlyFile plyFile;
		plyFile["vertex"].nrElements = randPoints.rows();
		plyFile["vertex"].setScalars("x,y,z", randPoints.data());
		plyFile["vertex"].setScalars("red,green,blue", randColors.data());

		// save it
		plyFile.save("random.ply", true);
		std::cout << "created PLY file with random points and colors and saved it to >random.ply<"
				<< std::endl;
	}

	// Load a file and save it and save XYZ as ASCII
	{
		std::string inFile("random.ply");
		yaply::PlyFile plyFile(inFile.c_str());

		std::cout << "loaded file " << inFile << ":" << std::endl;
		for (const auto& el : plyFile.elements_) {
			std::cout << "element " << el.name << " " << el.nrElements << std::endl;
			for (const auto& p : el.properties)
				std::cout << "property " << p->name << std::endl;
		}

		auto& vertices = plyFile["vertex"];
		if (vertices.nrElements <= 0) {
			std::cerr << "could not find vertex attribute in the plyfile" << std::endl;
			return EXIT_FAILURE;
		}

		std::vector<double> x, y, z;
		if (vertices.getScalarProperty("x", x) && vertices.getScalarProperty("y", y)
				&& vertices.getScalarProperty("z", z)) {
			yaply::PlyFile outFile;
			outFile["vertex"].nrElements = x.size();
			outFile["vertex"].setScalars("x", x.data());
			outFile["vertex"].setScalars("y", y.data());
			outFile["vertex"].setScalars("z", z.data());

			outFile.save("random_ascii.ply", false);
		}
	}
}


/***
 * This is an example of using turtlesim with publisher-subscriber.
 * It needs the address of a .txt file with the recipe as parameter in order to do his job.
 */

#include <boost/bind.hpp>
#include <ros/ros.h>
#include <turtlesim/Pose.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Empty.h>
#include <fstream>
#include <iostream>
#include <string>
#include <sstream>
#include <vector>
#include <stdlib.h>

#define PI 3.141592

std::vector<double> distance;
std::vector<double> angle;

turtlesim::PoseConstPtr g_pose;
turtlesim::Pose g_goal;

unsigned int cantGiros=0, i=0, a=0, d=0;

// lista de los estados posibles
enum State
{
  FORWARD,
  STOP_FORWARD,
  TURN,
  STOP_TURN,
};

// crea dos estados globales: estado actual y estado anterior
State g_state = FORWARD;
State g_last_state = FORWARD;
bool g_first_goal_set = false;

// almacena en g_pose el valor actual de pose
void poseCallback(const turtlesim::PoseConstPtr& pose)
{
  g_pose = pose;
}

// comprueba si la tortuga ha llegado a la meta (BOOL)

bool hasReachedGoal()
{
  return fabsf(g_pose->x - g_goal.x) < 0.03 && fabsf(g_pose->y - g_goal.y) < 0.03 && fabsf(g_pose->theta - g_goal.theta) < 0.003;
}

// comprueba si la tortuga se ha detenido (BOOL)
bool hasStopped()
{
  return g_pose->angular_velocity < 0.00005 && g_pose->linear_velocity < 0.00005;
}

// escribe en pantalla los valores de meta x,y,theta
void printGoal()
{
  ROS_INFO("New goal [%f %f, %f]", g_goal.x, g_goal.y, g_goal.theta);
}

// graba los valores de linear y angular en el publisher
void commandTurtle(ros::Publisher twist_pub, float linear, float angular)
{
  geometry_msgs::Twist twist;
  twist.linear.x = linear;
  twist.angular.z = angular;
  twist_pub.publish(twist);
}

// detiene el movimiento forward
void stopForward(ros::Publisher twist_pub)
{
	double temp;
  if (hasStopped()) // si la tortuga se ha detenido:
  {
    ROS_INFO("Reached goal");
    g_state = TURN;
    g_goal.x = g_pose->x;	// mantiene los valores de x e y
    g_goal.y = g_pose->y;
		if((g_pose->theta + angle[a]) >= 2*PI)  {
    	g_goal.theta =g_pose->theta + angle[a] - 2*PI;		// mantiene el valor de theta menor a 2*PI
		}
		else g_goal.theta =g_pose->theta + angle[a];
		a++;
    printGoal();
  }
  else	// sino, da la orden de detenerla
  {
    commandTurtle(twist_pub, 0, 0);
  }
}

// detiene el movimiento de giro
void stopTurn(ros::Publisher twist_pub)
{
  if (hasStopped()) // si el giro se ha detenido:
  {
    ROS_INFO("Reached goal");

    g_state = FORWARD;
    g_goal.x = distance[d]* cos(g_pose->theta) + g_pose->x;
    g_goal.y = distance[d]* sin(g_pose->theta) + g_pose->y;
    g_goal.theta = g_pose->theta;
		d++;
    printGoal();
  }
  else	// sino, da la orden de detener el giro:
  {
    commandTurtle(twist_pub, 0, 0);
  }
}

// orden de movimiento forward
void forward(ros::Publisher twist_pub)
{
  if (hasReachedGoal())	// si ha llegado a la meta, da la orden de detener el movimiento:
  {
    g_state = STOP_FORWARD;
    commandTurtle(twist_pub, 0, 0);
  }
  else	// sino, da la orden de continuar el movimiento:
  {
    commandTurtle(twist_pub, 1.0, 0.0);
  }
}

// orden de movimiento angular
void turn(ros::Publisher twist_pub)
{
  if (hasReachedGoal())	// si se ha llegado a la meta, se detiene el giro:
  {
    g_state = STOP_TURN;
    commandTurtle(twist_pub, 0, 0);
    i++;
    if(i==cantGiros) {
	ROS_INFO("DONE");
	 ros::shutdown();
    }
  }
  else	// sino, se envia la orden de continuar girando:
  {
    commandTurtle(twist_pub, 0.0, 0.4);
  }
}

// funcion donde se ejecuta la logica, se llama cada vez que se cumple el tiempo configurado en timer
void timerCallback(const ros::TimerEvent&, ros::Publisher twist_pub)
{
  if (!g_pose)	// si g_pose==0, termina la funcion
  {
    return;
  }

  if (!g_first_goal_set) // si no hubiese una meta, ejecuta:
  {
    g_first_goal_set = true;
    g_state = FORWARD;
    g_goal.x = distance[d]*cos(g_pose->theta) + g_pose->x;
    g_goal.y = distance[d]*sin(g_pose->theta) + g_pose->y;
    g_goal.theta = g_pose->theta;
		d++;
    printGoal();
  }

  if (g_state == FORWARD)	// si el estado es FORWARD, ejecuta:
  {
    forward(twist_pub);	// envia la orden de movimiento a la funcion forward
  }
  else if (g_state == STOP_FORWARD)	// si el estado es STOP_FORWARD, ejecuta:
  {
    stopForward(twist_pub); // envia la orden de detenerse a la funcion stop_forward
  }
  else if (g_state == TURN) // si el estado es TURN, ejecuta:
  {
    turn(twist_pub); // envia la orden de giro a la funcion turn
  }
  else if (g_state == STOP_TURN) // si el estado es STOP_TURN, ejecuta:
  {
    stopTurn(twist_pub); // envia la orden de detener giro a la funcion stop_turn
  }
}

// funcion principal
int main(int argc, char** argv)
{
  ros::init(argc, argv, "draw_square");
  ros::NodeHandle nh;
  ros::Subscriber pose_sub = nh.subscribe("turtle1/pose", 1, poseCallback); // se suscribe a turtle1/pose
  ros::Publisher twist_pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1); // publica en turtle1/pose
	std::string nombre;
  std::string line;
  std::string word;
  std::string::size_type sz;
  double num=0.0;
  unsigned int numLin=0;
  if ( argc > 2 )
    std::cout<<"usage: "<< argv[0] <<" <filename>\n";
  else {
    if(argc == 2)
        nombre=argv[1];
    else
      nombre="example.txt";
      std::ifstream myfile( nombre.c_str());
      if (myfile)
        {
        while (getline( myfile, line ))
        {
            std::istringstream iss(line);
            while(iss >> word)
            {
                num=atof(word.c_str());
                if(numLin==0) {
                    distance.push_back(num);
                }
                if(numLin==1) {
                    angle.push_back(num);
                }
            }
            numLin++;
        }
        myfile.close();
        }
        else std::cout << "fooey\n";
				cantGiros=distance.size() + 1;
        std::cout << "\ndistance: ";
        for(unsigned int j=0; j<distance.size();j++) {
            std::cout << distance[j] << " ";
        }
        std::cout << "\nangle: ";
        for(unsigned int j=0; j<angle.size();j++) {
            std::cout << angle[j] << " ";
        }
        std::cout << "\n";
    }
  ros::Timer timer = nh.createTimer(ros::Duration(0.002), boost::bind(timerCallback, _1, twist_pub));
  ros::spin();
}


// Copyright (C) 2011 by the BEM++ Authors
//
// 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 "bempp/common/config_ahmed.hpp"

#ifdef WITH_AHMED

#include "../type_template.hpp"
#include "../check_arrays_are_close.hpp"

#include "assembly/context.hpp"
#include "assembly/discrete_boundary_operator.hpp"
#include "assembly/laplace_3d_adjoint_double_layer_boundary_operator.hpp"
#include "assembly/laplace_3d_double_layer_boundary_operator.hpp"
#include "assembly/laplace_3d_hypersingular_boundary_operator.hpp"
#include "assembly/laplace_3d_single_layer_boundary_operator.hpp"
#include "assembly/helmholtz_3d_hypersingular_boundary_operator.hpp"
#include "assembly/numerical_quadrature_strategy.hpp"
#include "grid/grid_factory.hpp"
#include "space/piecewise_constant_scalar_space.hpp"
#include "space/piecewise_linear_continuous_scalar_space.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/type_traits/is_complex.hpp>
#include "grid/grid.hpp"

using namespace Bempp;

// Tests

BOOST_AUTO_TEST_SUITE(AcaAssembly)

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_assembled_operator_agrees_with_dense_assembly_for_614_element_mesh,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../examples/meshes/sphere-h-0.2.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(1);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            laplace3dDoubleLayerBoundaryOperator<BFT, RT>(
                contextDense, pwiseLinears, pwiseLinears, pwiseConstants);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            laplace3dDoubleLayerBoundaryOperator<BFT, RT>(
                contextAca, pwiseLinears, pwiseLinears, pwiseConstants);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_disassembled_single_layer_potential_operator_agrees_with_dense_assembly_for_614_element_mesh,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../examples/meshes/sphere-h-0.2.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            laplace3dSingleLayerBoundaryOperator<BFT, RT>(
                contextDense, pwiseLinears, pwiseConstants, pwiseLinears);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.globalAssemblyBeforeCompression = false;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            laplace3dSingleLayerBoundaryOperator<BFT, RT>(
                contextAca, pwiseLinears, pwiseConstants, pwiseLinears);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_disassembled_double_potential_operator_agrees_with_dense_assembly_for_614_element_mesh,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../examples/meshes/sphere-h-0.2.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(1);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            laplace3dDoubleLayerBoundaryOperator<BFT, RT>(
                contextDense, pwiseLinears, pwiseLinears, pwiseConstants);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.globalAssemblyBeforeCompression = false;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            laplace3dDoubleLayerBoundaryOperator<BFT, RT>(
                contextAca, pwiseLinears, pwiseLinears, pwiseConstants);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_disassembled_adjoint_double_potential_operator_agrees_with_dense_assembly_for_614_element_mesh,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../examples/meshes/sphere-h-0.2.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(1);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            laplace3dAdjointDoubleLayerBoundaryOperator<BFT, RT>(
                contextDense, pwiseConstants, pwiseConstants, pwiseLinears);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.globalAssemblyBeforeCompression = false;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            laplace3dAdjointDoubleLayerBoundaryOperator<BFT, RT>(
                contextAca, pwiseConstants, pwiseConstants, pwiseLinears);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_disassembled_hypersingular_operator_agrees_with_dense_assembly_for_614_element_mesh,
                              ValueType, result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../examples/meshes/sphere-h-0.2.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    BoundaryOperator<BFT, RT> opDense =
            laplace3dHypersingularBoundaryOperator<BFT, RT>(
                contextDense, pwiseLinears, pwiseConstants, pwiseLinears);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.globalAssemblyBeforeCompression = false;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            laplace3dHypersingularBoundaryOperator<BFT, RT>(
                contextAca, pwiseLinears, pwiseConstants, pwiseLinears);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(aca_of_disassembled_helmholtz_hypersingular_operator_agrees_with_dense_assembly_for_614_element_mesh,
                              ValueType, complex_result_types)
{
    typedef ValueType RT;
    typedef typename ScalarTraits<ValueType>::RealType RealType;
    typedef RealType BFT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../examples/meshes/sphere-h-0.2.msh", false /* verbose */);

    shared_ptr<Space<BFT> > pwiseConstants(
        new PiecewiseConstantScalarSpace<BFT>(grid));
    shared_ptr<Space<BFT> > pwiseLinears(
        new PiecewiseLinearContinuousScalarSpace<BFT>(grid));

    AccuracyOptions accuracyOptions;
    accuracyOptions.doubleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
                new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));

    AssemblyOptions assemblyOptionsDense;
    assemblyOptionsDense.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > contextDense(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsDense));

    const RT waveNumber = 3.;
    BoundaryOperator<BFT, RT> opDense =
            helmholtz3dHypersingularBoundaryOperator<BFT>(
                contextDense, pwiseLinears, pwiseConstants, pwiseLinears,
                waveNumber);
    arma::Mat<RT> weakFormDense = opDense.weakForm()->asMatrix();

    AssemblyOptions assemblyOptionsAca;
    assemblyOptionsAca.setVerbosityLevel(VerbosityLevel::LOW);
    AcaOptions acaOptions;
    acaOptions.globalAssemblyBeforeCompression = false;
    assemblyOptionsAca.switchToAcaMode(acaOptions);
    shared_ptr<Context<BFT, RT> > contextAca(
        new Context<BFT, RT>(quadStrategy, assemblyOptionsAca));

    BoundaryOperator<BFT, RT> opAca =
            helmholtz3dHypersingularBoundaryOperator<BFT>(
                contextAca, pwiseLinears, pwiseConstants, pwiseLinears,
                waveNumber);
    arma::Mat<RT> weakFormAca = opAca.weakForm()->asMatrix();

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    weakFormDense, weakFormAca, 2. * acaOptions.eps));
}

BOOST_AUTO_TEST_SUITE_END()

#endif // WITH_AHMED


//=======================================================================
// Copyright (c)
//
// 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 "test_utils/sample_graph.hpp"

#include "paal/data_structures/facility_location/facility_location_solution.hpp"
#include "paal/local_search/facility_location/facility_location_solution_adapter.hpp"
#include "paal/local_search/facility_location/facility_location_add.hpp"
#include "paal/local_search/facility_location/facility_location_remove.hpp"
#include "paal/local_search/facility_location/facility_location_swap.hpp"

#include <boost/test/unit_test.hpp>

using namespace paal::data_structures;
using namespace paal::local_search;

BOOST_AUTO_TEST_CASE(FacilityLocationRemoveTest) {
    typedef sample_graphs_metrics SGM;
    auto gm = SGM::get_graph_metric_small();
    std::vector<int> fcosts{ 7, 8 };
    auto cost = [&](int i) { return fcosts[i]; };

    typedef voronoi<decltype(gm)> VorType;
    typedef typename VorType::GeneratorsSet FSet;
    VorType voronoi(FSet{ SGM::A },
                    FSet{ SGM::A, SGM::B, SGM::C, SGM::D, SGM::E }, gm);
    typedef facility_location_solution<decltype(cost), VorType> Sol;
    Sol sol(std::move(voronoi), FSet{ SGM::B }, cost);
    facility_location_solution_adapter<Sol> sa(sol);

    {
        facility_locationget_moves_remove ng;
        auto r = ng(sa);
        auto b = std::begin(r);
        auto e = std::end(r);
        BOOST_CHECK_EQUAL(std::distance(b, e), 1);
    }
    {
        facility_locationget_moves_add ng;
        auto r = ng(sa);
        auto b = std::begin(r);
        auto e = std::end(r);
        BOOST_CHECK_EQUAL(std::distance(b, e), 1);
    }
    {
        facility_locationget_moves_swap ng;
        auto r = ng(sa);
        auto b = std::begin(r);
        auto e = std::end(r);
        BOOST_CHECK_EQUAL(std::distance(b, e), 1);
    }
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2022 Matt Borland. 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_MP_DETAIL_FPCLASSIFY_HPP
#define BOOST_MP_DETAIL_FPCLASSIFY_HPP

#include <cmath>
#include <limits>
#include <type_traits>
#include <boost/multiprecision/detail/standalone_config.hpp>
#include <boost/multiprecision/detail/float128_functions.hpp>

#ifdef BOOST_MP_MATH_AVAILABLE
#include <boost/math/special_functions/fpclassify.hpp>

#define BOOST_MP_ISNAN(x) (boost::math::isnan)(x)
#define BOOST_MP_ISINF(x) (boost::math::isinf)(x)
#define BOOST_MP_FPCLASSIFY(x) (boost::math::fpclassify)(x)
#define BOOST_MP_ISFINITE(x) (!(boost::math::isnan)(x) && !(boost::math::isinf)(x))

#else

namespace boost { namespace multiprecision { namespace detail {

template <typename T, typename std::enable_if<std::is_floating_point<T>::value
                      #ifdef BOOST_HAS_FLOAT128
                      || std::is_same<T, float128_type>::value
                      #endif
                      , bool>::type = true>
inline bool isnan BOOST_PREVENT_MACRO_SUBSTITUTION (const T x)
{
    BOOST_MP_FLOAT128_USING;
    using std::isnan;
    return static_cast<bool>((isnan)(x));
}

template <typename T, typename std::enable_if<!std::is_floating_point<T>::value
                      #ifdef BOOST_HAS_FLOAT128
                      && !std::is_same<T, float128_type>::value
                      #endif
                      , bool>::type = true>
inline bool isnan BOOST_PREVENT_MACRO_SUBSTITUTION (const T x)
{
    return x != x;
}

template <typename T, typename std::enable_if<std::is_floating_point<T>::value
                      #ifdef BOOST_HAS_FLOAT128
                      || std::is_same<T, float128_type>::value
                      #endif
                      , bool>::type = true>
inline bool isinf BOOST_PREVENT_MACRO_SUBSTITUTION (const T x)
{
    BOOST_MP_FLOAT128_USING;
    using std::isinf;
    return static_cast<bool>((isinf)(x));
}

template <typename T, typename std::enable_if<!std::is_floating_point<T>::value
                      #ifdef BOOST_HAS_FLOAT128
                      && !std::is_same<T, float128_type>::value
                      #endif
                      , bool>::type = true>
inline bool isinf BOOST_PREVENT_MACRO_SUBSTITUTION (const T x)
{
    return x == std::numeric_limits<T>::infinity() || x == -std::numeric_limits<T>::infinity();
}

template <typename T, typename std::enable_if<std::is_floating_point<T>::value, bool>::type = true>
inline int fpclassify BOOST_PREVENT_MACRO_SUBSTITUTION (const T x)
{
    using std::fpclassify;
    return fpclassify(x);
}

template <typename T, typename std::enable_if<!std::is_floating_point<T>::value, bool>::type = true>
inline int fpclassify BOOST_PREVENT_MACRO_SUBSTITUTION (const T x)
{
    BOOST_MP_FLOAT128_USING;
    using std::isnan;
    using std::isinf;
    using std::abs;

    return (isnan)(x) ? FP_NAN :
           (isinf)(x) ? FP_INFINITE :
           abs(x) == T(0) ? FP_ZERO :
           abs(x) > 0 && abs(x) < (std::numeric_limits<T>::min)() ? FP_SUBNORMAL : FP_NORMAL;
}

}}} // Namespace boost::multiprecision::detail

#define BOOST_MP_ISNAN(x) (boost::multiprecision::detail::isnan)(x)
#define BOOST_MP_ISINF(x) (boost::multiprecision::detail::isinf)(x)
#define BOOST_MP_FPCLASSIFY(x) (boost::multiprecision::detail::fpclassify)(x)
#define BOOST_MP_ISFINITE(x) (!(boost::multiprecision::detail::isnan)(x) && !(boost::multiprecision::detail::isinf)(x))

#endif

#endif // BOOST_MP_DETAIL_FPCLASSIFY_HPP


#include <iostream>
#include <pcl/io/io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_parallel_line.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <boost/thread/thread.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>

int main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>),
  								                       cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>),
  										                   cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);

  // Fill in the cloud data
  pcl::PCDReader reader;
  reader.read ("rgb_edges_scene.pcd", *cloud);

  pcl::PCDWriter writer;

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());

  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PARALLEL_LINE);
  seg.setMethodType (pcl::SAC_RANSAC);
  Eigen::Vector3f parralel_to_x;
  parralel_to_x(0) = 1;
  parralel_to_x(1) = 0;
  parralel_to_x(2) = 0;
  seg.setAxis(parralel_to_x);
  seg.setEpsAngle(0.5);
  seg.setMaxIterations (1000);
  seg.setDistanceThreshold (0.01);

  // Create the filtering object
  pcl::ExtractIndices<pcl::PointXYZRGB> extract;

  int i = 0, nr_points = (int) cloud->points.size ();
  // While 60% of the original cloud is still there
  while (cloud->points.size () > 0.6 * nr_points)
  {
    // Segment the largest line from the remaining cloud
    seg.setInputCloud (cloud);
    seg.segment (*inliers, *coefficients);

    if (inliers->indices.size () == 0)
    {
      std::cerr << "Could not estimate a line model for the given dataset." << std::endl;
      break;
    }

    // Extract the inliers
    extract.setInputCloud (cloud);
    extract.setIndices (inliers);
    extract.setNegative (false);
    extract.filter (*cloud_p);
    std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;

    std::stringstream ss;
    ss << "test_pcd_line_" << i << ".pcd";
    writer.write<pcl::PointXYZRGB> (ss.str (), *cloud_p, false);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> singleColor (cloud_p, 255 - (100*i)%256, (100*i)%256, (100*i)%256);

    // Create the filtering object
    extract.setNegative (true);
    extract.filter (*cloud_f);
    cloud.swap (cloud_f);
    i++;
  }

  return (0);
}


// https://qiita.com/lucidfrontier45/items/5048ef74fbf32eeb9f08

#include <iostream>
#include <pybind11/eigen.h>
#include <Eigen/Dense>
namespace py = pybind11;

template <typename T>
using RMatrix = Eigen::Matrix<T, -1, -1, Eigen::RowMajor>;

template <typename T>
void print_array(RMatrix<T> m){
    py::print(m);
}
template <typename T>
RMatrix<T> modify_array(RMatrix<T> m, T a){
    return m * a;
}
template <typename T>
void modify_array_inplace(Eigen::Ref<RMatrix<T>> m, T a){ //中身参照
    m = m * a;
}
PYBIND11_MODULE(QT_TARGET, m){
    m.def("print_array", &print_array<double>, "");
    m.def("modify_array", &modify_array<double>, "");
    m.def("modify_array_inplace", &modify_array_inplace<double>, "");
}


/******************************************************************************
* Author:   Steffen Urban													 *
* Contact:  urbste@gmail.com                                                 *
* License:  Copyright (c) 2016 Steffen Urban, ANU. All rights reserved.      *
*                                                                            *
* Redistribution and use in source and binary forms, with or without         *
* modification, are permitted provided that the following conditions         *
* are met:                                                                   *
* * Redistributions of source code must retain the above copyright           *
*   notice, this list of conditions and the following disclaimer.            *
* * Redistributions in binary form must reproduce the above copyright        *
*   notice, this list of conditions and the following disclaimer in the      *
*   documentation and/or other materials provided with the distribution.     *
* * Neither the name of ANU nor the names of its contributors may be         *
*   used to endorse or promote products derived from this software without   *
*   specific prior written permission.                                       *
*                                                                            *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE  *
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
* ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE        *
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT         *
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY  *
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF     *
* SUCH DAMAGE.                                                               *
******************************************************************************/

#ifndef OPENGV_ABSOLUTE_POSE_MODULES_MLPNP_HPP
#define OPENGV_ABSOLUTE_POSE_MODULES_MLPNP_HPP

#include <stdlib.h>
#include <Eigen/Eigen>
#include <Eigen/Sparse>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <opengv/types.hpp>

namespace opengv
{
namespace absolute_pose
{
namespace modules
{
namespace mlpnp
{
	void mlpnpJacs(
		const point_t& pt,
		const Eigen::Vector3d& nullspace_r,
		const Eigen::Vector3d& nullspace_s,
		const rodrigues_t& c,
		const translation_t& t,
		Eigen::MatrixXd& jacs);

	void do_scale(const point_t& pt,
	const rotation_t& rot,
	const translation_t& t,
	double& v1,
	Eigen::Vector2d& scales);

	void mlpnp_lm(Eigen::VectorXd& x,
		const points_t& pts,
		const std::vector<Eigen::MatrixXd>& nullspaces,
		const Eigen::SparseMatrix<double> Kll,
		bool use_cov);

	void mlpnp_gn(Eigen::VectorXd& x,
			const points_t& pts,
			const std::vector<Eigen::MatrixXd>& nullspaces,
			const Eigen::SparseMatrix<double> Kll,
			bool use_cov);

	void mlpnp_gn(Eigen::VectorXd& x,
		const points_t& pts,
		const std::vector<Eigen::MatrixXd>& nullspaces,
		const Eigen::SparseMatrix<double> Kll,
		Eigen::MatrixXd& Qldld,
		Eigen::MatrixXd& Qxx,
		bool use_cov);

	void mlpnp_residuals_and_jacs(
		const Eigen::VectorXd& x,
		const points_t& pts,
		const std::vector<Eigen::MatrixXd>& nullspaces,
		Eigen::VectorXd& r,
		Eigen::MatrixXd& fjac,
		bool getJacs);
}
}
}
}

#endif


// General includes
#include <vector>
#include <cstdlib>
#include <ctime>
#include <boost/thread.hpp>
#include <cmath>

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

// ROS includes
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseArray.h>
#include <tf/transform_datatypes.h>

// Map class
class PathPlanner {

  // ROS handle, publishers and subscribers
  ros::NodeHandle _nh, _priv_nh;
  ros::Subscriber _odm_sub;
  ros::Publisher _goal_pub, _next_pub, _traj_pub;

  // Map and planner varibles
  cv::Mat _img_map;                         // Original map image
  cv::Mat _eroded_map;                      // Eroded map image
  std::vector<std::vector<int> > _occ_map;  // Occupancy map
  std::vector<cv::Point3i> _traj;           // Full trajectory
  std::vector<cv::Point> _waypoints;        // List of waypoints

  // Robot and goal positions
  cv::Point _goal;
  cv::Point _robot;
  float _alpha;        // Robot rotation
  int _distance;       // Total distance between goal and robot
  double _current_x, _current_y, _last_x, _last_y;

  // Occupancy states
  typedef enum {
    OCC_GOAL     =  0,
    OCC_DRIVABLE = -1,
    OCC_OBSTACLE = -2
  } OccupanceStates;

  /**
   * Private map and planner function
   */

  // Private thread variables
  boost::thread m_Thread;
  bool _searching;

  // Thread for running the planner
  void runPlanner() {
    this->OccupancyMap();
    if( this->goalReachable() ) {
      this->Search();
      if( this->robotReachable() ) {
	      this->Trajectory();
      }
      else {
	      ROS_WARN("[PathPlanner]: Could not get a trajectory between goal and robot position (robot is either inside a wall or too close to a wall).");
      }
    }
    else {
      ROS_WARN("[PathPlanner]: Could not reach the goal position (position is either inside a wall or too close to a wall).");
    }
    this->_searching = false;
  }

  // Thread-starting-function
  void Plan() {
    if( !this->_searching ) {
      this->_searching = true;
      m_Thread = boost::thread( &PathPlanner::runPlanner, this);
    }
  }

  // Planner search function
  bool Search() {
    int i, j, dist;
    std::vector<cv::Point3i> queue;

    // Wavefront search from goal to robot
    cv::Point3i p;
    queue.push_back(cv::Point3i( this->_goal.y, this->_goal.x, OCC_GOAL));
    while( !queue.empty() ) {

      // Get next element from the list
      p = queue.front();
      queue.erase( queue.begin() );

      // Have the planner reached the robot?
      if( p.x == this->_robot.y &&
	    p.y == this->_robot.x ) {
        this->_distance = p.z;
        ROS_INFO("Distance to goal: %d", this->_distance);

        // Clear the list and clean upp the map
        while( !queue.empty() ) {
          p = queue.front();
          queue.erase( queue.begin() );
          this->_occ_map[p.x][p.y] = OCC_DRIVABLE;
        }

        return true;
      }
      else {
        for( int k = 0; k < 4; k++ ) {
          i = p.x;
          j = p.y;
          dist = p.z + 1;
          if( this->getGridValue( k, i, j) == OCC_DRIVABLE ) { // ...changes i, j -index as well.
            this->_occ_map[i][j] = dist;
            queue.push_back(cv::Point3i( i, j, dist));
          }
        }
      }
    }
    return false;
  }

  // Calculate the closest trajectory
  void Trajectory() {
    this->_traj.clear();  // ...clear all previous waypoints

    // Search for a new trajectory
    int i = this->_robot.y, j = this->_robot.x;
    int dist = this->getGridValue( -1, i, j);
    this->_traj.push_back(cv::Point3i( i, j, dist));
    while( this->_traj.back().z != OCC_GOAL ) {

      // Find closest point in neghbourhood
      int _i, _j, _dist = this->_traj.back().z;
      for( int k = 0; k < 4; k++) {
        i = this->_traj.back().x;
        j = this->_traj.back().y;
        dist = this->getGridValue( k, i, j);
        if( dist >= 0 && dist <= _dist ) {

	        // Add some randomness to avoid straight lines
          if( dist == _dist ) {
            if( (int)(rand() % 2) == 0 ) {
              _i = i;
              _j = j;
            }
          }
          else {
            _i = i;
            _j = j;
          }
          _dist = dist;
        }
      }
      this->_traj.push_back(cv::Point3i( _i, _j, _dist));
    }

    // Set waypoints along the trajectory
    int counter = 0;
    this->_waypoints.clear();
    std::vector<cv::Point3i>::iterator ite;
    for( ite = this->_traj.begin(); ite != this->_traj.end(); ++ite) {
      if( counter % 10 == 0 ) {
	      this->_waypoints.push_back(cv::Point(ite->y, ite->x));
      }
      counter++;
    }

    // Remove current position and adding goal position
    this->_waypoints.erase(this->_waypoints.begin());
    this->_waypoints.push_back(this->_goal);

    // Publish trajectory
    this->publishTrajectory();
  }

  // Calculate the occupancy map
  void OccupancyMap() {
    for( int i = 0; i < this->_eroded_map.rows; i++) {
      for( int j = 0; j < this->_eroded_map.cols; j++) {
        if( this->_eroded_map.at<uchar>( i, j) < 128 ) {
          this->_occ_map[i][j] = OCC_OBSTACLE;
        }
        else {
          this->_occ_map[i][j] = OCC_DRIVABLE;
        }
      }
    }
    if( this->_goal.x >= 0 && this->_goal.y >= 0 ) {
      this->_occ_map[this->_goal.y][this->_goal.x] = OCC_GOAL;
    }
  }

  /**
   * Helper functions
   */
  int getGridValue( int idx, int &i, int &j) {
    if( idx == 0 ) i--;
    else if( idx == 1 ) i++;
    else if( idx == 2 ) j--;
    else if( idx == 3 ) j++;
    return this->_occ_map[i][j];
  }

  bool goalReachable() {
    int x = this->_goal.x, y = this->_goal.y;
    if( (x >= 0 && x <= this->_eroded_map.cols) &&
	  (y >= 0 && y <= this->_eroded_map.rows) ) {
      if(this->_eroded_map.at<uchar>(y, x) > 128) {
	      return true;
      }
    }
    return false;
  }

  bool robotReachable() {
    int x = this->_robot.x, y = this->_robot.y;
    if( (x >= 0 && x <= this->_eroded_map.cols) &&
	  (y >= 0 && y <= this->_eroded_map.rows) ) {
      if(this->_eroded_map.at<uchar>(y, x) > 128) {
	      return true;
      }
    }
    return false;
  }

  // NEED TO READ MAP DIMENSION AND RESOLUTION AS PARAMETERS
  cv::Point convertMetricToPixel(double x, double y) {
    x = (x + 12.5) / 0.05;
    y = (12.5 - y) / 0.05;
    return cv::Point( (int)x, (int)y);
  }

  geometry_msgs::Pose convertPixelToMetric(cv::Point p) {
    geometry_msgs::Pose waypoint;
    waypoint.position.x = p.x * 0.05 - 12.5;
    waypoint.position.y = -(p.y * 0.05 - 12.5);
    return waypoint;
  }

  /**
   * Callback function for robot odometry
   */
  void odometryCallback(const nav_msgs::Odometry::ConstPtr& msg) {

    // Robot pose (odometry)
    geometry_msgs::Pose robot_pose = msg->pose.pose;
    this->_current_x = robot_pose.position.x;
    this->_current_y = robot_pose.position.y;
    this->_alpha = tf::getYaw(robot_pose.orientation);

    // Convert and set new robot position
    this->_robot = this->convertMetricToPixel( this->_current_x, this->_current_y);
  }

  void publishTrajectory(){
    geometry_msgs::PoseArray trajectory;
    geometry_msgs::Pose pose, next_pose;
    std::vector<cv::Point>::iterator iter = this->_waypoints.begin();
    pose = convertPixelToMetric(*iter);
    iter++;

    for( ; iter != this->_waypoints.end(); iter++) {
      next_pose = convertPixelToMetric(*iter);
      pose.orientation = getOrientation(pose.position, next_pose.position);
      trajectory.poses.push_back(pose);
      pose = next_pose;
    }

    ROS_INFO("Publishing trajectory");
    this->_traj_pub.publish(trajectory);
  }

  geometry_msgs::Quaternion getOrientation(geometry_msgs::Point pos1, geometry_msgs::Point pos2){
      double X = pos2.x - pos1.x;
      double Y = pos2.y - pos1.y;
      double angle = std::atan2(Y, X);
      geometry_msgs::Quaternion orientation;
      orientation.z = std::sin(angle/2.0);
      orientation.w = std::cos(angle/2.0);
      return orientation;
  }

//////////////////////////////////////////////////////////////
//
// Public functions
//
/////////////////////////////////////////////////////////////
public:

  // Constructor
  PathPlanner(cv::Mat &map) : _img_map(map), _searching(false), _priv_nh("~") {

    // Initiate default goal and robot position
    this->_robot = cv::Point( -1, -1);
    this->_goal = cv::Point( -1, -1);

    // ROS publishers and subscribers
    this->_odm_sub = this->_nh.subscribe( "/robot_0/base_pose_ground_truth",
					  10,
					  &PathPlanner::odometryCallback,
					  this);
    this->_next_pub = this->_nh.advertise<geometry_msgs::Pose>( "/planner/waypoints/next", 10);
    this->_goal_pub = this->_nh.advertise<geometry_msgs::Pose>( "/planner/waypoints/goal", 10);
    this->_traj_pub = this->_nh.advertise<geometry_msgs::PoseArray>( "/planner/trajectory", 10);

    // Inititat the occupancy map
    for( int i = 0; i < map.rows; i++) {
      this->_occ_map.push_back(std::vector<int>( map.cols, -1));
    }

    // Initialize random seed
    srand( time(NULL) );

    // Read ROS params
    float dist = -1.0;
    if( !_priv_nh.getParam("wall_distance", dist) ) {
      this->_img_map.copyTo(this->_eroded_map);
    }
    else {

      // Erode the map (to avoid a path that is too close to the walls)
      int sz = (int)(dist/0.05);
      cv::Mat kernel = cv::getStructuringElement( cv::MORPH_ELLIPSE,
						  cv::Size( 2*sz + 1, 2*sz+1),
						  cv::Point( sz, sz) );
      cv::erode( this->_img_map, this->_eroded_map, kernel);
    }
  }

  /**
   * Draw functions
   */
  cv::Mat drawPathPlanner() {
    cv::Mat img;
    cv::addWeighted( this->_img_map, 0.9, this->_eroded_map, 0.1, 0.0, img);
    cv::cvtColor( img, img, CV_GRAY2BGR);

    // Define colors
    cv::Vec3b orange = cv::Vec3b( 32, 84, 233);  // Orange
    cv::Vec3b white = cv::Vec3b( 255, 255, 255); // White
    cv::Scalar purple = cv::Scalar(80, 39, 94);  // Blue
    cv::Scalar gray = cv::Scalar::all(51);       // Dark gray

    // Draw wavefront
    if( !this->_searching ) {
      for( int i = 0; i < this->_img_map.rows; i++) {
        for( int j = 0; j < this->_img_map.cols; j++) {
          if( this->_occ_map[i][j] > OCC_DRIVABLE ) {
            float scalar = this->_occ_map[i][j] / (float)this->_distance;
            cv::Vec3b scaled_color = cv::Vec3b( orange[0] + (uchar)((white[0] - orange[0]) * scalar),
                  orange[1] + (uchar)((white[1] - orange[1]) * scalar),
                        orange[2] + (uchar)((white[2] - orange[2]) * scalar) );
            img.at<cv::Vec3b>(i,j) = scaled_color;
          }
        }
      }
    }

    // Rescale the image (just for display purposes)
    float scalar = 2.0;
    cv::resize( img, img, cv::Size(), scalar, scalar, CV_INTER_LINEAR);

    // Draw trajectory
    for( int i = 0; i < this->_waypoints.size(); i++) {
      cv::circle( img, this->_waypoints[i] * scalar, 4, purple, -1);
      cv::circle( img, this->_waypoints[i] * scalar, 12, purple, 4);

      // Draw the goal point...
      if( this->_waypoints[i].x == this->_goal.x &&
	    this->_waypoints[i].y == this->_goal.y ) {
        cv::circle( img, this->_goal * scalar, 24, purple, 4);
        cv::Point p = this->_goal * scalar;
        cv::line( img,
            cv::Point(p.x - 20, p.y - 20),
            cv::Point(p.x + 20, p.y + 20),
            purple, 4 );
        cv::line( img,
            cv::Point(p.x + 20, p.y - 20),
            cv::Point(p.x - 20, p.y + 20),
            purple, 4 );
      }
      else if( i == 0 ) {  // ..draw the next point
        cv::circle( img, this->_waypoints[i] * scalar, 24, purple, 4);
      }
    }

    // Draw the robot
    if( (this->_robot.x >= 0 &&  this->_robot.x < this->_img_map.cols) &&
	  (this->_robot.y >= 0 &&  this->_robot.y < this->_img_map.rows) ) {
      cv::circle( img, this->_robot * scalar, 16, gray, 4);
      cv::Point p( cos(this->_alpha) * 28, -sin(this->_alpha) * 28);
      cv::line( img, this->_robot * scalar, this->_robot * scalar + p, gray, 4);
    }

    // Rescale the image (just for display purposes)
    cv::resize( img, img, cv::Size(), 1.0 / scalar, 1.0 / scalar, CV_INTER_AREA);

    // Return resulting image
    return img;
  }

  /**
   * Reset function - clear the map and the goal
   */
  void reset() {
    this->_goal = cv::Point( -1, -1);
    this->OccupancyMap();
    this->_waypoints.clear();
  }

  /**
   * Replan the path every now and then
   */
  void replan() {
    double diff_x = (this->_current_x - this->_last_x);
    double diff_y = (this->_current_y - this->_last_y);
    double dist = sqrt( diff_x * diff_x + diff_y * diff_y );

    if( !this->_searching ) {
      if( dist >= 0.5 && this->_goal.x >= 0 && this->_goal.x >= 0 ) {
        this->_last_x = this->_current_x;
        this->_last_y = this->_current_y;
        this->Plan();
      }
    }

    // Publish waypoints
    if( !this->_waypoints.empty() ) {
      geometry_msgs::Pose next_point = this->convertPixelToMetric(this->_waypoints.front());
      geometry_msgs::Pose goal_point = this->convertPixelToMetric(this->_goal);
      this->_next_pub.publish(next_point);
      this->_goal_pub.publish(goal_point);
    }
  }

  /**
   * Callback and wrapper function for handle mouse click.
   * Used for setting the goal position.
   */
  static void clickHandler(int event, int x, int y, int flags, void *param) {
    PathPlanner *self = static_cast<PathPlanner*>(param);
    self->clickHandleWrapper( event, x, y, flags);
    self->replan();
  }

  void clickHandleWrapper(int event, int x, int y, int flags) {
    if  ( event == cv::EVENT_LBUTTONDOWN ) {
      if( this->_eroded_map.at<uchar>(y, x) < 128 ) {
	      ROS_WARN("[PathPlanner]: Could not set goal position (position is either inside a wall or too close to a wall).");
      }
      else {

        // Set new goal (and plan the path)
        this->_goal = cv::Point(x, y);
        this->Plan();
      }
    }
  }

  /**
   * Access for the ROS node handle status function
   */
  bool ok() { return _nh.ok(); }
};

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

  // Init the connection with the ROS system
  ros::init(argc, argv, "intro_path_planner_node");

  // Read image
  cv::Mat img;
  if( argc != 2 ) {
    ROS_ERROR("[PathPlanner]: No map image given.");
    return -1;
  }
  else {
    img = cv::imread(argv[1], CV_LOAD_IMAGE_GRAYSCALE);
    if( !img.data ) {
      ROS_ERROR("[PathPlanner]: Could not open map image.");
      return -1;
    }
  }

  // Create a class instance
  PathPlanner planner(img);

  // Initialize the GUI
  const char *window = "intro_path_planner";
  cv::namedWindow(window, CV_WINDOW_AUTOSIZE);

  // Set the callback function for mouse event
  cv::setMouseCallback( window, PathPlanner::clickHandler, &planner);

  // ROS main loop
  ros::Rate loop_rate(10);
  while (planner.ok()) {

    // Dsiplay frame on screen
    cv::imshow( window, planner.drawPathPlanner());

    // Check for key input
    char key = (char)cv::waitKey(30);
    if( key == 27 || !planner.ok() ) {
      break;
    }
    else if( key == 'r' || key == 'R' ) {
      ROS_INFO("[PathPlanner]: Goal and map cleared.");
      planner.reset();
    }
    else if( key == 'h' || key == 'H' ) {
      std::cout<<"Commands:"<<std::endl;
      std::cout<<"---------"<<std::endl;
      std::cout<<"Righ-click   Set new goal position."<<std::endl;
      std::cout<<"H, h         Help menu."<<std::endl;
      std::cout<<"R, r         Reset goal position."<<std::endl;
      std::cout<<"ESC          Exit."<<std::endl;
      std::cout<<"---------"<<std::endl;
    }

    // Replan every 0.5m of movement
    //planner.replan();

    // Check for incomming sensor messages
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/ext/std/tuple.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/pair.hpp>

#include <tuple>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_ASSERT(lift<List>('x') == list('x'));
    BOOST_HANA_CONSTEXPR_ASSERT(lift<StdTuple>('x') == std::make_tuple('x'));

    BOOST_HANA_CONSTEXPR_LAMBDA auto f = pair;
    BOOST_HANA_CONSTEXPR_LAMBDA auto g = flip(pair);
    BOOST_HANA_CONSTEXPR_ASSERT(
        ap(list(f, g), list(1, 2, 3), list('a', 'b'))
            ==
        list(
            f(1, 'a'), f(1, 'b'), f(2, 'a'), f(2, 'b'), f(3, 'a'), f(3, 'b'),
            g(1, 'a'), g(1, 'b'), g(2, 'a'), g(2, 'b'), g(3, 'a'), g(3, 'b')
        )
    );
    //! [main]
}


// Copyright 2017 Peter Dimov.
// Distributed under the Boost Software License, Version 1.0.
//
// Endian-dependent test

// lib1.hpp

#include <string>

// forward declaration of boost::hash2::hash_append

namespace boost
{
namespace hash2
{

template<class H, class T> void hash_append( H & h, T const & v );

} // namespace hash2
} // namespace boost

// lib1::X

namespace lib1
{

class X
{
private:

    std::string s1_;
    std::string s2_;

    template<class H> friend void do_hash_append( H& h, X const& x )
    {
        using boost::hash2::hash_append;

        hash_append( h, x.s1_ );
        hash_append( h, x.s2_ );
    }

public:

    X(): s1_( "s1" ), s2_( "s2" )
    {
    }
};

} // namespace lib1

// lib2.hpp

#include <boost/hash2/byte_type.hpp>
#include <boost/cstdint.hpp> // boost::uint32_t
#include <cstddef>

namespace lib2
{

class fnv1a
{
private:

    boost::uint32_t st_;

public:

    typedef boost::uint32_t result_type;
    typedef boost::uint32_t size_type;

    fnv1a(): st_( 0x811C9DC5ul )
    {
    }

    void update( boost::hash2::byte_type const * p, std::ptrdiff_t n )
    {
        boost::uint32_t h = st_;

        for( std::ptrdiff_t i = 0; i < n; ++i )
        {
            h ^= static_cast<boost::uint32_t>( p[i] );
            h *= 0x01000193ul;
        }

        st_ = h;
    }

    boost::uint32_t result()
    {
        boost::uint32_t r = st_;

        st_ = ( st_ ^ 0xFF ) * 0x01000193ul;

        return r;
    }
};

} // namespace lib2

// test

#include <boost/hash2/hash_append.hpp>
#include <boost/hash2/fnv1a.hpp>
#include <boost/core/lightweight_test.hpp>
#include <vector>
#include <list>

template<class H, class R> void test( R r )
{
    {
        H h;

        using boost::hash2::hash_append;

        hash_append( h, std::vector<lib1::X>( 3 ) );

        BOOST_TEST_EQ( h.result(), r );
    }

    {
        H h;

        using boost::hash2::hash_append;

        hash_append( h, std::list<lib1::X>( 3 ) );

        BOOST_TEST_EQ( h.result(), r );
    }
}

int main()
{
    test<boost::hash2::fnv1a_32>( 2425039999ul );
    test<lib2::fnv1a>( 2425039999ul );

    return boost::report_errors();
}