/**
 * Beck Pang, 20180510, the local path planner
 * Mellinger, Daniel, and Vijay Kumar. "Minimum snap trajectory generation and control for quadrotors."
 * Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011.
 */
#include <iostream>
#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include <geometry_msgs/Twist.h>

using namespace std;
using namespace Eigen;
ros::Publisher local_path_pub;
string pub_topic, path_topic;

// trajectory coefficients
unsigned int derivatives = 4;
unsigned int order = derivatives * 2;
double max_velocity = 10.0; // m/s

// stage I calculate the coefficients if new global path arrived
void path_callback(const nav_msgs::Path::ConstPtr &global_path) {
    unsigned long periods = global_path->poses.size() - 1;

    // T and T accumulative, time vector
    VectorXd T = VectorXd::Zero(periods, 1);
    VectorXd T_acc = VectorXd::Zero(periods + 1, 1);
    for (int i = 0; i < periods; ++i) {
        geometry_msgs::Pose pose_old = global_path->poses[i].pose;
        geometry_msgs::Pose pose_new = global_path->poses[i + 1].pose;
        double dx = pose_new.position.x - pose_new.position.x;
        double dy = pose_new.position.y - pose_new.position.y;
        double dz = pose_new.position.z - pose_new.position.z;
        T[i] = sqrt(pow(dx, 2) + pow(dy, 2) + pow(dz, 2)) / max_velocity;
        T_acc[i + 1] = T_acc[i] + T[i];
    }

    // A, derivative constraints matrix
    MatrixXd A = MatrixXd::Zero(periods * order, periods * order);
    for (int i = 0; i < periods; ++i) {
        double ii = i * order;
        A(ii, ii) = 1;
        A(ii + 1, ii + 1) = 1;
        A(ii + 2, ii + 2) = 1;
        A(ii + 3, ii + 3) = 1;
        for (int j = 0; j < order; ++j) { A(ii + 4, ii + j) = pow(T(i), j); }
        for (int j = 1; j < order; ++j) { A(ii + 5, ii + j) = pow(T(i), j - 1) * j; }
        for (int j = 2; j < order; ++j) { A(ii + 6, ii + j) = pow(T(i), j - 2) * j * (j - 1); }
        for (int j = 3; j < order; ++j) { A(ii + 7, ii + j) = pow(T(i), j - 3) * j * (j - 1) * (j - 2); }
    }

    // Q, cost function matrix
    MatrixXd Q = MatrixXd::Zero(periods * order, periods * order);
    for (int i = 0; i < periods; ++i) {
        double ii = i * order;
        for (int j = derivatives; j <= order; ++j) {
            for (int k = derivatives; k <= order; ++k) {
                Q(ii + j - 1, ii + k - 1) =
                        i * (i - 1) * (i - 2) * (i - 3) * j * (j - 1) * (j - 2) * (j - 3) / (i + j + 7) *
                        pow(T(k - 1), i + j - 7);
            }
        }
    }

    // C, selection matrix, remove free variables and
    // break down the variables to fixed and process variables
    MatrixXd C = MatrixXd::Zero(periods * order, (periods + 1) * derivatives);
    C(0, 0) = 1;
    for (int i = 0; i < periods - 2; ++i) {

    }

    // df, fixed derivative constraints
    // dp, process derivative constraints
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "local_planner_minimum_snap");
    ros::NodeHandle n("~");

//    ros::Duration(11).sleep();

    n.param("path_topic", path_topic, string("/global_path_hardcode"));
    n.param("pub_topic", pub_topic, string("cmd_vel"));
    ros::Subscriber sub = n.subscribe(path_topic, 10, path_callback);
    local_path_pub = n.advertise<geometry_msgs::Twist>(pub_topic, 100);

    // 400 Hz local path planner command
    while (ros::ok()) {
        // stage II excute the trajectory to the motor
    }
}


/*
 * eigen_numpy_tensor.cpp
 *
 *  Created on: Feb 5, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   Licensed under the Apache License, Version 2.0 (the "License");
 *   you may not use this file except in compliance with the License.
 *   You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *   See the License for the specific language governing permissions and
 *   limitations under the License.
 */

//standard library
#include <array>
#include <cstdlib>

//libraries
#include <boost/python.hpp>
//#include <Eigen/Eigen>
#include <unsupported/Eigen/CXX11/Tensor>
#include <Python.h>

#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION

#include <numpy/arrayobject.h>

//local
#include "eigen_numpy.hpp"
#include "numpy_conversions_shared.hpp"
#include "../math/typedefs.hpp"

// These macros were renamed in NumPy 1.7.1.
#if !defined(NPY_ARRAY_C_CONTIGUOUS) && defined(NPY_C_CONTIGUOUS)
#define NPY_ARRAY_C_CONTIGUOUS NPY_C_CONTIGUOUS
#endif

#if !defined(NPY_ARRAY_ALIGNED) && defined(NPY_ALIGNED)
#define NPY_ARRAY_ALIGNED NPY_ALIGNED
#endif

namespace bp = boost::python;

using namespace Eigen;

template<class TensorType>
struct EigenTensorToPython {
	static PyObject* convert(const TensorType& tensor) {

		const int num_dimensions = TensorType::NumDimensions;
		npy_intp* shape = static_cast<npy_intp*>(malloc(sizeof(npy_intp) * num_dimensions));
		//npy_int shape2[num_dimensions]; //will error, check
		for (int i_dimension = 0; i_dimension < num_dimensions; i_dimension++) {
			shape[i_dimension] = static_cast<npy_intp>(tensor.dimension(i_dimension));
		}

		PyArrayObject* python_array = (PyArrayObject*) PyArray_SimpleNew(
				num_dimensions, shape, NumpyEquivalentType<typename TensorType::Scalar>::type_code);

		copy_tensor(tensor.data(),
				(typename TensorType::Scalar*) PyArray_DATA(python_array),
				num_dimensions,
				shape,
				tensor.size(),
				static_cast<Eigen::StorageOptions>(TensorType::Layout) == Eigen::RowMajor);
		free(shape);
		return (PyObject*) python_array;

	}
};

template<>
struct EigenTensorToPython<math::Tensor3v3f> {
	static PyObject* convert(const math::Tensor3v3f& tensor) {

		const int num_dimensions = math::Tensor3v3f::NumDimensions;
		npy_intp* shape_orig = static_cast<npy_intp*>(malloc(sizeof(npy_intp) * num_dimensions));
		npy_intp* shape_dest = static_cast<npy_intp*>(malloc(sizeof(npy_intp) * (num_dimensions+1)));
		//npy_int shape2[num_dimensions]; //will error, check
		for (int i_dimension = 0; i_dimension < num_dimensions; i_dimension++) {
			shape_orig[i_dimension] = static_cast<npy_intp>(tensor.dimension(i_dimension));
			shape_dest[i_dimension] = static_cast<npy_intp>(tensor.dimension(i_dimension));
		}
		shape_dest[num_dimensions] = 3;

		PyArrayObject* python_array = (PyArrayObject*) PyArray_SimpleNew(
				num_dimensions+1, shape_dest, NumpyEquivalentType<typename math::Tensor3v3f::Scalar>::type_code);

		copy_tensor(tensor.data(),
				(typename math::Tensor3v3f::Scalar*) PyArray_DATA(python_array),
				num_dimensions,
				shape_orig,
				tensor.size(),
				static_cast<Eigen::StorageOptions>(math::Tensor3v3f::Layout) == Eigen::RowMajor);
		free(shape_orig);
		free(shape_dest);
		return (PyObject*) python_array;

	}
};

template<typename TensorType>
struct EigenTensorFromPython {
	typedef typename TensorType::Scalar T;
	EigenTensorFromPython() {
		bp::converter::registry::push_back(&convertible,
				&construct,
				bp::type_id<TensorType>());
	}

	static void* convertible(PyObject* obj_ptr) {
		PyArrayObject* array = reinterpret_cast<PyArrayObject*>(obj_ptr);
		if (!PyArray_Check(array)) {
			//LOG(ERROR) << "PyArray_Check failed";
			return 0;
		}

		int dimension_count = PyArray_NDIM(array);
		if (dimension_count <= 2) {
			//This should be an Eigen::Matrix, not Eigen::Tensor
			//LOG(ERROR) << "PyArray_Check failed";
			return 0;
		} else if (dimension_count == 3) {
			npy_intp* dimensions = PyArray_DIMS(array);
			if (dimensions[2] == 2) {
				//This should be an Eigen::Matrix<Vector2>, not Eigen::Tensor
				return 0;
			}
		} else if (dimension_count != TensorType::NumDimensions) {
			//LOG(ERROR) << "PyArray_Check failed";
			return 0;
		}
		if (PyArray_ObjectType(obj_ptr, 0) != NumpyEquivalentType<T>::type_code) {
			//LOG(ERROR) << "types not compatible";
			return 0;
		}
		int flags = PyArray_FLAGS(array);
		if (!(flags & NPY_ARRAY_C_CONTIGUOUS)) {
			//LOG(ERROR) << "Contiguous C array required";
			return 0;
		}
		if (!(flags & NPY_ARRAY_ALIGNED)) {
			//LOG(ERROR) << "Aligned array required";
			return 0;
		}
		return obj_ptr;
	}

	static void construct(PyObject* obj_ptr,
			bp::converter::rvalue_from_python_stage1_data* data) {

		using bp::extract;

		PyArrayObject* array = reinterpret_cast<PyArrayObject*>(obj_ptr);
		npy_intp* numpy_array_dimensions = PyArray_DIMS(array);

		T* raw_data = reinterpret_cast<T*>(PyArray_DATA(array));

		typedef TensorMap<Tensor<T, TensorType::NumDimensions, RowMajor>, Aligned> TensorMapType;
		typedef TensorLayoutSwapOp<Tensor<T, TensorType::NumDimensions, RowMajor>> TensorSwapLayoutType;

		void* storage = ((bp::converter::rvalue_from_python_storage<TensorType>*)
				(data))->storage.bytes;

		std::array<Index, TensorType::NumDimensions> tensor_dimensions;
		std::array<int, TensorType::NumDimensions> inverse_dimensions;
		for (size_t i_dim = 0, inv_dim = TensorType::NumDimensions - 1; i_dim < tensor_dimensions.size();
				i_dim++, inv_dim--) {
			tensor_dimensions[i_dim] = static_cast<Index>(numpy_array_dimensions[i_dim]);
			inverse_dimensions[i_dim] = inv_dim;
		}

		new (storage) TensorType;
		TensorType* etensor = (TensorType*) storage;

		// TODO: This is a (potentially) expensive copy operation. There should be a better way
		auto mapped_t = TensorMapType(raw_data, tensor_dimensions);
		*etensor = TensorSwapLayoutType(mapped_t).shuffle(inverse_dimensions);
		data->convertible = storage;
	}
};

#if PY_VERSION_HEX >= 0x03000000
void*
#else
void
#endif
setup_Eigen_tensor_converters() {

	static bool is_setup = false;
	if (is_setup)
		return NUMPY_IMPORT_ARRAY_RETVAL;
	is_setup = true;
	import_array();

	EigenTensorFromPython<Eigen::Tensor<float, 3>>();
	bp::to_python_converter<Eigen::Tensor<float, 3>, EigenTensorToPython<Eigen::Tensor<float, 3>>>();

	EigenTensorFromPython<Eigen::Tensor<float, 4>>();
	bp::to_python_converter<Eigen::Tensor<float, 4>, EigenTensorToPython<Eigen::Tensor<float, 4>>>();

	bp::to_python_converter<Eigen::Tensor<float, 3, RowMajor>, EigenTensorToPython<Eigen::Tensor<float, 3, RowMajor>>>();
	bp::to_python_converter<Eigen::Tensor<float, 4, RowMajor>, EigenTensorToPython<Eigen::Tensor<float, 4, RowMajor>>>();
	bp::to_python_converter<math::Tensor3v3f, EigenTensorToPython<math::Tensor3v3f> >();

#if PY_VERSION_HEX >= 0x03000000
	return 0;
#endif
}


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

#ifndef BOOST_MPL11_DETAIL_LOGICAL_OR_STRICT_SPECIALIZATION_HPP
#define BOOST_MPL11_DETAIL_LOGICAL_OR_STRICT_SPECIALIZATION_HPP

#include <boost/mpl11/bool.hpp>

namespace boost { namespace mpl11 { namespace detail { namespace logical_or {
    template <typename ...xs>
    struct strict_specialization_impl
        : true_
    { };

    template <typename ...T>
    struct strict_specialization_impl<integer_c<T, false>...>
        : false_
    { };

    /*!
     * @ingroup details
     *
     * Strict (non short-circuiting) logical or implemented with (clever)
     * partial specialization.
     */
    template <typename ...xs>
    using strict_specialization = strict_specialization_impl<
        bool_<(bool)xs::type::value>...
    >;
}}}} // end namespace boost::mpl11::detail::logical_or

#endif // !BOOST_MPL11_DETAIL_LOGICAL_OR_STRICT_SPECIALIZATION_HPP


#include <k4a/k4a.hpp>

#include <iostream>
#include <vector>
#include <array>

#include <Eigen/Dense>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

#include "k4a_grabber.h"

using namespace std;
using namespace boost;
using namespace pcl;

typedef pcl::PointXYZRGBA PointType;

int main(int argc, char **argv)
{
	const uint32_t deviceCount = k4a::device::get_installed_count();
	if (deviceCount == 0)
	{
		cout << "no azure kinect devices detected!" << endl;
	}

	// PCL Visualizer
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Point Cloud Viewer"));

	// Point Cloud
	pcl::PointCloud<PointType>::ConstPtr cloud;

	// Retrieved Point Cloud Callback Function
	boost::mutex mutex;
	boost::function<void(const pcl::PointCloud<PointType>::ConstPtr&)> function =
		[&cloud, &mutex](const pcl::PointCloud<PointType>::ConstPtr& ptr)
	{
		boost::mutex::scoped_lock lock(mutex);

		/* Point Cloud Processing */

		cloud = ptr->makeShared();
	};

	// KinectAzureDKGrabber
	boost::shared_ptr<pcl::Grabber> grabber =
		boost::make_shared<pcl::KinectAzureDKGrabber>(0, K4A_DEPTH_MODE_WFOV_2X2BINNED, K4A_IMAGE_FORMAT_COLOR_BGRA32, K4A_COLOR_RESOLUTION_720P);

	boost::shared_ptr<pcl::KinectAzureDKGrabber> grabber_ = boost::dynamic_pointer_cast<pcl::KinectAzureDKGrabber>(grabber);

	// Register Callback Function
	boost::signals2::connection connection = grabber->registerCallback(function);

	// Start Grabber
	grabber->start();

	k4a::calibration calibration = grabber_->getCalibration();
	k4a_calibration_intrinsic_parameters_t *intrinsics = &calibration.color_camera_calibration.intrinsics.parameters;
	Eigen::Matrix3f intrinsics_eigen;
	intrinsics_eigen <<
		intrinsics->param.fx, 0.0f, intrinsics->param.cx,
		0.0f, intrinsics->param.fy, intrinsics->param.cy,
		0.0f, 0.0f, 1.0f;
	Eigen::Matrix4f extrinsics_eigen = Eigen::Matrix4f::Identity();
	viewer->setCameraParameters(intrinsics_eigen, extrinsics_eigen);

	while (!viewer->wasStopped())
	{
		// Update Viewer
		viewer->spinOnce();
		boost::mutex::scoped_try_lock lock(mutex);
		if (lock.owns_lock() && cloud)
		{
			// Update Point Cloud
			if (!viewer->updatePointCloud(cloud, "cloud"))
			{
				viewer->addPointCloud(cloud, "cloud");
			}
		}
	}

	// Stop Grabber
	grabber->stop();

	// Disconnect Callback Function
	if (connection.connected())
	{
		connection.disconnect();
	}
	return 0;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  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/simd/function/slide.hpp>
#include <boost/simd/function/shuffle.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

using namespace boost::simd;

STF_CASE_TPL( "Unary slide shuffle - cardinal 2", STF_NUMERIC_TYPES)
{
  bs::pack<T,2> a{ Valmax<T>(), T(42) };
  STF_ALL_EQUAL( (shuffle<-1, 0>(a)), bs::slide<-1>(a) );
  STF_ALL_EQUAL( (shuffle< 1,-1>(a)), bs::slide<+1>(a) );
}

STF_CASE_TPL( "Binary slide shuffle - cardinal 2", STF_NUMERIC_TYPES)
{
  bs::pack<T,2> a{ Valmax<T>(), T(42) };
  bs::pack<T,2> b{ T(69)      , Valmin<T>() };

  STF_ALL_EQUAL( (shuffle<-1, 0>(a,b)), bs::slide<-1>(a) );
  STF_ALL_EQUAL( (shuffle< 1,-1>(a,b)), bs::slide<+1>(a) );
  STF_ALL_EQUAL( (shuffle<-1, 2>(a,b)), bs::slide<-1>(b) );
  STF_ALL_EQUAL( (shuffle< 3,-1>(a,b)), bs::slide<+1>(b) );
}

STF_CASE_TPL( "Unary slide shuffle - cardinal 4", STF_NUMERIC_TYPES)
{
  bs::pack<T,4> a{ Valmax<T>(), T(42), T(69), Valmin<T>() };

  STF_ALL_EQUAL( (shuffle<-1,-1,-1, 0>(a)), bs::slide<-3>(a) );
  STF_ALL_EQUAL( (shuffle<-1,-1, 0, 1>(a)), bs::slide<-2>(a) );
  STF_ALL_EQUAL( (shuffle<-1, 0, 1, 2>(a)), bs::slide<-1>(a) );
  STF_ALL_EQUAL( (shuffle< 1, 2, 3,-1>(a)), bs::slide<+1>(a) );
  STF_ALL_EQUAL( (shuffle< 2, 3,-1,-1>(a)), bs::slide<+2>(a) );
  STF_ALL_EQUAL( (shuffle< 3,-1,-1,-1>(a)), bs::slide<+3>(a) );
}

STF_CASE_TPL( "Binary slide shuffle - cardinal 4", STF_NUMERIC_TYPES)
{
  bs::pack<T,4> a{ Valmax<T>(), T(42), T(69), Valmin<T>() };
  bs::pack<T,4> b{ T(13), Valmin<T>(), T(37), Valmax<T>() };

  STF_ALL_EQUAL( (shuffle<-1,-1,-1, 0>(a,b)), bs::slide<-3>(a) );
  STF_ALL_EQUAL( (shuffle<-1,-1, 0, 1>(a,b)), bs::slide<-2>(a) );
  STF_ALL_EQUAL( (shuffle<-1, 0, 1, 2>(a,b)), bs::slide<-1>(a) );
  STF_ALL_EQUAL( (shuffle< 1, 2, 3,-1>(a,b)), bs::slide<+1>(a) );
  STF_ALL_EQUAL( (shuffle< 2, 3,-1,-1>(a,b)), bs::slide<+2>(a) );
  STF_ALL_EQUAL( (shuffle< 3,-1,-1,-1>(a,b)), bs::slide<+3>(a) );

  STF_ALL_EQUAL( (shuffle<-1,-1,-1, 4>(a,b)), bs::slide<-3>(b) );
  STF_ALL_EQUAL( (shuffle<-1,-1, 4, 5>(a,b)), bs::slide<-2>(b) );
  STF_ALL_EQUAL( (shuffle<-1, 4, 5, 6>(a,b)), bs::slide<-1>(b) );
  STF_ALL_EQUAL( (shuffle< 5, 6, 7,-1>(a,b)), bs::slide<+1>(b) );
  STF_ALL_EQUAL( (shuffle< 6, 7,-1,-1>(a,b)), bs::slide<+2>(b) );
  STF_ALL_EQUAL( (shuffle< 7,-1,-1,-1>(a,b)), bs::slide<+3>(b) );
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_ARITHMETIC_FUNCTION_SIMD_COMMON_REMQUO_HPP_INCLUDED
#define NT2_TOOLBOX_ARITHMETIC_FUNCTION_SIMD_COMMON_REMQUO_HPP_INCLUDED
#include <boost/fusion/tuple.hpp>
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/include/functions/idivround.hpp>
#include <nt2/include/functions/tofloat.hpp>
#include <nt2/include/functions/is_gtz.hpp>
#include <nt2/include/functions/remainder.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type  is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::remquo_, tag::cpu_,
                         (A0)(X),
                         ((simd_<arithmetic_<A0>,X>))
                         ((simd_<arithmetic_<A0>,X>))
                        );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::remquo_(tag::simd_<tag::arithmetic_, X> ,
                           tag::simd_<tag::arithmetic_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0,A0)>
    {
      typedef typename meta::strip<A0>::type                 stA0;
      typedef typename meta::as_integer<A0>::type             iA0;
      typedef boost::fusion::tuple<stA0,iA0>                 type;
    };

    NT2_FUNCTOR_CALL(2)
    {
      typename NT2_RETURN_TYPE(2)::type res;
      eval( a0, a1
          , boost::fusion::at_c<0>(res),  boost::fusion::at_c<1>(res)
          );
      return res;
    }
  private:
    template<class A0,class A1,class R0,class R1> inline void
    eval(A0 const& a0,A1 const& a1,R0& r0, R1& r1) const
    {
      r1 = idivround(a0, a1);
      r0 = remainder(a0, a1);
//       r1 = b_and(r1, nt2::Seven<R1>());
//       R1 r2 =  b_not(b_xor(nt2::Seven<R1>(), r1));
//       r1 = sel(is_gtz(b_and(a0, a1)), r1, r2);
    }

  };
} }

#endif
// modified by jt the 04/01/2011


//=======================================================================
// Copyright 2001 University of Notre Dame.
// Author: Jeremy G. Siek
//
// 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)
//=======================================================================

/*
   Sample output:

   filtered edge set: (A,B) (C,D) (D,B)
   filtered out-edges:
   A --> B
   B -->
   C --> D
   D --> B
   E -->
 */

#include <boost/config.hpp>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/graph_utility.hpp>

template < typename EdgeWeightMap > struct positive_edge_weight
{
    positive_edge_weight() {}
    positive_edge_weight(EdgeWeightMap weight) : m_weight(weight) {}
    template < typename Edge > bool operator()(const Edge& e) const
    {
        return 0 < boost::get(m_weight, e);
    }
    EdgeWeightMap m_weight;
};

int main()
{
    using namespace boost;

    typedef adjacency_list< vecS, vecS, directedS, no_property,
        property< edge_weight_t, int > >
        Graph;
    typedef property_map< Graph, edge_weight_t >::type EdgeWeightMap;

    enum
    {
        A,
        B,
        C,
        D,
        E,
        N
    };
    const char* name = "ABCDE";
    Graph g(N);
    add_edge(A, B, 2, g);
    add_edge(A, C, 0, g);
    add_edge(C, D, 1, g);
    add_edge(C, E, 0, g);
    add_edge(D, B, 3, g);
    add_edge(E, C, 0, g);

    positive_edge_weight< EdgeWeightMap > filter(get(edge_weight, g));
    filtered_graph< Graph, positive_edge_weight< EdgeWeightMap > > fg(
        g, filter);

    std::cout << "filtered edge set: ";
    print_edges(fg, name);

    std::cout << "filtered out-edges:" << std::endl;
    print_graph(fg, name);

    return 0;
}


//                  _  _
//  _   _|_ _  _|o_|__|_
// (_||_||_(_)(_|| |  |
//
// automatic differentiation made easier in C++
// https://github.com/autodiff/autodiff
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2018-2020 Allan Leal
//
// 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.

// Eigen includes
#include <Eigen/Core>

// pybind11 includes
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/operators.h>
namespace py = pybind11;

template<typename Vec, typename T, bool isarray, bool isconst, bool isview>
void exportVector(py::module& m, const char* typestr)
{
    auto cls = py::class_<Vec>(m, typestr);

    if constexpr (!isview) {
        using VecRef = Eigen::Ref<Vec>;
        using VecConstRef = Eigen::Ref<const Vec>;

        cls.def(py::init<>());
        cls.def(py::init<long>());
        cls.def(py::init<const VecRef&>());
        cls.def(py::init<const VecConstRef&>());
    }

    // Define constructors if not a view (e.g., wrapped into an Eigen::Ref)
    if constexpr (!isview) {
        cls.def(py::init<>());
        cls.def(py::init<long>());
    }

    cls.def("__len__", [](const Vec& s) { return s.size(); });

    cls.def("__getitem__", [](const Vec& s, size_t i) {
        const size_t size = s.size();
        if(i >= size) throw py::index_error();
        return s[i];
    });

    if constexpr (!isconst) {
        cls.def("__setitem__", [](Vec& s, size_t i, const T& val) {
            const size_t size = s.size();
            if(i >= size) throw py::index_error();
            s[i] = val;
        });
    }

#if EIGEN_VERSION_AT_LEAST(3, 3, 90)
    cls.def("__iter__", [](const Vec& s) {
        return py::make_iterator(s.begin(), s.end()); // begin/end iterators have not always being available!
    }, py::keep_alive<0, 1>()); // keep object alive while iterator exists;
#endif

    cls.def("__str__", [](const Vec& s) {
        std::stringstream stream;
        stream << s;
        return stream.str();
    });

    cls.def("__repr__", [=](const Vec& s) {
        std::stringstream stream;
        stream << "autodiff." << typestr << "([";
        for(auto i = 0; i < s.size(); ++i)
            stream << (i == 0 ? ", " : "") << s[i];
        stream << "])";
        return stream.str();
    });

    cls.def(-py::self);

    cls.def(py::self + py::self);
    cls.def(py::self - py::self);

    if constexpr(!isconst) {
        cls.def(py::self += py::self);
        cls.def(py::self -= py::self);
        cls.def(py::self *= T());
        cls.def(py::self /= T());
    }

    if constexpr (!isarray) {
        cls.def(py::self == py::self);
        cls.def(py::self != py::self);
    } else {
        cls.def("__eq__", [](const Vec& l, const Vec& r) { return (l == r).all(); } );
        cls.def("__eq__", [](const Vec& l, const py::array_t<double>& r) { return (l == Eigen::ArrayXd::Map(r.data(), r.size())).all(); } );
        cls.def("__ne__", [](const Vec& l, const Vec& r) { return (l != r).any(); } );
        cls.def("__eq__", [](const Vec& l, const py::array_t<double>& r) { return (l != Eigen::ArrayXd::Map(r.data(), r.size())).any(); } );
    }

    if constexpr (isarray) {
        cls.def(py::self * py::self);
        cls.def(py::self / py::self);

        cls.def(py::self + T());
        cls.def(py::self - T());
        cls.def(py::self * T());
        cls.def(py::self / T());

        cls.def(T() + py::self);
        cls.def(T() - py::self);
        cls.def(T() * py::self);
        cls.def(T() / py::self);

        if constexpr (!isconst) {
            cls.def(py::self *= py::self);
            cls.def(py::self /= py::self);
            cls.def(py::self += T());
            cls.def(py::self -= T());
        }
    }

    cls.def("asarray", [](const Vec& s) {
        py::array_t<double> res(s.size());
        auto data = res.mutable_data();
        for(auto i = 0; i < s.size(); ++i)
            data[i] = autodiff::val(s[i]);
        return res;
    });
}

constexpr auto isview(bool val) { return val; }
constexpr auto isconst(bool val) { return val; }
constexpr auto isarray(bool val) { return val; }


/*
    MIT License

    Copyright (c) <2021> Advanced Micro Devices, Inc. All rights reserved

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

// =======================================================
//
// Purpose: Read a matrix market in coordinate format to solve by MUMPS
//
// Usage : mpirun -np X "executable" "inputmatrix"
// Standard C++ includes
//
#include <mpi.h>
#include <math.h>
#include <sstream>
#include <fstream>
#include <iostream>
#include <cstdlib>
#include <iomanip>
#include <vector>
#include <string>
#include <cstdio>
#include <boost/algorithm/string.hpp>
//#include <mkl.h>
#include <omp.h>
#include "dmumps_c.h"
#define JOB_INIT -1
#define JOB_END -2
#define USE_COMM_WORLD -987654

//
// Timing includes
//
# include  <chrono>
using ns = std::chrono::nanoseconds;
using get_time = std::chrono::steady_clock;

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

    const std::string matrix_name = argv[1];
    std::string fileline;
    std::vector<std::string> vecstrng;
    char* filechar;

    DMUMPS_STRUC_C id;
    MUMPS_INT nrows, ncols;
    MUMPS_INT nnz, i, nrhs;
    MUMPS_INT* irn;
    MUMPS_INT* jcn;
    double* aa;
    double* rhs, * duprhs;

    int myid, ierr;
    int comm_size;

    FILE* f;
    nrhs = 1;

    // =======================================
    //   Beginning of Executable Statements
    // =======================================

#ifdef VERBOSE_OUTPUT
    std::cout << "before mpiInit" << std::endl;
#endif
    ierr = MPI_Init(&argc, &argv);
    ierr = MPI_Comm_rank(MPI_COMM_WORLD, &myid);
    ierr = MPI_Comm_size(MPI_COMM_WORLD, &comm_size);

    if (myid == 0)
    {
#ifdef VERBOSE_OUTPUT
        std::cout << " - matrix_name = " << matrix_name << std::endl;
        std::cout << "MPI comm size  = " << comm_size << std::endl;
#endif
    }

    vecstrng.reserve(5);
    // -----------------------------------------
    //   Read in the matrix
    // -----------------------------------------

    //   Create the ifstream reader
    std::ifstream mmfile(matrix_name.c_str());

    //   Get first line which is junk
    std::getline(mmfile, fileline);

    //   Get second line which has dimension data
    std::getline(mmfile, fileline);
    boost::algorithm::trim(fileline);
    boost::algorithm::split(vecstrng, fileline, boost::is_any_of(" "), boost::token_compress_on);

    nrows = atoi(vecstrng[0].c_str());
    ncols = atoi(vecstrng[1].c_str());
    nnz = atoi(vecstrng[2].c_str());

    //   Allocate arrays with the given dimensional data
    irn = new MUMPS_INT[nnz]; // row index
    jcn = new MUMPS_INT[nnz]; // col index
    aa = new double[nnz];    // value
    rhs = new double[nrows]; // dummy rhs.

    std::cout << std::fixed << std::setprecision(20);
    int cnt = 0;
    while (getline(mmfile, fileline)) {
        filechar = strtok((char*)fileline.c_str(), " ");
        irn[cnt] = atoi(filechar);
        filechar = strtok(NULL, " ");
        jcn[cnt] = atoi(filechar);
        filechar = strtok(NULL, " ");
        aa[cnt] = atof(filechar);
        cnt++;
    }
    mmfile.close();
#ifdef VERBOSE_OUTPUT
    std::cout << " Done reading in the matrix ..." << std::endl;
#endif
    // --- Set DUM RHS ---
    for (i = 0; i < nrows; i++)
    {
        rhs[i] = 0.0;
    }

    // ---------------------------------------------
    //   Done reading in the matrix
    // ---------------------------------------------

    // ---------------------------------------------
    //   Setup the MUMPS Solver
    // ---------------------------------------------

              /* Initialize a MUMPS instance. Use MPI_COMM_WORLD */
    id.job = JOB_INIT; /* JOB = -1 : initializes an instance of the package */
    id.comm_fortran = USE_COMM_WORLD;
    id.sym = 1; /* A is assumed to be symmetric positive definite */
    id.par = 1; /* The host is also involved in the parallel steps of the factorization and solve phases */

#ifdef VERBOSE_OUTPUT
    printf("Start Mumps Initialization, JOB Id = %d\n", id.job);
    fflush(stdout);
#endif
    dmumps_c(&id);
    if (id.infog[0] < 0)
    {
        printf("Error Return in Test application, id.infog[0] = %d\n", id.infog[0]);
        fflush(stdout);
        printf("Error Return in Test application, id.infog[1] = %d\n", id.infog[1]);
        fflush(stdout);
        printf("id.infog[2] = %d\n", id.infog[2]);
        fflush(stdout);
        return 1;
    }
#ifdef VERBOSE_OUTPUT
    printf("Done Mumps Initialization\n");
    fflush(stdout);
#endif

    /* Define the problem on the host */
    if (myid == 0) {
        id.n = nrows; id.nz = nnz; id.irn = irn; id.jcn = jcn;
        id.a = aa; id.rhs = rhs;
        id.nrhs = nrhs; id.lrhs = nrows;
    }
    //    mkl_set_dynamic(0);
    //    mkl_set_num_threads(24);
    //    omp_set_num_threads(24);

#define ICNTL(I) icntl[(I)-1] /* macro s.t. indices match documentation */
/* No outputs */

#ifndef VERBOSE_OUTPUT
    id.ICNTL(1) = 6;
    id.ICNTL(2) = 0;
    id.ICNTL(3) = 6;
    id.ICNTL(4) = 1; /* level of printing: Errors, warnings, and main statistics printed */
#endif
    id.ICNTL(7) = 5; //5; /* use METIS ordering if previously installed by the user otherwise treated as 7*/
    /*
    id.ICNTL(7) = 7 means:
       Automatic choice by the software during analysis phase.This choice will depend on the
       ordering packages made available, on the matrix(type and size), and on the number of
       processors.
    */
    id.ICNTL(10) = 0; /* max num of iterative refinements */
    id.ICNTL(22) = 0; /* in core factorization */
    id.ICNTL(24) = 1; // null pivot detection
    id.ICNTL(14) = 20; /* percentage increase in the estimated working space - Default value: 20 (which corresponds to a 20 % increase) */
    /* When significant extra fill-in is caused by numerical pivoting, increasing ICNTL(14) may help */
    id.ICNTL(23) = 250000; // / 16; /* max size of the working memory (MB) that can allocate per processor - node2 on 16 processors*/
    //id.ICNTL(16) = num_thrds; /* SMP Threads*/
//    for (MUMPS_INT ii=0; ii<60; ii++) std::cout << "id.ICNTL ="<< id.ICNTL(ii) <<std::endl;
    // ---------------------------------------------
    //   Done with MUMPS Solver Setup
    // ---------------------------------------------

    // ---------------------------------------------
    //   Symbolic Factorization
    // ---------------------------------------------
    auto SymbolicFactorizationTimeBegin = get_time::now();
    id.job = 1; /* performs the analysis */
#ifdef VERBOSE_OUTPUT
    printf("Start Mumps Analysis, JOB Id = %d\n", id.job);
    fflush(stdout);
#endif
    dmumps_c(&id);
    if (id.infog[0] < 0)
    {
        printf("Error Return in Test application, id.infog[0] = %d\n", id.infog[0]);
        fflush(stdout);
        printf("Error Return in Test application, id.infog[1] = %d\n", id.infog[1]);
        fflush(stdout);
        printf("id.infog[2] = %d\n", id.infog[2]);
        fflush(stdout);
        return 1;
    }
#ifdef VERBOSE_OUTPUT
    printf("End Mumps Analysis\n");
    fflush(stdout);
#endif
    auto SymbolicFactorizationTime = get_time::now() - SymbolicFactorizationTimeBegin;

    // ---------------------------------------------
    //   Numeric Factorization
    // ---------------------------------------------
    auto NumericFactorizationTimeBegin = get_time::now();
    id.job = 2; /* performs the factorization */
#ifdef VERBOSE_OUTPUT
    printf("Start Mumps Factorization, JOB Id = %d\n", id.job);
    fflush(stdout);
#endif
    dmumps_c(&id);
    if (id.infog[0] < 0)
    {
        printf("Error Return in Test application, id.infog[0] = %d\n", id.infog[0]);
        fflush(stdout);
        printf("Error Return in Test application, id.infog[1] = %d\n", id.infog[1]);
        fflush(stdout);
        printf("id.infog[2] = %d\n", id.infog[2]);
        fflush(stdout);
        return 1;
    }
#ifdef VERBOSE_OUTPUT
    printf("End Mumps Factorization\n");
    fflush(stdout);
#endif
    auto NumericFactorizationTime = get_time::now() - NumericFactorizationTimeBegin;

    // ---------------------------------------------
    //   Back substitution
    // ---------------------------------------------
    auto FBSolveTimeBegin = get_time::now();
    id.job = 3; /* computes the solution */
#ifdef VERBOSE_OUTPUT
    printf("Start Mumps Solution, JOB Id = %d\n", id.job);
    fflush(stdout);
#endif
    dmumps_c(&id);
    if (id.infog[0] < 0)
    {
        printf("Error Return in Test application, id.infog[0] = %d\n", id.infog[0]);
        fflush(stdout);
        printf("Error Return in Test application, id.infog[1] = %d\n", id.infog[1]);
        fflush(stdout);
        printf("id.infog[2] = %d\n", id.infog[2]);
        fflush(stdout);
        return 1;
    }
#ifdef VERBOSE_OUTPUT
    printf("End Mumps Solution\n");
    fflush(stdout);
#endif

    auto FBSolveTime = get_time::now() - FBSolveTimeBegin;
    // ---------------------------------------------
    //   Termination and release of memory.
    // ---------------------------------------------
    id.job = JOB_END; /* JOB = -2 : terminates an instance of the package */
#ifdef VERBOSE_OUTPUT
    printf("Start Mumps Termination, JOB Id = %d\n", id.job);
    fflush(stdout);
#endif
    dmumps_c(&id);
    if (id.infog[0] < 0)
    {
        printf("Error Return in Test application, id.infog[0] = %d\n", id.infog[0]);
        fflush(stdout);
        printf("Error Return in Test application, id.infog[1] = %d\n", id.infog[1]);
        fflush(stdout);
        printf("id.infog[2] = %d\n", id.infog[2]);
        fflush(stdout);
        return 1;
    }
#ifdef VERBOSE_OUTPUT
    printf("End Mumps Termination\n");
    fflush(stdout);
#endif
    free(irn);
    free(jcn);
    free(aa);

    double numerator = 0.0;
    for (int i = 1; i < nrows; i++)
    {
        numerator += pow((rhs[i] - 1.0), 2);
    }
    numerator = sqrt(numerator);
    double denominator = sqrt(static_cast<double>(nrows));
    double relativeError = numerator / denominator;

    ierr = MPI_Finalize();
    free(rhs);

    // ---------------------------------------------
    //   Important statistics
    // ---------------------------------------------
    if (myid == 0)
    {
#if 1//def VERBOSE_OUTPUT
        std::cout << " - comm_size = " << comm_size << std::endl;
        std::cout << " - matrix_name = " << matrix_name << std::endl;
        printf("\n$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$");
        fflush(stdout);
        printf("\nRelative Error is: %e ", relativeError);
        fflush(stdout);
        printf("\nTime Symbolic:     %10.2f ", std::chrono::duration_cast<ns>(SymbolicFactorizationTime).count() / 1.0e9);
        fflush(stdout);
        printf("\nTime Numeric:      %10.2f ", std::chrono::duration_cast<ns>(NumericFactorizationTime).count() / 1.0e9);
        fflush(stdout);
        printf("\nTime Solve:        %10.2f ", std::chrono::duration_cast<ns>(FBSolveTime).count() / 1.0e9);
        fflush(stdout);
        printf("\nTime S+N+S:        %10.2f ", std::chrono::duration_cast<ns>(SymbolicFactorizationTime + NumericFactorizationTime + FBSolveTime).count() / 1.0e9);
        fflush(stdout);
        printf("\n$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$\n");
        fflush(stdout);
#endif
        int ompNumThrds = omp_get_max_threads();
        double sparsity_percent = 0.0;
        double symbolic_t = 0.0, numeric_t = 0.0, solve_t = 0.0, sns_t = 0.0;
        sparsity_percent = (nrows * nrows) - nnz;
        sparsity_percent = sparsity_percent/(nrows * nrows);
        sparsity_percent = sparsity_percent * 100;
        symbolic_t = std::chrono::duration_cast<ns>(SymbolicFactorizationTime).count() / 1.0e9;
        numeric_t = std::chrono::duration_cast<ns>(NumericFactorizationTime).count() / 1.0e9;
        solve_t = std::chrono::duration_cast<ns>(FBSolveTime).count() / 1.0e9;
        sns_t = std::chrono::duration_cast<ns>(SymbolicFactorizationTime + NumericFactorizationTime + FBSolveTime).count() / 1.0e9;
        //input, nrows, nnz, sparsity_%, mpiProcs, ompThrds, symbolic_t, numeric_t, solve_t, sns_t, relError
        printf("%s, %d, %d, %5.2f, %d, %d, %5.2f, %5.2f, %5.2f, %5.2f, %5.2f\n", matrix_name.c_str(), nrows, nnz, sparsity_percent, comm_size, ompNumThrds,
                                                                                symbolic_t, numeric_t, solve_t, sns_t, relativeError);
    }

    return 0;
}


/*
 * This file is part of the DUDS project. It is subject to the BSD-style
 * license terms in the LICENSE file found in the top-level directory of this
 * distribution and at http://www.somewhere.org/somepath/license.html.
 * No part of DUDS, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 *
 * Copyright (C) 2020  Jeff Jackowski
 */
/**
 * @file
 * Test of the duds::general::SignedMagnitudeToTwosComplement template.
 */

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <duds/general/SignedMagnitudeToTwosComplement.hpp>

BOOST_AUTO_TEST_SUITE(SignedMagnitudeToTwosComplement)

BOOST_AUTO_TEST_CASE(SignedMagnitude) {
	std::int16_t v = 0xF;
	BOOST_CHECK_EQUAL(duds::general::SignedMagnitudeToTwosComplement<5>(v), 0xF);
	v = 0x1F;
	BOOST_CHECK_EQUAL(duds::general::SignedMagnitudeToTwosComplement<5>(v), -0xF);
	v = 0x10;
	BOOST_CHECK_EQUAL(duds::general::SignedMagnitudeToTwosComplement<5>(v), 0);
	BOOST_CHECK_EQUAL(duds::general::SignedMagnitudeToTwosComplement<16>(v), 0x10);
	// the following two lines intentionally fail to compile
	//duds::general::SignedMagnitudeToTwosComplement<1>(v);
	//duds::general::SignedMagnitudeToTwosComplement<17>(v);
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/euler/include/functions/erf.hpp>

#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/mzero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( erf_real,  NT2_REAL_TYPES)
{
  using nt2::erf;
  using nt2::tag::erf_;
  typedef typename nt2::meta::call<erf_(T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(erf(nt2::Inf<T>()), nt2::One<r_t>(), 10);
  NT2_TEST_ULP_EQUAL(erf(nt2::Nan<T>()), nt2::Nan<r_t>(), 10);
#endif
  NT2_TEST_ULP_EQUAL(erf(nt2::Mzero<T>()), nt2::Zero<r_t>(), 10);
  NT2_TEST_ULP_EQUAL(erf(nt2::Half<T>()), T( 0.520499877813047), 10);
  NT2_TEST_ULP_EQUAL(erf(nt2::One<T>()), T(0.842700792949715), 10);
  NT2_TEST_ULP_EQUAL(erf(nt2::Two<T>()), T(0.995322265018953), 10);
  NT2_TEST_ULP_EQUAL(erf(nt2::Zero<T>()), nt2::Zero<r_t>(), 10);
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestFunctionalConvert
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/copy_n.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/functional/convert.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(convert_int_float)
{
    int data[] = { 1, 2, 3, 4, 5, 6, 7, 8 };
    compute::vector<int> input(8, context);
    compute::copy_n(data, 8, input.begin(), queue);

    compute::vector<float> output(8, context);
    compute::transform(
        input.begin(),
        input.end(),
        output.begin(),
        compute::convert<float>(),
        queue
    );
    CHECK_RANGE_EQUAL(
        float, 8, output, (1.f, 2.f, 3.f, 4.f, 5.f, 6.f, 7.f, 8.f)
    );
}

BOOST_AUTO_TEST_SUITE_END()


// 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 "fiber/geometrical_data.hpp"
#include "fiber/laplace_3d_double_layer_potential_kernel_functor.hpp"
#include "fiber/default_collection_of_kernels.hpp"

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

#include <algorithm>
#include "common/armadillo_fwd.hpp"
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/version.hpp>
#include <complex>

// Tests

BOOST_AUTO_TEST_SUITE(Laplace3dDoubleLayerPotentialKernelFunctor)

BOOST_AUTO_TEST_CASE_TEMPLATE(addGeometricalDependencies_works,
                              ValueType, kernel_types)
{
    Fiber::Laplace3dDoubleLayerPotentialKernelFunctor<ValueType> op;
    size_t testGeomDeps = 1024, trialGeomDeps = 16; // random initial values
    op.addGeometricalDependencies(testGeomDeps, trialGeomDeps);

    BOOST_CHECK(testGeomDeps & Fiber::GLOBALS);
    BOOST_CHECK(trialGeomDeps & Fiber::GLOBALS);
    BOOST_CHECK(trialGeomDeps & Fiber::NORMALS);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluateOnGrid_works_for_points_on_x_axis,
                              ValueType, kernel_types)
{
    typedef Fiber::Laplace3dDoubleLayerPotentialKernelFunctor<ValueType> Functor;
    typedef Fiber::DefaultCollectionOfKernels<Functor> Kernels;
    Kernels kernels((Functor()));

    Fiber::GeometricalData<typename Functor::CoordinateType>
            testGeomData, trialGeomData;
    const int worldDim = 3;
    const int testPointCount = 2, trialPointCount = 3;
    // Note: points lying on x axis only
    testGeomData.globals.set_size(worldDim, testPointCount);
    testGeomData.globals.fill(0.);
    testGeomData.globals(0, 1) = 1.;

    trialGeomData.globals.set_size(worldDim, trialPointCount);
    trialGeomData.globals.fill(0.);
    trialGeomData.globals(0, 0) = 2.;
    trialGeomData.globals(0, 1) = 3.;
    trialGeomData.globals(0, 2) = 4.;

    trialGeomData.normals.set_size(worldDim, trialPointCount);
    trialGeomData.normals.fill(0.);
    trialGeomData.normals(0, 0) = 1.;
    trialGeomData.normals(0, 1) = 1.;
    trialGeomData.normals(0, 2) = 1.;

    Fiber::CollectionOf4dArrays<ValueType> result;
    kernels.evaluateOnGrid(testGeomData, trialGeomData, result);

    Fiber::_4dArray<ValueType> expected(1, 1, testPointCount, trialPointCount);
    for (int trialPoint = 0; trialPoint < trialPointCount; ++trialPoint)
        for (int testPoint = 0; testPoint < testPointCount; ++testPoint) {
            typename Functor::CoordinateType diff =
                    std::abs(testGeomData.globals(0, testPoint) -
                             trialGeomData.globals(0, trialPoint));
            expected(0, 0, testPoint, trialPoint) =
                    -(1. / (4. * M_PI)) / diff / diff;
        }

    BOOST_CHECK(check_arrays_are_close<ValueType>(result[0], expected, 1e-6));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluateOnGrid_works_for_points_on_y_axis,
                              ValueType, kernel_types)
{
    typedef Fiber::Laplace3dDoubleLayerPotentialKernelFunctor<ValueType> Functor;
    typedef Fiber::DefaultCollectionOfKernels<Functor> Kernels;
    Kernels kernels((Functor()));

    Fiber::GeometricalData<typename Functor::CoordinateType>
            testGeomData, trialGeomData;
    const int worldDim = 3;
    const int testPointCount = 2, trialPointCount = 3;
    // Note: points lying on y axis only
    testGeomData.globals.set_size(worldDim, testPointCount);
    testGeomData.globals.fill(0.);
    testGeomData.globals(1, 1) = 1.;

    trialGeomData.globals.set_size(worldDim, trialPointCount);
    trialGeomData.globals.fill(0.);
    trialGeomData.globals(1, 0) = 2.;
    trialGeomData.globals(1, 1) = 3.;
    trialGeomData.globals(1, 2) = 4.;

    trialGeomData.normals.set_size(worldDim, trialPointCount);
    trialGeomData.normals.fill(0.);
    trialGeomData.normals(1, 0) = 1.;
    trialGeomData.normals(1, 1) = 1.;
    trialGeomData.normals(1, 2) = 1.;

    Fiber::CollectionOf4dArrays<ValueType> result;
    kernels.evaluateOnGrid(testGeomData, trialGeomData, result);

    Fiber::_4dArray<ValueType> expected(1, 1, testPointCount, trialPointCount);
    for (int trialPoint = 0; trialPoint < trialPointCount; ++trialPoint)
        for (int testPoint = 0; testPoint < testPointCount; ++testPoint) {
            typename Functor::CoordinateType diff =
                    std::abs(testGeomData.globals(1, testPoint) -
                             trialGeomData.globals(1, trialPoint));
            expected(0, 0, testPoint, trialPoint) =
                    -(1. / (4. * M_PI)) / diff / diff;
        }

    BOOST_CHECK(check_arrays_are_close<ValueType>(result[0], expected, 1e-6));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(evaluateOnGrid_agrees_with_evaluateAtPointPairs,
                              ValueType, kernel_types)
{
    typedef Fiber::Laplace3dDoubleLayerPotentialKernelFunctor<ValueType> Functor;
    typedef Fiber::DefaultCollectionOfKernels<Functor> Kernels;
    Kernels kernels((Functor()));

    typedef Fiber::GeometricalData<typename Functor::CoordinateType> GeomData;

    const int worldDim = 3;
    const int testPointCount = 2, trialPointCount = 3;

    // Collect data with evaluateOnGrid
    GeomData testGeomDataOnGrid, trialGeomDataOnGrid;

    testGeomDataOnGrid.globals.set_size(worldDim, testPointCount);
    testGeomDataOnGrid.globals.fill(0.);
    testGeomDataOnGrid.globals(0, 1) = 1.;

    trialGeomDataOnGrid.globals.set_size(worldDim, trialPointCount);
    trialGeomDataOnGrid.globals.fill(1.);
    trialGeomDataOnGrid.globals(0, 0) = 2.;
    trialGeomDataOnGrid.globals(0, 1) = 3.;
    trialGeomDataOnGrid.globals(0, 2) = 4.;

    trialGeomDataOnGrid.normals.set_size(worldDim, trialPointCount);
    trialGeomDataOnGrid.normals.fill(0.);
    trialGeomDataOnGrid.normals(1, 0) = 1.;
    trialGeomDataOnGrid.normals(1, 1) = 1.;
    trialGeomDataOnGrid.normals(1, 2) = 1.;

    Fiber::CollectionOf4dArrays<ValueType> resultOnGrid;
    kernels.evaluateOnGrid(testGeomDataOnGrid, trialGeomDataOnGrid, resultOnGrid);

    arma::Col<ValueType> convertedResultOnGrid(testPointCount * trialPointCount);
    for (int testPoint = 0; testPoint < testPointCount; ++testPoint)
        for (int trialPoint = 0; trialPoint < trialPointCount; ++trialPoint)
            convertedResultOnGrid(testPoint + trialPoint * testPointCount) =
                    resultOnGrid[0](0, 0, testPoint, trialPoint);

    // Collect data with evaluateAtPointPairs
    GeomData testGeomDataAtPointPairs, trialGeomDataAtPointPairs;
    testGeomDataAtPointPairs.globals.set_size(worldDim, testPointCount * trialPointCount);
    trialGeomDataAtPointPairs.globals.set_size(worldDim, testPointCount * trialPointCount);
    trialGeomDataAtPointPairs.normals.set_size(worldDim, testPointCount * trialPointCount);
    for (int testPoint = 0; testPoint < testPointCount; ++testPoint)
        for (int trialPoint = 0; trialPoint < trialPointCount; ++trialPoint) {
            testGeomDataAtPointPairs.globals.col(testPoint + trialPoint * testPointCount) =
                    testGeomDataOnGrid.globals.col(testPoint);
            trialGeomDataAtPointPairs.globals.col(testPoint + trialPoint * testPointCount) =
                    trialGeomDataOnGrid.globals.col(trialPoint);
            trialGeomDataAtPointPairs.normals.col(testPoint + trialPoint * testPointCount) =
                    trialGeomDataOnGrid.normals.col(trialPoint);
        }

    Fiber::CollectionOf3dArrays<ValueType> resultAtPointPairs;
    kernels.evaluateAtPointPairs(testGeomDataAtPointPairs, trialGeomDataAtPointPairs,
                                 resultAtPointPairs);
    arma::Col<ValueType> convertedResultAtPointPairs(testPointCount * trialPointCount);
    for (int point = 0; point < testPointCount * trialPointCount; ++point)
        convertedResultAtPointPairs(point) =
                resultAtPointPairs[0](0, 0, point);

    BOOST_CHECK(check_arrays_are_close<ValueType>(
                    convertedResultAtPointPairs, convertedResultOnGrid, 1e-6));
}

BOOST_AUTO_TEST_SUITE_END()


#include <mealy.hpp>
#include <reachability.hpp>
#include <splitting_tree.hpp>

#include <docopt.h>
#include <boost/lexical_cast.hpp>

#include <iostream>
#include <random>
#include <fstream>

using namespace std;

static const char USAGE[] =
R"(Random Mealy machine generator

    Usage:
      generator random [options] <states> <inputs> <outputs> <machines> [<seed>]
      generator hopcroft a <states>
      generator hopcroft b <states>

    Options:
      -h, --help                    Show this screen
      --version                     Show version
      -m, --minimal                 Only generate minimal machines
      -c, --connected               Only generate reachable machines
      --output-cluster <factor>     How clustered should the outputs be
      --state-cluster <factor>      And what about states
      --single-output-boost <fctr>  Boost for a single output (e.g. quiescence)
      --permute-alphabets <n>       Makes n copies with permuted input/output
)";

static size_t number_of_leaves(splitting_tree const & root) {
	if (root.children.empty()) return 1;

	return accumulate(root.children.begin(), root.children.end(), 0ul,
	                  [](auto const & l, auto const & r) { return l + number_of_leaves(r); });
}

struct random_options {
	double output_spread = 0;
	double state_spread = 0;
	double single_output_boost = 1;
};

static mealy permute_alphabets(mealy const & m){
	mt19937 gen(random_device{}());
	const auto create_permutation = [&gen](size_t n){
		vector<size_t> p(n);
		iota(p.begin(), p.end(), 0);
		shuffle(p.begin(), p.end(), gen);
		return p;
	};

	const auto ip = create_permutation(m.input_size);
	const auto op = create_permutation(m.output_size);

	mealy ret = m;
	for(state s = 0; s < m.graph_size; ++s){
		for(input i = 0; i < m.input_size; ++i) {
			ret.graph[s][i] = m.graph[s][ip[i]];
			ret.graph[s][i].out = op[ret.graph[s][i].out];
		}
	}
	return ret;
}

static mealy generate_random_machine(size_t N, size_t P, size_t Q, random_options opts, mt19937 & gen) {
	mealy m;

	m.graph_size = N;
	m.input_size = P;
	m.output_size = Q;

	m.graph.assign(m.graph_size, vector<mealy::edge>(m.input_size));

	auto o_dist = [&] {
		const auto factor = opts.output_spread;
		vector<double> probs(m.output_size);
		for (output o = 0; o < m.output_size; ++o)
			probs[o] = exp(factor * o / double(m.output_size - 1));
		probs[0] *= opts.single_output_boost;
		discrete_distribution<output> dist(probs.begin(), probs.end());
		return dist;
	}();

	auto s_dist = [&] {
		const auto factor = opts.state_spread;
		vector<double> probs(m.graph_size);
		for (output o = 0; o < m.graph_size; ++o)
			probs[o] = exp(factor * o / double(m.graph_size - 1));
		discrete_distribution<state> dist(probs.begin(), probs.end());
		return dist;
	}();

	vector<state> states(m.graph_size);
	iota(states.begin(), states.end(), 0);

	for (state s = 0; s < m.graph_size; ++s) {
		shuffle(states.begin(), states.end(), gen);
		for (input i = 0; i < m.input_size; ++i) {
			m.graph[s][i] = {states[s_dist(gen)], o_dist(gen)};
		}
	}

	return m;
}

static void print_machine(string const & prefix, mealy const & m, size_t count) {
	ofstream file(prefix + "_" + to_string(m.graph_size) + "_" + to_string(m.input_size) + "_"
	              + to_string(m.output_size) + "_" + to_string(count) + ".txt");
	for (state s = 0; s < m.graph_size; ++s) {
		for (input i = 0; i < m.input_size; ++i) {
			auto e = m.graph[s][i];
			file << s << " -- " << i << " / " << e.out << " -> " << e.to << endl;
		}
	}
}

int main(int argc, char * argv[]) {
	const auto args = docopt::docopt(USAGE, {argv + 1, argv + argc}, true, __DATE__ __TIME__);

	if (args.at("random").asBool()) {
		random_options opts;
		if (args.at("--output-cluster"))
			opts.output_spread = -boost::lexical_cast<double>(args.at("--output-cluster").asString());
		if (args.at("--state-cluster"))
			opts.state_spread = -boost::lexical_cast<double>(args.at("--state-cluster").asString());
		if (args.at("--single-output-boost"))
			opts.single_output_boost = boost::lexical_cast<double>(args.at("--single-output-boost").asString());

		auto gen = [&] {
			if (args.at("<seed>")) {
				auto seed = args.at("<seed>").asLong();
				return mt19937(seed);
			}
			random_device rd;
			return mt19937(rd());
		}();

		size_t number_of_machines = args.at("<machines>").asLong();
		size_t constructed = 0;

		while (constructed < number_of_machines) {
			auto const m = generate_random_machine(args.at("<states>").asLong(),
			                                       args.at("<inputs>").asLong(),
			                                       args.at("<outputs>").asLong(), opts, gen);

			if (args.at("--connected").asBool()) {
				auto const m2 = reachable_submachine(m, 0);
				if (m2.graph_size != m.graph_size) continue;
			}

			if (args.at("--minimal").asBool()) {
				auto const tree = create_splitting_tree(m, min_hopcroft_style, 0).root;
				if (number_of_leaves(tree) != m.graph_size) continue;
			}

			if (args.at("--permute-alphabets")) {
				auto permuted_copies = args.at("--permute-alphabets").asLong();
				while (permuted_copies--) {
					constructed++;
					auto copy = permute_alphabets(m);
					print_machine("machine", copy, constructed);
				}
			} else {
				constructed++;
				print_machine("machine", m, constructed);
			}
		}
	} else if (args.at("hopcroft").asBool() && args.at("a").asBool()) {
		mealy m;

		m.graph_size = args.at("<states>").asLong();
		m.input_size = m.output_size = 2;
		m.graph.assign(m.graph_size, vector<mealy::edge>(m.input_size));

		for (state s = 0; s < m.graph_size; ++s) {
			m.graph[s][0] = mealy::edge(s + 1, 0);
			m.graph[s][1] = mealy::edge(s, 0);
		}

		// "accepting state"
		m.graph[m.graph_size - 1][0] = mealy::edge(m.graph_size - 1, 1);
		m.graph[m.graph_size - 1][1] = mealy::edge(m.graph_size - 1, 1);

		print_machine("hopcroft_a", m, 1);
	} else if (args.at("hopcroft").asBool() && args.at("b").asBool()) {
		// In the original paper, the machine is not well defined...
		// So I don't know what Hopcroft had in mind exactly...
		mealy m;

		auto n = m.graph_size = args.at("<states>").asLong();
		m.input_size = m.output_size = 2;
		m.graph.assign(m.graph_size, vector<mealy::edge>(m.input_size));

		for (state s = 0; s < n; ++s) {
			m.graph[s][0] = mealy::edge(s ? s - 1 : 0, s < n / 2 ? 0 : 1);
			if (s < n / 2) {
				m.graph[s][1] = mealy::edge(2 * s + 1, 0);
			} else {
				m.graph[s][1] = mealy::edge(s + (n - s) / 2, 0);
			}
		}

		print_machine("hopcroft_b", m, 1);
	}
}


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

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Custom Polygon Example
#include <iostream>

#include <boost/geometry/geometry.hpp>

#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/ring.hpp>

struct my_point
{
    my_point(double an_x = 0, double an_y = 0)
        : x(an_x)
        , y(an_y)
    {}

    double x, y;
};

struct my_ring : std::deque<my_point>
{};

// Define a struct of a polygon, having always two holes
// (of course this can be implemented differently, usually
// with a vector or deque, but it is just an exampe)
struct my_polygon
{
    // required for a polygon: an outer ring...
    my_ring boundary;
    // ... and a Boost.Range compatible inner ring collection
    boost::array<my_ring, 2> holes;

    // just for the sample
    std::string name;

    my_polygon(std::string const& n = "") : name(n) {}
};

// We can conveniently use macro's to register point and ring
BOOST_GEOMETRY_REGISTER_POINT_2D(my_point, double, cs::cartesian, x, y)
BOOST_GEOMETRY_REGISTER_RING(my_ring)

// There is currently no registration macro for polygons
// and besides that a boost::array<T,N> in a macro would
// be very specific, so we show it "by hand":
namespace boost { namespace geometry { namespace traits
{

template<> struct tag<my_polygon> { typedef polygon_tag type; };
template<> struct ring_const_type<my_polygon> { typedef my_ring const& type; };
template<> struct ring_mutable_type<my_polygon> { typedef my_ring& type; };

template<> struct interior_const_type<my_polygon>
{
    typedef boost::array<my_ring, 2> const& type;
};

template<> struct interior_mutable_type<my_polygon>
{
    typedef boost::array<my_ring, 2>& type;
};

template<> struct exterior_ring<my_polygon>
{
    static my_ring& get(my_polygon& p)
    {
        return p.boundary;
    }

    static my_ring const& get(my_polygon const& p)
    {
        return p.boundary;
    }
};

template<> struct interior_rings<my_polygon>
{
    typedef boost::array<my_ring, 2> holes_type;

    static holes_type& get(my_polygon& p)
    {
        return p.holes;
    }

    static holes_type const& get(my_polygon const& p)
    {
        return p.holes;
    }
};

}}} // namespace boost::geometry::traits

int main()
{
    my_polygon p1("my polygon");

    // Fill it the my-way, triangle
    p1.boundary.push_back(my_point(2, 0));
    p1.boundary.push_back(my_point(1, 5));
    p1.boundary.push_back(my_point(7, 6));
    p1.boundary.push_back(my_point(2, 0));

    // Triangle
    p1.holes[0].push_back(my_point(2, 1));
    p1.holes[0].push_back(my_point(2.4, 2));
    p1.holes[0].push_back(my_point(1.9, 2));
    p1.holes[0].push_back(my_point(2, 1));

    // Box
    p1.holes[1].push_back(my_point(3, 3));
    p1.holes[1].push_back(my_point(4, 3));
    p1.holes[1].push_back(my_point(4, 4));
    p1.holes[1].push_back(my_point(3, 4));
    p1.holes[1].push_back(my_point(3, 3));

    std::cout << "Representation of " << p1.name << ": "
        << boost::geometry::dsv(p1) << std::endl;
    std::cout << "Area of " << p1.name << ": "
        << boost::geometry::area(p1) << std::endl;
    std::cout << "Perimeter of " << p1.name << ": "
        << boost::geometry::perimeter(p1) << std::endl;
    std::cout << "Centroid of " << p1.name << ": "
        << boost::geometry::dsv(boost::geometry::return_centroid<my_point>(p1)) << std::endl;

    return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ANY_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_ANY_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/logical.hpp>
#include <boost/simd/meta/as_logical.hpp>
#include <boost/simd/function/simd/genmask.hpp>
#include <boost/simd/function/simd/hmsb.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(any_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
     using sA0 =  bd::scalar_of_t<A0>;
     using result =bs::as_logical_t<sA0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return result(hmsb(genmask(a0)) != 0);
      }
   };

} } }

#endif


// Boost.Geometry Index
//
// n-dimensional box's margin value (hypersurface), 2d perimeter, 3d surface, etc...
//
// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_MARGIN_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_MARGIN_HPP

#include <boost/geometry/index/detail/indexable.hpp>

namespace boost { namespace geometry { namespace index { namespace detail {

template <typename Box>
struct default_margin_result
{
    typedef typename select_most_precise<
        typename coordinate_type<Box>::type,
        long double
    >::type type;
};

//template <typename Box, size_t CurrentDimension, size_t EdgeDimension>
//struct margin_for_each_edge
//{
//    BOOST_STATIC_ASSERT(0 < CurrentDimension);
//    BOOST_STATIC_ASSERT(0 < EdgeDimension);
//
//    static inline typename default_margin_result<Box>::type apply(Box const& b)
//    {
//        return margin_for_each_edge<Box, CurrentDimension, EdgeDimension - 1>::apply(b) *
//            ( geometry::get<max_corner, EdgeDimension - 1>(b) - geometry::get<min_corner, EdgeDimension - 1>(b) );
//    }
//};
//
//template <typename Box, size_t CurrentDimension>
//struct margin_for_each_edge<Box, CurrentDimension, CurrentDimension>
//{
//    BOOST_STATIC_ASSERT(0 < CurrentDimension);
//
//    static inline typename default_margin_result<Box>::type apply(Box const& b)
//    {
//        return margin_for_each_edge<Box, CurrentDimension, CurrentDimension - 1>::apply(b);
//    }
//};
//
//template <typename Box, size_t CurrentDimension>
//struct margin_for_each_edge<Box, CurrentDimension, 1>
//{
//    BOOST_STATIC_ASSERT(0 < CurrentDimension);
//
//    static inline typename default_margin_result<Box>::type apply(Box const& b)
//    {
//        return geometry::get<max_corner, 0>(b) - geometry::get<min_corner, 0>(b);
//    }
//};
//
//template <typename Box>
//struct margin_for_each_edge<Box, 1, 1>
//{
//    static inline typename default_margin_result<Box>::type apply(Box const& /*b*/)
//    {
//        return 1;
//    }
//};
//
//template <typename Box, size_t CurrentDimension>
//struct margin_for_each_dimension
//{
//    BOOST_STATIC_ASSERT(0 < CurrentDimension);
//    BOOST_STATIC_ASSERT(CurrentDimension <= detail::traits::dimension<Box>::value);
//
//    static inline typename default_margin_result<Box>::type apply(Box const& b)
//    {
//        return margin_for_each_dimension<Box, CurrentDimension - 1>::apply(b) +
//            margin_for_each_edge<Box, CurrentDimension, detail::traits::dimension<Box>::value>::apply(b);
//    }
//};
//
//template <typename Box>
//struct margin_for_each_dimension<Box, 1>
//{
//    static inline typename default_margin_result<Box>::type apply(Box const& b)
//    {
//        return margin_for_each_edge<Box, 1, detail::traits::dimension<Box>::value>::apply(b);
//    }
//};

template <typename Box, size_t CurrentDimension>
struct simple_margin_for_each_dimension
{
    BOOST_STATIC_ASSERT(0 < CurrentDimension);
    BOOST_STATIC_ASSERT(CurrentDimension <= detail::traits::dimension<Box>::value);

    static inline typename default_margin_result<Box>::type apply(Box const& b)
    {
        return simple_margin_for_each_dimension<Box, CurrentDimension - 1>::apply(b) +
            geometry::get<max_corner, CurrentDimension - 1>(b) - geometry::get<min_corner, CurrentDimension - 1>(b);
    }
};

template <typename Box>
struct simple_margin_for_each_dimension<Box, 1>
{
    static inline typename default_margin_result<Box>::type apply(Box const& b)
    {
        return geometry::get<max_corner, 0>(b) - geometry::get<min_corner, 0>(b);
    }
};

template <typename Box>
typename default_margin_result<Box>::type comparable_margin(Box const& b)
{
    //return detail::margin_for_each_dimension<Box, detail::traits::dimension<Box>::value>::apply(b);
    return detail::simple_margin_for_each_dimension<Box, detail::traits::dimension<Box>::value>::apply(b);
}

//template <typename Box>
//typename default_margin_result<Box>::type margin(Box const& b)
//{
//    return 2 * detail::margin_for_each_dimension<Box, detail::traits::dimension<Box>::value>::apply(b);
//}

}}}} // namespace boost::geometry::index::detail

#endif // BOOST_GEOMETRY_INDEX_DETAIL_ALGORITHMS_MARGIN_HPP


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

#ifndef poplibs_test_Gru_hpp
#define poplibs_test_Gru_hpp

#include <boost/multi_array.hpp>
#include <boost/optional.hpp>
#include <popnn/GruDef.hpp>
#include <popnn/NonLinearityDef.hpp>

namespace poplibs_test {
namespace gru {

// Defines for state information in forward pass
#define GRU_NUM_FWD_STATES 4
#define GRU_FWD_STATE_RESET_GATE_IDX 0
#define GRU_FWD_STATE_UPDATE_GATE_IDX 1
#define GRU_FWD_STATE_CANDIDATE_IDX 2
#define GRU_FWD_STATE_ACTS_IDX 3

// Defines for state information in backward pass
#define GRU_NUM_BWD_STATES BASIC_GRU_CELL_NUM_UNITS

/**
 * Compute the output of a basic non-fused GRU cell (without
 * peephole connections).
 *
 * \param outputFullSequence  if true, the all sequence of outputs will be
 *                            returned, otherwise, only the output of the last
 *                            cell will be returned.
 * \param input               Input to the GRU cell of dimension
 *                            [sequenceSize][batchSize][inputSize]
 * \param biases              Biases in the GRU cell of dimension
 *                            [BASIC_GRU_CELL_NUM_UNITS][outputSize]
 * \param prevOutput          Previous output used in the first time step
 *                            [batchSize][outputSize]
 * \param weightsInput        Weights in the GRU cell which weigh the input
 *                            sequence. It is of dimension
 *                            [BASIC_GRU_CELL_NUM_UNITS][inputSize][outputSize]
 * \param weightsOutput       Weights in the GRU cell which weigh the output
 *                            sequence. It is of dimension
 *                            [BASIC_GRU_CELL_NUM_UNITS][outputSize][outputSize]
 * \param state               The forward state for all the sequence steps of
 *                            dimension
 *                            [GRU_NUM_FWD_STATES][sequenceSize][batchSize]
 *                            [outputSize]
 * \param lastOutput          Output after the final time step, of shape
 *                            [batchSize][outputSize]
 * \param cellOrder           The order that the weights for each gate are
 *                            stored in the input
 * \param resetAfter          Whether to apply the reset gate after the matrix
 *                            multiplication
 * \param recurrantBiases     A second set of biases applied to the recurrant
 *                            side of each gate
 *                            Only used when resetAfter=true
 *                            shape: [BASIC_GRU_CELL_NUM_UNITS][output ch]
 * \param activation          Activation function.
 * \param recurrentActivation Recurrent activation function.
 */
void basicGruCellForwardPass(
    const boost::multi_array_ref<double, 3> input,
    const boost::multi_array_ref<double, 2> biases,
    const boost::multi_array_ref<double, 2> prevOutput,
    const boost::multi_array_ref<double, 3> weightsInput,
    const boost::multi_array_ref<double, 3> weightsOutput,
    const boost::optional<boost::multi_array_ref<double, 2>> &attScoresOpt,
    const boost::optional<boost::multi_array_ref<unsigned, 1>> &timeSteps,
    boost::multi_array_ref<double, 4> state,
    boost::multi_array_ref<double, 2> lastOutput,
    const std::vector<BasicGruCellUnit> &cellOrder, bool resetAfter = false,
    const boost::optional<boost::multi_array_ref<double, 2>> recurrantBiases =
        boost::none,
    const popnn::NonLinearityType activation = popnn::NonLinearityType::TANH,
    const popnn::NonLinearityType recurrentActivation =
        popnn::NonLinearityType::SIGMOID);

/** Run backward pass given forward sequence
 *
 * \param outputFullSequence if true, the full sequence of outputs will be
 *                           returned, otherwise, only the output of the last
 *                           cell will be returned.
 * \param weightsInput    Input weights
 *                        shape: [BASIC_GRU_CELL_NUM_UNITS][input ch][output ch]
 * \param weightsOutput   Output weights
 *                        shape: [BASIC_GRU_CELL_NUM_UNITS][output ch]
                                 [output ch]
 * \param gradsNextLayer  Gradients from next layer needed to compute gradients
 *                        for this layer. shape: [sequence][batch][output ch]
 * \param fwdState        Forward state returned by basicGRUCellForwardPass.
 *                        shape: [GRU_NUM_FWD_STATES][sequence][batch]
 *                               [output ch]
 * \param outputActsInit  Initial activations for the forward pass
 *                        shape: [batch][output ch]
 * \param bwdState        Backward state returned by this function
 *                        shape:[GRU_NUM_BWD_STATES][sequence]
 *                              [batch][output ch]
 * \param gradsPrevIn     Gradients for previous layer computed by this function
 *                        shape: [sequence][batch][input ch]
 * \param gradsPrevOut    The gradient computed by this function of the layer
 *                        that precedes the earliest layer computed.
 *                        shape: [batch][output ch]
 * \param cellOrder       The order that the weights for each gate are
 *                        stored in the input
 * \param resetAfter      Whether the reset gate was applied after the matrix
 *                        multiplication
 * \param recurrantBiases     A second set of biases applied to the recurrant
 *                            side of each gate
 *                            Only used when resetAfter=true
 *                            shape: [BASIC_GRU_CELL_NUM_UNITS][output ch]
 * \param activation          Activation function.
 * \param recurrentActivation Recurrent activation function.
 */
void basicGruCellBackwardPass(
    bool outputFullSequence,
    const boost::multi_array_ref<double, 3> weightsInput,
    const boost::multi_array_ref<double, 3> weightsOutput,
    const boost::multi_array_ref<double, 3> gradsNextLayer,
    const boost::multi_array_ref<double, 4> fwdState,
    const boost::multi_array_ref<double, 2> outputActsInit,
    const boost::optional<boost::multi_array_ref<unsigned, 1>> &timeSteps,
    const boost::optional<boost::multi_array_ref<double, 2>> &attScoresOpt,
    const boost::optional<boost::multi_array_ref<double, 2>> &attScoresGradsOpt,
    boost::multi_array_ref<double, 4> bwdState,
    boost::multi_array_ref<double, 3> gradsPrevIn,
    boost::multi_array_ref<double, 2> gradsPrevOut,
    const std::vector<BasicGruCellUnit> &cellOrder, bool resetAfter = false,
    const boost::optional<boost::multi_array_ref<double, 2>> recurrantBiases =
        boost::none,
    const popnn::NonLinearityType activation = popnn::NonLinearityType::TANH,
    const popnn::NonLinearityType recurrentActivation =
        popnn::NonLinearityType::SIGMOID);

/** Param update
 *
 * \param prevLayerActs   Activations from previous layer
 *                        shape: [sequence][batch][input ch]
 * \param fwdState        Forward state compute by basicGRUCellForwardPass
 *                        shape: [GRU_NUM_FWD_STATES][sequence][batch][output]
 * \param outputActsInit  Initial activations for the forward pass
 *                        shape: [batch][output ch]
 * \param bwdState        Backward channel state generated by backward pass
 *                        shape: [GRU_NUM_BWD_STATES][sequence][batch][output]
 * \param weightsInputDeltas  Weight deltas computed by this function
 *                            shape: [BASIC_GRU_CELL_NUM_UNITS][input ch]
                                     [output ch]
 * \param weightsOutputDeltas Weight deltas computed by this function
 *                            shape: [BASIC_GRU_CELL_NUM_UNITS][output ch]
                                     [output ch]
 * \param biasDeltas      Bias deltas computed by this function
 *                        shape: [BASIC_GRU_CELL_NUM_UNITS][output ch]
 * \param cellOrder       The order that the weights for each gate are
 *                        stored in the input
 * \param resetAfter      Whether the reset gate was applied after the matrix
 *                        multiplication
 * \param recurrantBiasDeltas Bias deltas computed by this function for the
 *                            recurrant biases
 *                            Only used when resetAfter=true
 *                            shape: [BASIC_GRU_CELL_NUM_UNITS][output ch]
 */

void basicGruCellParamUpdate(
    const boost::multi_array_ref<double, 3> prevLayerActs,
    const boost::multi_array_ref<double, 4> fwdState,
    const boost::multi_array_ref<double, 2> outputActsInit,
    const boost::multi_array_ref<double, 4> bwdState,
    boost::multi_array_ref<double, 3> weightsInputDeltas,
    boost::multi_array_ref<double, 3> weightsOutputDeltas,
    boost::multi_array_ref<double, 2> biasDeltas,
    const std::vector<BasicGruCellUnit> &cellOrder, bool resetAfter = false,
    boost::optional<boost::multi_array_ref<double, 2>> recurrantBiasDeltas =
        boost::none);
} // namespace gru
} // namespace poplibs_test

#endif // poplibs_test_gru_hpp


/***************************************************************************
 *   Copyright (C) 2006 by Nicola Bellotto                                 *
 *   nbellotto@lincoln.ac.uk                                                    *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "ekf.h"
#include <float.h>
#include <boost/numeric/ublas/io.hpp>
#include "bayes_tracking/jacobianmodel.h"
#include "bayes_tracking/BayesFilter/matSup.hpp"
#include "bayes_tracking/BayesFilter/bayesFlt.hpp"

using namespace Bayesian_filter;
using namespace Bayesian_filter_matrix;

EKFilter::EKFilter(std::size_t x_size) :
        Kalman_state_filter(x_size),
        Covariance_scheme(x_size),
        s(Empty), z_p(Empty)
{
    EKFilter::x_size = x_size;
    FM::Vec x0(x_size);
    x0.clear();
    FM::SymMatrix P0(x_size, x_size);
    P0.clear();
    init(x0, P0);
}

EKFilter::EKFilter(const FM::Vec& x0, const FM::SymMatrix& P0) :
        Kalman_state_filter(x0.size()),
        Covariance_scheme(x0.size()),
        s(Empty), z_p(Empty)
{
    EKFilter::x_size = x0.size();
    init(x0, P0);
}

EKFilter::~EKFilter()
{
}

void EKFilter::init(const FM::Vec& x0, const FM::SymMatrix& P0)
{
    init_kalman(x0, P0);
}

Bayes_base::Float EKFilter::predict (Linrz_predict_model& f) {
    dynamic_cast<JacobianModel&>(f).updateJacobian(x); // update model linearization
    return Covariance_scheme::predict(f);
}

Bayes_base::Float EKFilter::predict (Gaussian_predict_model& f) {
    dynamic_cast<JacobianModel&>(f).updateJacobian(x); // update model linearization
    return Covariance_scheme::predict(f);
}

void EKFilter::update(Linrz_predict_model& predict_model,
                      Linrz_correlated_observe_model& observe_model,
                      const FM::Vec& z)
{
    dynamic_cast<JacobianModel&>(predict_model).updateJacobian(x); // update model linearization
    predict(predict_model);

    dynamic_cast<JacobianModel&>(observe_model).updateJacobian(x); // update model linearization
    observe(observe_model, z);
}

void EKFilter::predict_observation(Linrz_correlated_observe_model& observe_model, FM::Vec& z_pred, FM::SymMatrix& R_pred)
{
    dynamic_cast<JacobianModel&>(observe_model).updateJacobian(x); // update model linearization
    z_pred = observe_model.h(x);  // predicted observation
    // covariance of predicted observation
    Bayesian_filter_matrix::Matrix dum(prod(X, trans(observe_model.Hx)));
    noalias(R_pred) = prod(observe_model.Hx, dum);
}

Bayes_base::Float EKFilter::observeInnovation(Linrz_correlated_observe_model& h, const Bayesian_filter_matrix::Vec& si, const Bayesian_filter_matrix::SymMatrix& Si)
/*
 * Extended linrz correlated observe, compute innovation for observe_innovation
 */
{
    dynamic_cast<JacobianModel&>(h).updateJacobian(x); // update model linearization

    if (last_z_size != si.size()) {
        s.resize(si.size());
    }
    FM::noalias(s) = si;

    // Size consistency, z to model
    if (s.size() != h.Z.size1())
        error (Logic_exception("observation and model size inconsistent in observeInnovation"));
    observe_size (s.size());// Dynamic sizing

    // Innovation covariance
    Bayesian_filter_matrix::Matrix temp_XZ (prod(X, trans(h.Hx)));
    noalias(S) = prod(h.Hx, temp_XZ) + h.Z;

    // Inverse innovation covariance
    Float rcond = UdUinversePD (SI, S);
    rclimit.check_PD(rcond, "S not PD in observeInnovation");

    // Kalman gain, X*Hx'*SI
    noalias(W) = prod(temp_XZ, SI);

    // State update
    noalias(x) += prod(W, s);
    // update state cov with modified innovation cov
    noalias(X) -= prod_SPD(W, Si, temp_XZ);   // temp_XZ used just as temporary storage matrix

    return rcond;
}

Bayes_base::Float
EKFilter::observe (Linrz_correlated_observe_model& h, const FM::Vec& z)
/*
 * Extended linrz correlated observe, compute innovation for observe_innovation
 */
{
    dynamic_cast<JacobianModel&>(h).updateJacobian(x); // update model linearization

    Covariance_scheme::update ();
    const FM::Vec& zp = h.h(x);      // Observation model, zp is predicted observation

    if (last_z_size != z.size()) {
        s.resize(z.size());
    }
    s = z;
    h.normalise(s, zp);
    FM::noalias(s) -= zp;

    return observe_innovation (h, s);
}

Bayes_base::Float
EKFilter::observe (Linrz_uncorrelated_observe_model& h, const FM::Vec& z)
/*
 * Extended kalman uncorrelated observe, compute innovation for observe_innovation
 */
{
    dynamic_cast<JacobianModel&>(h).updateJacobian(x); // update model linearization

    Covariance_scheme::update ();
    const FM::Vec& zp = h.h(x);      // Observation model, zp is predicted observation

    if (last_z_size != z.size()) {
        s.resize(z.size());
    }
    s = z;
    h.normalise(s, zp);
    FM::noalias(s) -= zp;
    return observe_innovation (h, s);
}

double EKFilter::logLikelihood() {
    SymMatrix Si(S.size1(), S.size2());
    Float detS;
    Float rcond = UdUinversePD(Si, detS, S);  // Si = inv(S)
    Numerical_rcond rclimit;
    rclimit.check_PD(rcond, "S not PD in EKFilter::logLikelihood");
    // exp(-0.5 * (s' * Si * s)) / sqrt(2pi^ns * |S|)
    return -0.5*(inner_prod(trans(s),prod(Si, s))) - 0.5*((double)s.size()*log(2*M_PI)+log(detS));
}


//
// Copyright Jason Rice 2015
// 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 <mpdef/append_if.hpp>

#include <boost/hana.hpp>

namespace hana = boost::hana;

int main()
{
  {
    constexpr auto xs = hana::to_tuple(hana::range_c<int, 1, 10>);
    constexpr auto pred = hana::demux(hana::not_)(hana::reverse_partial(hana::mod, hana::int_c<2>));
    constexpr auto result = hana::fold(xs, hana::make_tuple(), mpdef::append_if(pred));
    constexpr auto expected = hana::tuple_c<int, 2, 4, 6, 8>;
    BOOST_HANA_CONSTANT_ASSERT(result == expected);
  }
}


#include <boost/graph/adjacency_list.hpp>
#include <utility>
#include <algorithm>
#include <iterator>
#include <iostream>

int main()
{
  typedef boost::adjacency_list<boost::setS, boost::vecS,
    boost::undirectedS> graph;
  graph g;

  boost::adjacency_list<>::vertex_descriptor v1 = boost::add_vertex(g);
  boost::adjacency_list<>::vertex_descriptor v2 = boost::add_vertex(g);
  boost::add_vertex(g);
  boost::add_vertex(g);

  std::pair<graph::edge_descriptor, bool> p =
    boost::add_edge(v1, v2, g);
  std::cout.setf(std::ios::boolalpha);
  std::cout << p.second << '\n';

  p = boost::add_edge(v1, v2, g);
  std::cout << p.second << '\n';

  p = boost::add_edge(v2, v1, g);
  std::cout << p.second << '\n';

  std::pair<graph::edge_iterator,
    graph::edge_iterator> es = boost::edges(g);

  std::copy(es.first, es.second,
    std::ostream_iterator<graph::edge_descriptor>{std::cout, "\n"});
}