//   Copyright 2015 Patrick Putnam
//
//   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.
#ifndef CLOTHO_TRAIT_SPACE_GENERATOR_HPP_
#define CLOTHO_TRAIT_SPACE_GENERATOR_HPP_

#include <boost/property_tree/ptree.hpp>
#include "clotho/data_spaces/generators/weight_parameter.hpp"
#include "clotho/data_spaces/generators/neutral_generator.hpp"

namespace clotho {
namespace genetics {

template < class RNG, class TraitSpaceType >
class TraitSpaceGenerator {
public:
    typedef TraitSpaceGenerator< RNG, TraitSpaceType > self_type;

    typedef RNG                                     random_engine_type;
    typedef TraitSpaceType                          trait_space_type;
    typedef typename trait_space_type::weight_type  weight_type;

    typedef weight_parameter< weight_type >         parameter_type;
    typedef neutral_generator2                      neutrality_generator_type;
    typedef typename neutrality_generator_type::parameter_type      neutral_param_type;

    TraitSpaceGenerator( random_engine_type * rng, boost::property_tree::ptree & config ) :
        m_rng( rng )
        , m_weights( config )
        , m_neutral( config )
    {}

    TraitSpaceGenerator( random_engine_type * rng, const parameter_type & param, const neutral_param_type & nparam ) :
        m_rng( rng )
        , m_weights( param )
        , m_neutral( nparam)
    {}

    TraitSpaceGenerator( const self_type & other ) :
        m_rng( other.m_rng )
        , m_weights( other.m_weights )
        , m_neutral( other.m_neutral )
    {}

    template < class FreeSpaceType >
    void operator()( trait_space_type & traits, FreeSpaceType & fs, unsigned int N ) {
        generate( traits, fs, N );
    }

    template < class FreeSpaceType >
    void generate( trait_space_type & traits, FreeSpaceType & fs, unsigned int N ) {
        assert( N <= fs.free_size() );

        typename FreeSpaceType::base_type::iterator it = fs.free_begin();

        while( N-- ) {
            typename FreeSpaceType::size_type offset = *it++;
            operator()( traits, offset );
        }
    }

    void operator()( trait_space_type & traits, unsigned int offset ) {
        unsigned int i = 0;
        while( i < traits.trait_count() ) {
            weight_type w = 0;
            if( !m_neutral( *m_rng ) )
                w = m_weights.m_dist( *m_rng );
            traits.setWeight( w, offset, i );
            ++i;
        }
    }

    virtual ~TraitSpaceGenerator() {}

protected:
    random_engine_type  * m_rng;
    parameter_type      m_weights;
    neutrality_generator_type m_neutral;
};

template < class TraitSpaceType >
class TraitSpaceGenerator2 {
public:

    typedef TraitSpaceGenerator2< TraitSpaceType >  self_type;
    typedef TraitSpaceType                          trait_space_type;
    typedef typename trait_space_type::weight_type  weight_type;

    typedef weight_parameter< weight_type >         parameter_type;
    typedef neutral_generator2                      neutrality_generator_type;
    typedef typename neutrality_generator_type::parameter_type      neutral_param_type;

    TraitSpaceGenerator2( boost::property_tree::ptree & config ) :
        m_weights( config )
        , m_neutral( config )
    {}

    TraitSpaceGenerator2( const parameter_type & param, const neutral_param_type & neut_rate ) :
        m_weights( param )
        , m_neutral( neut_rate )
    {}

    TraitSpaceGenerator2( const self_type & other ) :
        m_weights( other.m_weights )
        , m_neutral( other.m_neutral )
    {}

    template < class Engine >
    void operator()( Engine & eng, trait_space_type & traits, unsigned int offset ) {
        unsigned int i = 0;
        while( i < traits.trait_count() ) {

            weight_type w = 0;
            if( !m_neutral( eng ) )
                w = m_weights.m_dist( eng );
            traits.setWeight( w, offset, i );
            ++i;
        }
    }

    virtual ~TraitSpaceGenerator2() {}
protected:

    parameter_type m_weights;
    neutrality_generator_type m_neutral;
};

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_TRAIT_SPACE_GENERATOR_HPP_


///////////////////////////////////////////////////////////////////////////////
// distribution::common::functor::log_unnormalized_pdf.hpp                   //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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_STATISTICS_DETAIL_DISTRIBUTION_COMMON_FUNCTOR_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_COMMON_FUNCTOR_LOG_UNNORMALIZED_PDF_HPP_ER_2009
#include <boost/mpl/assert.hpp>
#include <boost/statistics/detail/distribution_common/meta/value.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace functor{

    template<typename D>
    class log_unnormalized_pdf_
    {

        BOOST_MPL_ASSERT(( boost::mpl::not_< boost::is_reference<D> >));
        // use reference::wrapper if a ref is needed

        public:
        typedef D distribution_type;
        typedef typename meta::value<D>::type result_type;

        log_unnormalized_pdf_(){}
        log_unnormalized_pdf_(const D& d) : d_(d){}
        log_unnormalized_pdf_(const log_unnormalized_pdf_& that) : d_(that.d_){}
        log_unnormalized_pdf_& operator=(const log_unnormalized_pdf_& that)
        {
            if(&that!=this)
            {
                d_ = that.d_;
            }
            return (*this);
        }

        template<typename X>
        result_type
        operator()(const X& x)const{ return log_unnormalized_pdf( this->distribution(), x ); }

        const D& distribution()const{ return this->d_; }

        private:
        D d_;

    };

}// functor
}// distribution
}// detail
}// statistics
}// boost

#endif


#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_controlled_adams_bashforth_moulton

#include <boost/test/unit_test.hpp>

#include <boost/numeric/odeint/stepper/adaptive_adams_bashforth_moulton.hpp>
#include <boost/numeric/odeint/stepper/controlled_adams_bashforth_moulton.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

struct const_sys
{
    template< class State , class Deriv , class Value >
    void operator()( const State &x , Deriv &dxdt , const Value &dt ) const
    {
        dxdt[0] = 1;
    }
};

typedef boost::array< double , 1 > state_type;
typedef double value_type;

BOOST_AUTO_TEST_SUITE( controlled_adams_bashforth_moulton_test )

BOOST_AUTO_TEST_CASE( test_instantiation )
{
    controlled_adams_bashforth_moulton<adaptive_adams_bashforth_moulton<1, state_type> > s1;
    controlled_adams_bashforth_moulton<adaptive_adams_bashforth_moulton<2, state_type> > s2;
    controlled_adams_bashforth_moulton<adaptive_adams_bashforth_moulton<3, state_type> > s3;
    controlled_adams_bashforth_moulton<adaptive_adams_bashforth_moulton<4, state_type> > s4;
    controlled_adams_bashforth_moulton<adaptive_adams_bashforth_moulton<5, state_type> > s5;
    controlled_adams_bashforth_moulton<adaptive_adams_bashforth_moulton<6, state_type> > s6;
    controlled_adams_bashforth_moulton<adaptive_adams_bashforth_moulton<7, state_type> > s7;
    controlled_adams_bashforth_moulton<adaptive_adams_bashforth_moulton<8, state_type> > s8;
    controlled_adams_bashforth_moulton<adaptive_adams_bashforth_moulton<9, state_type> > s9;

    state_type x = {{ 10.0 }};
    value_type t = 0.0 , dt = 0.01;

    s1.try_step(const_sys(), x, t, dt);
    s2.try_step(const_sys(), x, t, dt);
    s3.try_step(const_sys(), x, t, dt);
    s4.try_step(const_sys(), x, t, dt);
    s5.try_step(const_sys(), x, t, dt);
    s6.try_step(const_sys(), x, t, dt);
    s7.try_step(const_sys(), x, t, dt);
    s8.try_step(const_sys(), x, t, dt);
    s9.try_step(const_sys(), x, t, dt);
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Segmentation.hpp
 *
 *  Created on: Mar 26, 2012
 *      Author: david
 */

#ifndef GRAPHSEG_LABELING_HPP_
#define GRAPHSEG_LABELING_HPP_

#include "Common.hpp"
#include "as_range.hpp"
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/connected_components.hpp>
#include <vector>
#include <iostream>

namespace graphseg
{

	/** Node labeling using unique consecutive unsigned integers */
	struct GraphLabeling
	{
		// label for each node
		std::vector<int> labels;

		// number of unique labels
		unsigned int num_labels;

		/** Computes consecutive labels starting with 0 */
		void relabel();

		/** Computes labeling from non-consecutive labels */
		static GraphLabeling CreateClean(const std::vector<int>& labels);

	};

	/** Using connected components to compute graph segments */
	template<typename Graph>
	GraphLabeling ComputeSegmentLabels_ConnectedComponents(const Graph& graph, float threshold);

	/** Using UCM algorithm to compute graph segments */
	template<typename Graph>
	GraphLabeling ComputeSegmentLabels_UCM(const Graph& graph, float threshold);

	namespace ComputeSegmentLabelsStrategies
	{
		enum type {
			ConnectedComponents,
			UCM
		};
	}
	typedef ComputeSegmentLabelsStrategies::type ComputeSegmentLabelsStrategy;

	/** Computes graph segments separated by edges with big weight
	 * Connects all vertices which are connected by edges with a weight smaller than the threshold.
	 * Graph requires edge_weight_t.
	 */
	template<typename WeightedGraph>
	GraphLabeling ComputeSegmentLabels(const WeightedGraph& graph, float threshold, ComputeSegmentLabelsStrategy strategy=ComputeSegmentLabelsStrategies::ConnectedComponents) {
		switch(strategy) {
		default: case ComputeSegmentLabelsStrategies::ConnectedComponents:
			return ComputeSegmentLabels_ConnectedComponents(graph, threshold);
		case ComputeSegmentLabelsStrategies::UCM:
			return ComputeSegmentLabels_UCM(graph, threshold);
		}
	}

	template<typename WeightedGraph>
	GraphLabeling ComputeSegmentLabels_ConnectedComponents(const WeightedGraph& graph, float threshold)
	{
		// create a graph will all edges with costs >= threshold
		SpectralGraph cropped(boost::num_vertices(graph));
		for(auto eid : as_range(boost::edges(graph))) {
			float weight = graph[eid];
			// take only edges with weight < threshold
			if(weight <= threshold) {
				boost::add_edge(boost::source(eid, graph), boost::target(eid, graph), cropped);
			}
		}
		// compute connected components
		std::vector<int> cluster_labels(boost::num_vertices(cropped));
		unsigned int num_labels = boost::connected_components(cropped, &cluster_labels[0]);
		// return result;
		return GraphLabeling{ cluster_labels, num_labels };
	}

	template<typename WeightedGraph>
	GraphLabeling ComputeSegmentLabels_UCM(const WeightedGraph& graph, float threshold)
	{
		// every superpixel is one region
		std::vector<int> cluster_labels(boost::num_vertices(graph));
		for(unsigned int i=0; i<cluster_labels.size(); i++) {
			cluster_labels[i] = i;
		}
		// get edge data
		struct Edge {
			unsigned int a, b;
			float weight;
		};
		std::vector<Edge> edges;
		for(auto eid : as_range(boost::edges(graph))) {
			edges.push_back(Edge{
				static_cast<unsigned int>(boost::source(eid, graph)),
				static_cast<unsigned int>(boost::target(eid, graph)),
				graph[eid]
			});
		}
		// sort edges by weight
		std::sort(edges.begin(), edges.end(), [](const Edge& x, const Edge& y){ return x.weight < y.weight; });
		// cut one edge after another and merge segments
		for(unsigned int k=0; k<edges.size(); k++) {
			if(edges[k].weight >= threshold) {
				break;
			}
			// join segments connected by edge
			int l_old = cluster_labels[edges[k].a];
			int l_new = cluster_labels[edges[k].b];
			std::replace(cluster_labels.begin(), cluster_labels.end(), l_old, l_new);
	#ifdef SEGS_DBG_SHOWGUI
			if(k % 10 == 0) {
				std::cout << "Removed edge with weight " << edges[k].cost << std::endl;
				// show ucm
				relabel();
				slimage::Image3ub vis = computeLabelImage(clusters);
				slimage::gui::Show("ucm", vis);
			}
	#endif
		}
		// create continuous labels
		return GraphLabeling::CreateClean(cluster_labels);
	}

	namespace impl
	{
		struct Edge
		{
			unsigned int a, b;
			float weight;
		};

		/** Prepare labels
		 * Assigns new labels to unsupervised vertices.
		 * Returns the maximal label for supervised vertices.
		 */
		template<typename Graph, typename VertexLabelMap>
		std::vector<int> PrepareLabels(
			const Graph& graph, VertexLabelMap vertex_label_map,
			int* supervised_max_id
		) {
			// find maximal supervised label
			int label_max = -1;
			for(auto vid : as_range(boost::vertices(graph))) {
				const int label = boost::get(vertex_label_map, vid);
				label_max = std::max(label_max, label);
			}
			*supervised_max_id = label_max;
			// give unsupervised vertices a unique label
			std::vector<int> v_labels(boost::num_vertices(graph));
			for(auto vid : as_range(boost::vertices(graph))) {
				int label = boost::get(vertex_label_map, vid);
				if(label < 0) {
					// unsupervised cluster -> need label
					label = ++label_max;
				}
				v_labels[vid] = label;
			}
			return v_labels;
		}

		inline
		void MergeLabels(std::vector<int>& labels, int a, int b) {
			// keep the smaller label (this is the potentially supervised label)
			if(a < b) {
				std::swap(a, b);
			}
			// join segments connected by edge
			std::replace(labels.begin(), labels.end(), a, b);
		}
	}

	/** Supervised labeling
	 * All edges with a weight lower than the threshold are merged.
	 * Edges are not merged if they connect clusters which already have
	 * a label which is not identical for both vertices.
	 */
	template<typename Graph, typename EdgeWeightMap, typename VertexLabelMap>
	inline std::vector<int> ComputeSegmentLabels_UCM_Supervised(
		const Graph& graph,
		EdgeWeightMap edge_weight_map,
		VertexLabelMap vertex_label_map,
		float threshold)
	{
		// find supervised maximal label and give unique labels to unsupervised vertices
		int label_supervised_threshold;
		std::vector<int> cluster_labels = impl::PrepareLabels(graph, vertex_label_map, &label_supervised_threshold);
		// get all edges with a weight smaller than the threshold
		typedef impl::Edge Edge;
		std::vector<Edge> edges;
		edges.reserve(boost::num_edges(graph));
		for(auto eid : as_range(boost::edges(graph))) {
			float w = boost::get(edge_weight_map, eid);
			if(w < threshold) {
				edges.push_back(Edge{
					static_cast<unsigned int>(boost::source(eid, graph)),
					static_cast<unsigned int>(boost::target(eid, graph)),
					w
				});
			}
		}
		// sort edges by weight
		std::sort(edges.begin(), edges.end(), [](const Edge& x, const Edge& y) { return x.weight < y.weight; });
		// cut one edge after another and merge labels
		for(const Edge& edge : edges) {
			// get cluster labels
			const int label_a = cluster_labels[edge.a];
			const int label_b = cluster_labels[edge.b];
			if(label_a == label_b) {
				// clusters are already part of the same segment -> nothing to do
				continue;
			}
			// check if cluster segments are supervised
			const bool is_supervised_a = (label_a <= label_supervised_threshold);
			const bool is_supervised_b = (label_b <= label_supervised_threshold);
			// do not merge if both clusters are supervised
			if(is_supervised_a && is_supervised_b) {
				continue;
			}
			// merge labels
			impl::MergeLabels(cluster_labels, label_a, label_b);
		}

		// return raw labels
		return cluster_labels;
	}

	namespace impl
	{

		struct ucm_sd_debug {
			std::vector<Edge> force_merge;
			std::vector<Edge> normal_merge;
			std::vector<Edge> reject_merge;
			std::vector<Edge> keep_merge;
			std::vector<Edge> no_merge;
		};
	}

	/** Dynamic supervised labeling
	 * Similar to Supervised Labeling, but can also change supervised labels.
	 */
	template<typename Graph, typename EdgeWeightMap, typename VertexLabelMap>
	inline std::vector<int> ComputeSegmentLabels_UCM_Supervised_Dynamic(
		const Graph& graph,
		EdgeWeightMap edge_weight_map,
		VertexLabelMap vertex_label_map,
		float th_low, float th_normal, float th_high,
		impl::ucm_sd_debug* dbg=0)
	{
		// find supervised maximal label and give unique labels to unsupervised vertices
		int label_supervised_threshold;
		std::vector<int> cluster_labels = impl::PrepareLabels(graph, vertex_label_map, &label_supervised_threshold);
		// get all edges
		typedef impl::Edge Edge;
		std::vector<Edge> edges;
		edges.reserve(boost::num_edges(graph));
		for(auto eid : as_range(boost::edges(graph))) {
			edges.push_back(Edge{
				static_cast<unsigned int>(boost::source(eid, graph)),
				static_cast<unsigned int>(boost::target(eid, graph)),
				boost::get(edge_weight_map, eid)
			});
		}
		// sort edges by weight
		std::sort(edges.begin(), edges.end(), [](const Edge& x, const Edge& y) { return x.weight < y.weight; });
		// collect merged edges
		std::vector<Edge> merged_edges;
		// cut one edge after another and merge clusters into segments
		for(const Edge& edge : edges) {
			// get edge data
			int& label_a = cluster_labels[edge.a];
			int& label_b = cluster_labels[edge.b];
			const bool is_supervised_a = (label_a <= label_supervised_threshold);
			const bool is_supervised_b = (label_b <= label_supervised_threshold);
			const float edge_weight = edge.weight;
			// check low weight
			if(edge_weight < th_low) {
				// always merge edge
				merged_edges.push_back(edge);
				if(dbg) dbg->force_merge.push_back(edge);
				// merge labels now only if not both are supervised
				// when merging supervised labels we need to consider more ...
				if(!(is_supervised_a && is_supervised_b)) {
					impl::MergeLabels(cluster_labels, label_a, label_b);
				}
			}
			// check normal weight
			else if(edge_weight < th_normal) {
				// merge edges and labels now if at least one cluster is not supervised
				if(label_a == label_b || !(is_supervised_a && is_supervised_b)) {
					merged_edges.push_back(edge);
					if(dbg) dbg->normal_merge.push_back(edge);
					if(label_a != label_b) {
						impl::MergeLabels(cluster_labels, label_a, label_b);
					}
				}
				else {
					if(dbg) dbg->reject_merge.push_back(edge);
				}
			}
			// check high weight
			else if(edge_weight < th_high) {
				// merge edge if same label
				if(label_a == label_b) {
					merged_edges.push_back(edge);
					if(dbg) dbg->keep_merge.push_back(edge);
				}
				else {
					if(dbg) dbg->no_merge.push_back(edge);
				}
			}
			else {
				if(dbg) dbg->no_merge.push_back(edge);
			}
		}
		// compute label for new clusters
		int label_max = *std::max_element(cluster_labels.begin(), cluster_labels.end());
		label_max = std::max(label_max, label_supervised_threshold);
		// construct a graph with all merged edges
		SpectralGraph cropped(boost::num_vertices(graph));
		for(const Edge& e : merged_edges) {
			auto p = boost::add_edge(e.a, e.b, cropped);
			cropped[p.first] = e.weight;
		}
		// compute connected components
		std::vector<int> cluster_labels_components(boost::num_vertices(cropped));
		unsigned int num_labels = boost::connected_components(cropped, &cluster_labels_components[0]);
		// std::cout << "CC# = " << num_labels << std::endl;
		// check if two components have the same label index
		std::map<int, std::map<unsigned int, unsigned int>> label_component_count;
		for(unsigned int i=0; i<cluster_labels.size(); i++) {
			int label = cluster_labels[i];
			unsigned int cmp_id = cluster_labels_components[i];
			label_component_count[label][cmp_id] ++;
		}
		for(auto p : label_component_count) {
			if(p.second.size() > 1) {
				// std::cout << "Label " << p.first << ": " << std::endl;
				// std::cout << "\tcmp\tcnt" << std::endl;
				unsigned int max_cmp = 0;
				unsigned int max_num = 0;
				for(auto q : p.second) {
					if(q.second >= max_num) {
						max_cmp = q.first;
						max_num = q.second;
					}
				}
				std::map<unsigned int, int> label_remap;
				for(auto q : p.second) {
					if(q.first != max_cmp) {
						label_remap[q.first] = ++label_max;
					}
					else {
						label_remap[q.first] = p.first;
					}
					// std::cout << "\t" << q.first << "\t" << q.second << "\t->" << label_remap[q.first] << std::endl;
				}
				for(unsigned int i=0; i<cluster_labels.size(); i++) {
					if(cluster_labels[i] == p.first) {
						cluster_labels[i] = label_remap[cluster_labels_components[i]];
					}
				}
			}
		}
		// check if one component contains more than one label
		std::map<int, std::map<unsigned int, unsigned int>> component_label_count;
		for(unsigned int i=0; i<cluster_labels.size(); i++) {
			int label = cluster_labels[i];
			unsigned int cmp_id = cluster_labels_components[i];
			component_label_count[cmp_id][label] ++;
		}
		for(auto p : component_label_count) {
			unsigned int max_label = 0;
			unsigned int max_num = 0;
			for(auto q : p.second) {
				if(q.second >= max_num) {
					max_label = q.first;
					max_num = q.second;
				}
			}
			for(auto q : p.second) {
				impl::MergeLabels(cluster_labels, q.first, max_label);
			}
		}
		// return raw labels
		return cluster_labels;
	}

}

#endif


// Copyright 2013 Willow Garage, Inc.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright
//      notice, this list of conditions and the following disclaimer.
//
//    * Redistributions in binary form must reproduce the above copyright
//      notice, this list of conditions and the following disclaimer in the
//      documentation and/or other materials provided with the distribution.
//
//    * Neither the name of the Willow Garage, Inc. 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 THE COPYRIGHT HOLDER OR 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.

/** \Author Acorn Pooley */

#include <geometric_shapes/bodies.h>
#include <geometric_shapes/shape_operations.h>
#include <geometric_shapes/body_operations.h>
#include <boost/filesystem.hpp>
#include <gtest/gtest.h>

TEST(SphereBoundingSphere, Sphere1)
{
  shapes::Sphere shape(1.0);
  Eigen::Vector3d center;
  double radius;
  computeShapeBoundingSphere(&shape, center, radius);

  EXPECT_EQ(1.0, radius);
  EXPECT_EQ(0.0, center.x());
  EXPECT_EQ(0.0, center.y());
  EXPECT_EQ(0.0, center.z());
}

TEST(SphereBoundingSphere, Sphere2)
{
  shapes::Shape* shape = new shapes::Sphere(2.0);
  Eigen::Vector3d center;
  double radius;
  computeShapeBoundingSphere(shape, center, radius);

  EXPECT_EQ(2.0, radius);
  EXPECT_EQ(0.0, center.x());
  EXPECT_EQ(0.0, center.y());
  EXPECT_EQ(0.0, center.z());
  delete shape;
}

TEST(BoxBoundingSphere, Box1)
{
  shapes::Shape* shape = new shapes::Box(2.0, 4.0, 6.0);
  Eigen::Vector3d center;
  double radius;
  computeShapeBoundingSphere(shape, center, radius);

  EXPECT_EQ(sqrt(14.0), radius);
  EXPECT_EQ(0.0, center.x());
  EXPECT_EQ(0.0, center.y());
  EXPECT_EQ(0.0, center.z());
}

TEST(BoxBoundingSphere, Box2)
{
  shapes::Shape* shape = new shapes::Box(2.0, 2.0, 2.0);
  Eigen::Vector3d center;
  double radius;
  computeShapeBoundingSphere(shape, center, radius);

  EXPECT_EQ(sqrt(3.0), radius);
  EXPECT_EQ(0.0, center.x());
  EXPECT_EQ(0.0, center.y());
  EXPECT_EQ(0.0, center.z());
}

TEST(CylBoundingSphere, Cyl1)
{
  shapes::Shape* shape = new shapes::Cylinder(1.0, 4.0);
  Eigen::Vector3d center;
  double radius;
  computeShapeBoundingSphere(shape, center, radius);

  EXPECT_EQ(sqrt(1 + 4), radius);
  EXPECT_EQ(0.0, center.x());
  EXPECT_EQ(0.0, center.y());
  EXPECT_EQ(0.0, center.z());
}

TEST(CylBoundingSphere, Cyl2)
{
  shapes::Shape* shape = new shapes::Cylinder(2.0, 20.0);
  Eigen::Vector3d center;
  double radius;
  computeShapeBoundingSphere(shape, center, radius);

  EXPECT_EQ(sqrt(4 + 100), radius);
  EXPECT_EQ(0.0, center.x());
  EXPECT_EQ(0.0, center.y());
  EXPECT_EQ(0.0, center.z());
}

TEST(ConeBoundingSphere, Cone1)
{
  shapes::Shape* shape = new shapes::Cone(20.0, 2.0);
  Eigen::Vector3d center;
  double radius;
  computeShapeBoundingSphere(shape, center, radius);

  EXPECT_EQ(20.0, radius);
  EXPECT_EQ(0.0, center.x());
  EXPECT_EQ(0.0, center.y());
  EXPECT_EQ(-1.0, center.z());
}

TEST(ConeBoundingSphere, Cone2)
{
  shapes::Shape* shape = new shapes::Cone(5.0, 5.0);
  Eigen::Vector3d center;
  double radius;
  computeShapeBoundingSphere(shape, center, radius);

  EXPECT_EQ(5.0, radius);
  EXPECT_EQ(0.0, center.x());
  EXPECT_EQ(0.0, center.y());
  EXPECT_EQ(-2.5, center.z());
}

TEST(ConeBoundingSphere, Cone3)
{
  double height = 1.0 + 1.0 / sqrt(2);
  shapes::Shape* shape = new shapes::Cone(1 / sqrt(2), height);
  Eigen::Vector3d center;
  double radius;
  computeShapeBoundingSphere(shape, center, radius);

  EXPECT_EQ(1.0, radius);
  EXPECT_EQ(0.0, center.x());
  EXPECT_EQ(0.0, center.y());
  EXPECT_EQ(height / 2 - 1, center.z());
}

TEST(ConeBoundingSphere, Cone4)
{
  shapes::Shape* shape = new shapes::Cone(3, 10);
  Eigen::Vector3d center;
  double radius;
  computeShapeBoundingSphere(shape, center, radius);

  EXPECT_EQ(109.0 / 20, radius);
  EXPECT_EQ(0.0, center.x());
  EXPECT_EQ(0.0, center.y());
  EXPECT_EQ(5 - (109.0 / 20), center.z());
}

TEST(MeshBoundingSphere, Mesh1)
{
  shapes::Shape* shape = new shapes::Mesh(8, 12);
  EXPECT_EQ(shape->type, shapes::MESH);
  shapes::Mesh* m = static_cast<shapes::Mesh*>(shape);

  // box mesh

  m->vertices[3 * 0 + 0] = 0;
  m->vertices[3 * 0 + 1] = 0;
  m->vertices[3 * 0 + 2] = 0;

  m->vertices[3 * 1 + 0] = 1;
  m->vertices[3 * 1 + 1] = 0;
  m->vertices[3 * 1 + 2] = 0;

  m->vertices[3 * 2 + 0] = 0;
  m->vertices[3 * 2 + 1] = 1;
  m->vertices[3 * 2 + 2] = 0;

  m->vertices[3 * 3 + 0] = 1;
  m->vertices[3 * 3 + 1] = 1;
  m->vertices[3 * 3 + 2] = 0;

  m->vertices[3 * 4 + 0] = 0;
  m->vertices[3 * 4 + 1] = 0;
  m->vertices[3 * 4 + 2] = 1;

  m->vertices[3 * 5 + 0] = 1;
  m->vertices[3 * 5 + 1] = 0;
  m->vertices[3 * 5 + 2] = 1;

  m->vertices[3 * 6 + 0] = 0;
  m->vertices[3 * 6 + 1] = 1;
  m->vertices[3 * 6 + 2] = 1;

  m->vertices[3 * 7 + 0] = 1;
  m->vertices[3 * 7 + 1] = 1;
  m->vertices[3 * 7 + 2] = 1;

  m->triangles[3 * 0 + 0] = 0;
  m->triangles[3 * 0 + 1] = 1;
  m->triangles[3 * 0 + 2] = 2;

  m->triangles[3 * 1 + 0] = 1;
  m->triangles[3 * 1 + 1] = 3;
  m->triangles[3 * 1 + 2] = 2;

  m->triangles[3 * 2 + 0] = 5;
  m->triangles[3 * 2 + 1] = 4;
  m->triangles[3 * 2 + 2] = 6;

  m->triangles[3 * 3 + 0] = 5;
  m->triangles[3 * 3 + 1] = 6;
  m->triangles[3 * 3 + 2] = 7;

  m->triangles[3 * 4 + 0] = 1;
  m->triangles[3 * 4 + 1] = 5;
  m->triangles[3 * 4 + 2] = 3;

  m->triangles[3 * 5 + 0] = 5;
  m->triangles[3 * 5 + 1] = 7;
  m->triangles[3 * 5 + 2] = 3;

  m->triangles[3 * 6 + 0] = 4;
  m->triangles[3 * 6 + 1] = 0;
  m->triangles[3 * 6 + 2] = 2;

  m->triangles[3 * 7 + 0] = 4;
  m->triangles[3 * 7 + 1] = 2;
  m->triangles[3 * 7 + 2] = 6;

  m->triangles[3 * 8 + 0] = 2;
  m->triangles[3 * 8 + 1] = 3;
  m->triangles[3 * 8 + 2] = 6;

  m->triangles[3 * 9 + 0] = 3;
  m->triangles[3 * 9 + 1] = 7;
  m->triangles[3 * 9 + 2] = 6;

  m->triangles[3 * 10 + 0] = 1;
  m->triangles[3 * 10 + 1] = 0;
  m->triangles[3 * 10 + 2] = 4;

  m->triangles[3 * 11 + 0] = 1;
  m->triangles[3 * 11 + 1] = 4;
  m->triangles[3 * 11 + 2] = 5;

  Eigen::Vector3d center;
  double radius;
  computeShapeBoundingSphere(shape, center, radius);

  EXPECT_EQ(sqrt(0.75), radius);
  EXPECT_EQ(0.5, center.x());
  EXPECT_EQ(0.5, center.y());
  EXPECT_EQ(0.5, center.z());
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


// (c) Copyright Juergen Hunold 2012
// 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)

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/filtered_graph.hpp>

namespace boost
{

enum edge_info_t
{
    edge_info = 114
};

BOOST_INSTALL_PROPERTY(edge, info);
}

template < typename EdgeInfo, typename Directed > class Graph
{
public:
    typedef boost::property< boost::edge_info_t, EdgeInfo > tEdge_property;

    typedef boost::adjacency_list< boost::setS, boost::vecS, Directed,
        boost::no_property, tEdge_property >
        tGraph;

    typedef typename boost::graph_traits< tGraph >::vertex_descriptor tNode;
    typedef typename boost::graph_traits< tGraph >::edge_descriptor tEdge;

protected:
    tGraph m_Graph;
};

class DataEdge;

class UndirectedGraph : public Graph< DataEdge*, boost::undirectedS >
{
public:
    template < class Evaluator, class Filter >
    void dijkstra(Evaluator const&, Filter const&) const;
};

template < typename Graph, typename Derived >
struct Evaluator : public boost::put_get_helper< int, Derived >
{
    typedef int value_type;
    typedef typename Graph::tEdge key_type;
    typedef int reference;
    typedef boost::readable_property_map_tag category;

    explicit Evaluator(Graph const* pGraph);
};

template < typename Graph >
struct LengthEvaluator : public Evaluator< Graph, LengthEvaluator< Graph > >
{
    explicit LengthEvaluator(Graph const* pGraph);

    typedef typename Evaluator< Graph, LengthEvaluator< Graph > >::reference
        reference;
    typedef typename Evaluator< Graph, LengthEvaluator< Graph > >::key_type
        key_type;

    virtual reference operator[](key_type const& edge) const;
};

template < class Graph > struct EdgeFilter
{
    typedef typename Graph::tEdge key_type;

    EdgeFilter();

    explicit EdgeFilter(Graph const*);

    bool operator()(key_type const&) const;

private:
    const Graph* m_pGraph;
};

template < class Evaluator, class Filter >
void UndirectedGraph::dijkstra(
    Evaluator const& rEvaluator, Filter const& rFilter) const
{
    tNode nodeSource = vertex(0, m_Graph);

    std::vector< tNode > predecessors(num_vertices(m_Graph));

    boost::filtered_graph< tGraph, Filter > filteredGraph(m_Graph, rFilter);

    boost::dijkstra_shortest_paths(filteredGraph, nodeSource,
        boost::predecessor_map(&predecessors[0]).weight_map(rEvaluator));
}

// explicit instantiation
template void UndirectedGraph::dijkstra(
    LengthEvaluator< UndirectedGraph > const&,
    EdgeFilter< UndirectedGraph > const&) const;

int main(int, char**)
{
    return 0;
} // Tests above will fail to compile if anything is broken


#include <scitbx/array_family/boost_python/flex_fwd.h>

#if defined(SCITBX_LBFGS_HAVE_LBFGS_FEM)
#include <scitbx/lbfgs_fem.hpp>
#endif
#include <algorithm>
#include <stdexcept>
#include <cstdio>
#include <cmath>
#include <scitbx/error.h>
#include <scitbx/lbfgs.h>
#include <scitbx/lbfgs/drop_convergence_test.h>
#include <scitbx/lbfgs/raw.h>
#include <scitbx/lbfgs/raw_reference.h>
#include <scitbx/array_family/flex_types.h>
#include <scitbx/array_family/boost_python/utils.h>
#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>

namespace scitbx { namespace lbfgs { namespace ext {
  int
  fortran(
    int n,
    int m,
    af::ref<double> const& x,
    double f,
    af::const_ref<double> const& g,
    int diagco,
    af::ref<double> const& diag,
    af::tiny<int, 2> const& iprint,
    double eps,
    double xtol,
    af::ref<double> const& w,
    int iflag)
  {
    SCITBX_ASSERT(n > 0);
    SCITBX_ASSERT(m > 0);
    std::size_t n_ = static_cast<std::size_t>(n);
    std::size_t m_ = static_cast<std::size_t>(m);
    SCITBX_ASSERT(x.size() == n_);
    SCITBX_ASSERT(g.size() == n_);
    SCITBX_ASSERT(diagco == 0 || diagco == 1);
    SCITBX_ASSERT(diag.size() == n_);
    SCITBX_ASSERT(w.size() == n_*(2*m_+1)+2*m_);
#if defined(SCITBX_LBFGS_HAVE_LBFGS_FEM)
    static lbfgs_fem::common cmn(0, 0);
    lbfgs_fem::blockdata_lb2(cmn);
    lbfgs_fem::lbfgs(
      cmn,
      n,
      m,
      x[0],
      f,
      g[0],
      diagco,
      diag[0],
      iprint[0],
      eps,
      xtol,
      w[0],
      iflag);
#else
    throw std::runtime_error("lbfgs_fem is not available.");
#endif
    return iflag;
  }

  int
  raw_reference(
    int n,
    int m,
    af::ref<double> const& x,
    double f,
    af::const_ref<double> const& g,
    int diagco,
    af::ref<double> const& diag,
    af::tiny<int, 2> const& iprint,
    double eps,
    double xtol,
    af::ref<double> const& w,
    int iflag)
  {
    SCITBX_ASSERT(n > 0);
    SCITBX_ASSERT(m > 0);
    std::size_t n_ = static_cast<std::size_t>(n);
    std::size_t m_ = static_cast<std::size_t>(m);
    SCITBX_ASSERT(x.size() == n_);
    SCITBX_ASSERT(g.size() == n_);
    SCITBX_ASSERT(diagco == 0 || diagco == 1);
    SCITBX_ASSERT(diag.size() == n_);
    SCITBX_ASSERT(w.size() == n_*(2*m_+1)+2*m_);
    using scitbx::lbfgs::raw_reference::const_ref1; // fully-qualified
    using scitbx::lbfgs::raw_reference::ref1;       // to work around
    scitbx::lbfgs::raw_reference::lbfgs(            // gcc 3.2 bug
      n,
      m,
      ref1<double>(x.begin(), n),
      f,
      const_ref1<double>(g.begin(), n),
      diagco,
      ref1<double>(diag.begin(), n),
      const_ref1<int>(iprint.begin(), 2),
      eps,
      xtol,
      ref1<double>(w.begin(), static_cast<int>(w.size())),
      iflag);
    return iflag;
  }

struct raw_lbfgs : boost::noncopyable {

  scitbx::lbfgs::raw::lbfgs lbfgs_obj;

  int
  operator()(
    int n,
    int m,
    af::ref<double> const& x,
    double f,
    af::const_ref<double> const& g,
    int diagco,
    af::ref<double> const& diag,
    af::tiny<int, 2> const& iprint,
    double eps,
    double xtol,
    af::ref<double> const& w,
    int iflag)
  {
    SCITBX_ASSERT(n > 0);
    SCITBX_ASSERT(m > 0);
    std::size_t n_ = static_cast<std::size_t>(n);
    std::size_t m_ = static_cast<std::size_t>(m);
    SCITBX_ASSERT(x.size() == n_);
    SCITBX_ASSERT(g.size() == n_);
    SCITBX_ASSERT(diagco >= 0);
    SCITBX_ASSERT(diagco <= 3);
    SCITBX_ASSERT(diag.size() == n_);
    SCITBX_ASSERT(w.size() == n_*(2*m_+1)+2*m_);
    using scitbx::lbfgs::raw::const_ref1; // fully-qualified
    using scitbx::lbfgs::raw::ref1;       // to work around
    lbfgs_obj(                            // gcc 3.2 bug
      n,
      m,
      ref1<double>(x.begin(), n),
      f,
      const_ref1<double>(g.begin(), n),
      diagco,
      ref1<double>(diag.begin(), n),
      const_ref1<int>(iprint.begin(), 2),
      eps,
      xtol,
      ref1<double>(w.begin(), static_cast<int>(w.size())),
      iflag);
    return iflag;
  }

  int
  nfun() const { return lbfgs_obj.nfun; }

  int
  iter() const { return lbfgs_obj.iter; }

  double
  stp() const { return lbfgs_obj.stp; }

  void
  set_stp(double value) { lbfgs_obj.stp = value; }

  af::shared<double>
  current_search_direction() const
  {
    return af::shared<double>(
      lbfgs_obj.current_search_direction.begin(),
      lbfgs_obj.current_search_direction.end());
  }
};

struct raw_lbfgs_wrappers
{
  typedef raw_lbfgs w_t;

  static
  void
  wrap()
  {
    using namespace boost::python;
    class_<w_t, boost::noncopyable>("raw_lbfgs", no_init)
      .def(init<>())
      .def("__call__", &w_t::operator(), (
        arg("n"),
        arg("m"),
        arg("x"),
        arg("f"),
        arg("g"),
        arg("diagco"),
        arg("diag"),
        arg("iprint"),
        arg("eps"),
        arg("xtol"),
        arg("w"),
        arg("iflag")))
      .def("nfun", &w_t::nfun)
      .def("iter", &w_t::iter)
      .def("stp", &w_t::stp)
      .def("set_stp", &w_t::set_stp, (arg("value")))
      .def("current_search_direction", &w_t::current_search_direction)
    ;
  }
};

  struct minimizer_wrappers
  {
    typedef minimizer<double> w_t;

    static bool
    run_4(
      w_t& minimizer,
      af::flex_double& x,
      double f,
      af::flex_double const& g,
      af::flex_double const& diag)
    {
      using namespace scitbx::af::boost_python;
      SCITBX_ASSERT(flex_as_base_array(x).size() == minimizer.n());
      SCITBX_ASSERT(flex_as_base_array(g).size() == minimizer.n());
      SCITBX_ASSERT(flex_as_base_array(diag).size() == minimizer.n());
      return minimizer.run(x.begin(), f, g.begin(), diag.begin());
    }

    static bool
    run_3(
      w_t& minimizer,
      af::flex_double& x,
      double f,
      af::flex_double const& g)
    {
      using namespace scitbx::af::boost_python;
      SCITBX_ASSERT(flex_as_base_array(x).size() == minimizer.n());
      SCITBX_ASSERT(flex_as_base_array(g).size() == minimizer.n());
      return minimizer.run(x.begin(), f, g.begin());
    }

    //---> Insertion starts
    static bool
    run_6(
      w_t& minimizer,
      af::flex_double& x,
      double f,
      af::flex_double const& g,
      af::flex_double const& diag,
      bool gradient_only,
      bool line_search)
    {
      using namespace scitbx::af::boost_python;
      SCITBX_ASSERT(flex_as_base_array(x).size() == minimizer.n());
      SCITBX_ASSERT(flex_as_base_array(g).size() == minimizer.n());
      SCITBX_ASSERT(flex_as_base_array(diag).size() == minimizer.n());
      return minimizer.run(x.begin(), f, g.begin(), diag.begin(), gradient_only,line_search);
    }

    static bool
    run_5(
      w_t& minimizer,
      af::flex_double& x,
      double f,
      af::flex_double const& g,
      bool gradient_only,
      bool line_search)
    {
      using namespace scitbx::af::boost_python;
      SCITBX_ASSERT(flex_as_base_array(x).size() == minimizer.n());
      SCITBX_ASSERT(flex_as_base_array(g).size() == minimizer.n());
      return minimizer.run(x.begin(), f, g.begin(), gradient_only,line_search);
    }
    //<--- Insertion ends

    static double
    euclidean_norm(
      w_t const& minimizer,
      af::flex_double const& a)
    {
      using namespace scitbx::af::boost_python;
      SCITBX_ASSERT(flex_as_base_array(a).size() == minimizer.n());
      return minimizer.euclidean_norm(a.begin());
    }

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("minimizer", no_init)
        .def(init<optional<std::size_t, std::size_t, std::size_t,
          double, double, double, double> >())
        //---> Insertion starts
        .def("run", run_6)
        .def("run", run_5)
        //<--- Insertion ends
        .def("run", run_4)
        .def("run", run_3)
        .def("n", &w_t::n)
        .def("m", &w_t::m)
        .def("maxfev", &w_t::maxfev)
        .def("gtol", &w_t::gtol)
        .def("xtol", &w_t::xtol)
        .def("stpmin", &w_t::stpmin)
        .def("stpmax", &w_t::stpmax)
        .def("requests_f_and_g", &w_t::requests_f_and_g)
        .def("requests_diag", &w_t::requests_diag)
        .def("iter", &w_t::iter)
        .def("nfun", &w_t::nfun)
        .def("euclidean_norm", euclidean_norm)
        .def("stp", &w_t::stp)
      ;
    }
  };

  struct traditional_convergence_test_wrappers
  {
    typedef traditional_convergence_test<double> w_t;

    static bool
    call(
      w_t const& is_converged,
      af::flex_double const& x,
      af::flex_double const& g)
    {
      using namespace scitbx::af::boost_python;
      SCITBX_ASSERT(flex_as_base_array(x).size() == is_converged.n());
      SCITBX_ASSERT(flex_as_base_array(g).size() == is_converged.n());
      return is_converged(x.begin(), g.begin());
    }

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("traditional_convergence_test")
        .def(init<std::size_t, optional<double> >((
          arg("n"),
          arg("eps")=1.e-5)))
        .def("n", &w_t::n)
        .def("eps", &w_t::eps)
        .def("__call__", call)
      ;
    }
  };

  struct drop_convergence_test_wrappers
  {
    typedef drop_convergence_test<double> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("drop_convergence_test", no_init)
        .def(init<optional<std::size_t, double, double> >((
          arg("n_test_points")=5,
          arg("max_drop_eps")=1.e-5,
          arg("iteration_coefficient")=2)))
        .def("n_test_points", &w_t::n_test_points)
        .def("max_drop_eps", &w_t::max_drop_eps)
        .def("iteration_coefficient", &w_t::iteration_coefficient)
        .def("__call__", &w_t::operator())
        .def("objective_function_values", &w_t::objective_function_values, (
          arg("f")))
        .def("max_drop", &w_t::max_drop)
      ;
    }
  };

  void init_module()
  {
    using namespace boost::python;

    scope().attr("have_lbfgs_fem") =
#if defined(SCITBX_LBFGS_HAVE_LBFGS_FEM)
      true;
#else
      false;
#endif
    def("fortran", fortran, (
      arg("n"),
      arg("m"),
      arg("x"),
      arg("f"),
      arg("g"),
      arg("diagco"),
      arg("diag"),
      arg("iprint"),
      arg("eps"),
      arg("xtol"),
      arg("w"),
      arg("iflag")));
    def("raw_reference", raw_reference, (
      arg("n"),
      arg("m"),
      arg("x"),
      arg("f"),
      arg("g"),
      arg("diagco"),
      arg("diag"),
      arg("iprint"),
      arg("eps"),
      arg("xtol"),
      arg("w"),
      arg("iflag")));
    raw_lbfgs_wrappers::wrap();

    minimizer_wrappers::wrap();
    traditional_convergence_test_wrappers::wrap();
    drop_convergence_test_wrappers::wrap();
  }

}}} // namespace scitbx::lbfgs::ext

BOOST_PYTHON_MODULE(scitbx_lbfgs_ext)
{
  scitbx::lbfgs::ext::init_module();
}


/*******************************************************************************
Copyright(c) 2015-2021 Parker Hannifin Corp. All rights reserved.

MIT Licensed. See the included LICENSE.txt for a copy of the full MIT License.
*******************************************************************************/
#include "mscl/MicroStrain/MIP/Packets/MipFieldParser.h"
#include "mscl/MicroStrain/MIP/Packets/MipDataPacket.h"
#include "mscl/MicroStrain/MIP/MipDataPoint.h"
#include "mscl/MicroStrain/Inertial/Packets/InertialFieldParser_Sensor.h"

#include <boost/test/unit_test.hpp>

using namespace mscl;

BOOST_AUTO_TEST_SUITE(InertialFieldParser_Sensor_Test)

BOOST_AUTO_TEST_CASE(InertialFieldParser_RawAccelVector_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(1.234f);    //Accel 1 float
    bytes.append_float(0.0f);    //Accel 2 float
    bytes.append_float(0.0f);    //Accel 3 float

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_RAW_ACCEL_VEC, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_RAW_ACCEL_VEC);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_X);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 1.234, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_SENSOR_RAW_ACCEL_VEC);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_Y);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_SENSOR_RAW_ACCEL_VEC);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_Z);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_RawGyroVector_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(0.0f);    //Gyro 1 float
    bytes.append_float(1.234f);    //Gyro 2 float
    bytes.append_float(0.0f);    //Gyro 3 float

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_RAW_GYRO_VEC, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_RAW_GYRO_VEC);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_X);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_SENSOR_RAW_GYRO_VEC);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_Y);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 1.234, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_SENSOR_RAW_GYRO_VEC);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_Z);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_RawMagVector_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(0.0f);    //Mag 1 float
    bytes.append_float(0.0f);    //Mag 2 float
    bytes.append_float(2.345f);    //Mag 3 float

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_RAW_MAG_VEC, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_RAW_MAG_VEC);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_X);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_SENSOR_RAW_MAG_VEC);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_Y);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_SENSOR_RAW_MAG_VEC);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_Z);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 2.345, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_ScaledAccelVector_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(2.345f);    //X Accel float
    bytes.append_float(0.0f);    //Y Accel float
    bytes.append_float(0.0f);    //Z Accel float

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_SCALED_ACCEL_VEC, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_SCALED_ACCEL_VEC);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_X);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 2.345, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_SENSOR_SCALED_ACCEL_VEC);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_Y);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_SENSOR_SCALED_ACCEL_VEC);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_Z);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_ScaledGyroVector_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(2.345f);    //X Gyro float
    bytes.append_float(0.0f);    //Y Gyro float
    bytes.append_float(0.0f);    //Z Gyro float

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_SCALED_GYRO_VEC, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_SCALED_GYRO_VEC);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_X);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 2.345, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_SENSOR_SCALED_GYRO_VEC);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_Y);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_SENSOR_SCALED_GYRO_VEC);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_Z);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_ScaledMagVector_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(2.345f);    //X Mag float
    bytes.append_float(0.0f);    //Y Mag float
    bytes.append_float(0.0f);    //Z Mag float

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_SCALED_MAG_VEC, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_SCALED_MAG_VEC);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_X);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 2.345, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_SENSOR_SCALED_MAG_VEC);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_Y);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_SENSOR_SCALED_MAG_VEC);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_Z);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_DeltaThetaVector_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(2.345f);    //X Delta Theta float
    bytes.append_float(0.0f);    //Y Delta Theta float
    bytes.append_float(0.0f);    //Z Delta Theta float

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_DELTA_THETA_VEC, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_DELTA_THETA_VEC);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_X);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 2.345, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_SENSOR_DELTA_THETA_VEC);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_Y);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_SENSOR_DELTA_THETA_VEC);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_Z);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_DeltaVelocityVector_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(2.345f);    //X Delta Velocity float
    bytes.append_float(0.0f);    //Y Delta Velocity float
    bytes.append_float(0.0f);    //Z Delta Velocity float

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_DELTA_VELOCITY_VEC, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_DELTA_VELOCITY_VEC);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_X);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 2.345, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_SENSOR_DELTA_VELOCITY_VEC);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_Y);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_SENSOR_DELTA_VELOCITY_VEC);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_Z);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_OrientMatrix_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(0.0f);
    bytes.append_float(1.234f);
    bytes.append_float(2.345f);
    bytes.append_float(3.456f);
    bytes.append_float(0.0f);
    bytes.append_float(9123.2f);
    bytes.append_float(0.0f);
    bytes.append_float(0.0f);
    bytes.append_float(0.0f);

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_ORIENTATION_MATRIX, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 1);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_ORIENTATION_MATRIX);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_MATRIX);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_Matrix);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(0, 0), 0.0, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(0, 1), 1.234, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(0, 2), 2.345, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(1, 0), 3.456, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(1, 1), 0.0, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(1, 2), 9123.2, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(2, 0), 0.0, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(2, 1), 0.0, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(2, 2), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_Quaternion_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(2.345f);    //q0
    bytes.append_float(0.0f);    //q1
    bytes.append_float(0.0f);    //q2
    bytes.append_float(0.0f);    //q3

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_ORIENTATION_QUATERNION, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 1);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_ORIENTATION_QUATERNION);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_QUATERNION);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_Vector);
    BOOST_CHECK_CLOSE(data.at(0).as_Vector().as_floatAt(0), 2.345, 0.0001);
    BOOST_CHECK_EQUAL(data.at(0).as_Vector().as_floatAt(1), 0);
    BOOST_CHECK_EQUAL(data.at(0).as_Vector().as_floatAt(2), 0);
    BOOST_CHECK_EQUAL(data.at(0).as_Vector().as_floatAt(3), 0);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_OrientUpdateMatrix_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(0.0f);
    bytes.append_float(1.234f);
    bytes.append_float(2.345f);
    bytes.append_float(3.456f);
    bytes.append_float(0.0f);
    bytes.append_float(9123.2f);
    bytes.append_float(0.0f);
    bytes.append_float(0.0f);
    bytes.append_float(0.0f);

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_ORIENTATION_UPDATE_MATRIX, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 1);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_ORIENTATION_UPDATE_MATRIX);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_MATRIX);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_Matrix);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(0, 0), 0.0, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(0, 1), 1.234, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(0, 2), 2.345, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(1, 0), 3.456, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(1, 1), 0.0, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(1, 2), 9123.2, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(2, 0), 0.0, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(2, 1), 0.0, 0.0001);
    BOOST_CHECK_CLOSE(data.at(0).as_Matrix().as_floatAt(2, 2), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_EulerAngles_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(2.345f);    //roll
    bytes.append_float(0.0f);    //pitch
    bytes.append_float(0.0f);    //yaw

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_EULER_ANGLES, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_EULER_ANGLES);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_ROLL);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 2.345, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_SENSOR_EULER_ANGLES);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_PITCH);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_SENSOR_EULER_ANGLES);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_YAW);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_InternalTimestamp_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_uint32(1234567890);     //timestamp value (tick)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_INTERNAL_TIMESTAMP, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 1);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_INTERNAL_TIMESTAMP);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_TICK);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_uint32);
    BOOST_CHECK_EQUAL(data.at(0).as_uint32(), 1234567890);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_BeaconedTimestamp_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_uint8(1);
    bytes.append_uint32(1046317456);    //timestamp seconds (gps time)
    bytes.append_uint32(12345);         //timestamp nanoseconds

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_BEACONED_TIMESTAMP, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 1);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_BEACONED_TIMESTAMP);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_TIMESTAMP);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_Timestamp);
    BOOST_CHECK_EQUAL(data.at(0).as_Timestamp().str(), "2013-03-03 03:44:16.000012345");
    BOOST_CHECK_NO_THROW(data.at(0).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_StabilizedMagVector_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(2.345f);    //X Stab Mag float
    bytes.append_float(0.0f);    //Y Stab Mag float
    bytes.append_float(0.0f);    //Z Stab Mag float

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_STABILIZED_MAG_VEC, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_STABILIZED_MAG_VEC);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_X);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 2.345, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_SENSOR_STABILIZED_MAG_VEC);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_Y);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_SENSOR_STABILIZED_MAG_VEC);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_Z);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_StabilizedAccelVector_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(2.345f);    //X Stab Accel float
    bytes.append_float(0.0f);    //Y Stab Accel float
    bytes.append_float(0.0f);    //Z Stab Accel float

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_STABILIZED_ACCEL_VEC, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_STABILIZED_ACCEL_VEC);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_X);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 2.345, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_SENSOR_STABILIZED_ACCEL_VEC);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_Y);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_SENSOR_STABILIZED_ACCEL_VEC);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_Z);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 0.0, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_GpsCorrelationTimestamp_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_double(1.2);        //gps time of week
    bytes.append_uint16(1200);        //gps week number
    bytes.append_uint16(1);            //timestamp flags

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_GPS_CORRELATION_TIMESTAMP, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);

    //GPS Time of Week
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_GPS_CORRELATION_TIMESTAMP);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_TIME_OF_WEEK);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_double);
    BOOST_CHECK_CLOSE(data.at(0).as_double(), 1.2, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    //GPS Week Number
    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_SENSOR_GPS_CORRELATION_TIMESTAMP);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_WEEK_NUMBER);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_uint16);
    BOOST_CHECK_EQUAL(data.at(1).as_uint16(), 1200);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    //Timestamp Flags
    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_SENSOR_GPS_CORRELATION_TIMESTAMP);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_FLAGS);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_uint16);
    BOOST_CHECK_EQUAL(data.at(2).as_uint16(), 1);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_ScaledAmbientPressure_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(1.2f);        //ambient pressure

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_SENSOR_SCALED_AMBIENT_PRESSURE, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 1);

    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_SENSOR_SCALED_AMBIENT_PRESSURE);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_PRESSURE);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 1.2, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());
}

BOOST_AUTO_TEST_SUITE_END()


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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 "bdd_utils.hpp"

#include <functional>
#include <unordered_map>
#include <unordered_set>

#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/range/numeric.hpp>

#include <cudd.h>
#include <cuddInt.h>

#include <core/utils/range_utils.hpp>

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

/******************************************************************************
 * Functions                                                                  *
 ******************************************************************************/

BDD make_cube( Cudd& manager, const std::vector<BDD>& vars )
{
  return boost::accumulate( vars, manager.bddOne(), []( const BDD& x1, const BDD& x2 ) { return x1 & x2; } );
}

BDD make_cube( Cudd& manager, const std::string& cube )
{
  std::vector<BDD> vars;

  for ( auto i = 0u; i < cube.size(); ++i )
  {
    if ( cube[i] == '0' )
    {
      vars.push_back( ~manager.bddVar( i ) );
    }
    else if ( cube[i] == '1' )
    {
      vars.push_back( manager.bddVar( i ) );
    }
    else
    {
      assert( cube[i] == '-' );
    }
  }
  return make_cube( manager, vars );
}

bool is_selfdual( const Cudd& manager, const BDD& f )
{
  /* negative literals */
  std::vector<BDD> lits( manager.ReadSize() );
  for ( auto i = 0u; i < lits.size(); ++i )
  {
    lits[i] = !manager.bddVar( i );
  }

  return f.VectorCompose( lits ) == !f;
}

bool is_monotone( const Cudd& manager, const BDD& f )
{
  for ( auto i = 0; i < manager.ReadSize(); ++i )
  {
    if ( !f.Increasing( i ) ) { return false; }
  }
  return true;
}

/*
 * Implementation according to
 * Knuth TAOCP Exercise 7.1.4-106
 * [T. Horiyama and T. Ibaraki, Artificial Intelligence 136 (2002), 189-213]
 */
bool is_horn( const Cudd& manager, const BDD& f, const BDD& g, const BDD& h )
{
  if ( f > g ) { return is_horn( manager, g, f, h ); }
  if ( f == manager.bddZero() || h == manager.bddOne() ) { return true; }
  if ( g == manager.bddOne() || h == manager.bddZero() ) { return false; }

  assert( f != manager.bddOne() );
  assert( g != manager.bddZero() );

  BDD fl( manager, cuddE( f.getRegularNode() ) );
  BDD fh( manager, cuddT( f.getRegularNode() ) );
  BDD gl( manager, cuddE( g.getRegularNode() ) );
  BDD gh( manager, cuddT( g.getRegularNode() ) );
  BDD hl( manager, cuddE( h.getRegularNode() ) );
  BDD hh( manager, cuddT( h.getRegularNode() ) );

  return is_horn( manager, fl, gl, hl ) && is_horn( manager, fl, gh, hl ) && is_horn( manager, fh, gl, hl ) && is_horn( manager, fh, gh, hh );
}

bool is_horn( const Cudd& manager, const BDD& f )
{
  return is_horn( manager, f, f, f );
}

#define EXTRA_BDD_COMPARE_TAG 0x96
#define EXTRA_BDD_COMPARE_EQ ((DdNode*)1)
#define EXTRA_BDD_COMPARE_IC ((DdNode*)2)
#define EXTRA_BDD_COMPARE_LT ((DdNode*)3)
#define EXTRA_BDD_COMPARE_GT ((DdNode*)4)

DdNode * Extra_bddCompare( DdManager * dd, DdNode * f, DdNode * g )
{
  DdNode * one = DD_ONE( dd );
  DdNode * zero = Cudd_Not( one );

  if ( f == g ) { return EXTRA_BDD_COMPARE_EQ; }
  if ( f == zero || g == one ) { return EXTRA_BDD_COMPARE_LT; }
  if ( f == one || g == zero ) { return EXTRA_BDD_COMPARE_GT; }

  DdNode * r = nullptr;
  /*if ( ( r = cuddConstantLookup( dd, EXTRA_BDD_COMPARE_TAG, f, g, nullptr ) ) )
  {
    return r;
  }*/

  auto fl = Cudd_NotCond( Cudd_E( Cudd_Regular( f ) ), Cudd_IsComplement( f ) );
  auto fh = Cudd_NotCond( Cudd_T( Cudd_Regular( f ) ), Cudd_IsComplement( f ) );
  auto gl = Cudd_NotCond( Cudd_E( Cudd_Regular( g ) ), Cudd_IsComplement( g ) );
  auto gh = Cudd_NotCond( Cudd_T( Cudd_Regular( g ) ), Cudd_IsComplement( g ) );

  auto rl = Extra_bddCompare( dd, fl, gl );
  if ( rl == EXTRA_BDD_COMPARE_IC ) { r = EXTRA_BDD_COMPARE_IC; }
  else
  {
    auto rh = Extra_bddCompare( dd, fh, gh );
    if ( rh == EXTRA_BDD_COMPARE_IC ) { r = EXTRA_BDD_COMPARE_IC; }
    else if ( rl == EXTRA_BDD_COMPARE_EQ ) { r = rh; }
    else if ( rh == EXTRA_BDD_COMPARE_EQ ) { r = rl; }
    else if ( rl == rh ) { r = rl; }
    else { r = EXTRA_BDD_COMPARE_IC; }
  }

  //cuddCacheInsert( dd, EXTRA_BDD_COMPARE_TAG, f, g, nullptr, r );
  return r;
}

bool Extra_bddUnate( DdManager * dd, DdNode * f, std::vector<int>& ps )
{
  /* Initialize */
  ps.resize( Cudd_ReadSize( dd ) );
  boost::fill( ps, 0 );

  if ( Cudd_IsConstant( f ) ) return true;

  auto fr = Cudd_Regular( f );

  auto fl = Cudd_NotCond( Cudd_E( fr ), Cudd_IsComplement( f ) );
  auto fh = Cudd_NotCond( Cudd_T( fr ), Cudd_IsComplement( f ) );

  if ( !Extra_bddUnate( dd, fl, ps ) || !Extra_bddUnate( dd, fh, ps ) ) { return false; }

  auto r = Extra_bddCompare( dd, fl, fh );
  if ( r == EXTRA_BDD_COMPARE_IC ) { return false; }

  if ( r == EXTRA_BDD_COMPARE_LT )
  {
    if ( ps[fr->index] < 0 ) { return false; }
    ps[fr->index] = 1;
    return true;
  }
  if ( r == EXTRA_BDD_COMPARE_GT )
  {
    if ( ps[fr->index] > 0 ) { return false; }
    ps[fr->index] = -1;
    return true;
  }

  /* this should not happen */
  assert( r == EXTRA_BDD_COMPARE_EQ );
  assert( false );
  return false;
}

bool is_unate( const Cudd& manager, const BDD& f, std::vector<int>& ps )
{
  return Extra_bddUnate( manager.getManager(), f.getNode(), ps );
}

void collect_nodes( DdNode * f, std::unordered_set<DdNode*>& visited )
{
  assert( f );
  assert( !Cudd_IsComplement( f ) );

  /* node visited before? */
  if ( visited.find( f ) != visited.end() ) { return; }

  /* mark node as visited */
  visited.insert( f );

  /* terminate? */
  if cuddIsConstant( f ) { return; }

  /* recur */
  collect_nodes( cuddT( f ), visited );
  collect_nodes( Cudd_Regular( cuddE( f ) ), visited );
}

void collect_nodes_and_count( DdNode * f, std::unordered_map<DdNode*, unsigned>& visited )
{
  assert( f );
  assert( !Cudd_IsComplement( f ) );

  /* node visited before? */
  auto it = visited.find( f );
  if ( it != visited.end() )
  {
    /* visited once more */
    it->second++;
    return;
  }

  /* mark node as visited */
  visited.insert( {f, 1u} );

  /* terminate? */
  if cuddIsConstant( f ) { return; }

  /* recur */
  collect_nodes_and_count( cuddT( f ), visited );
  collect_nodes_and_count( Cudd_Regular( cuddE( f ) ), visited );
}

void collect_nodes_and_count_ignore_complemented( const DdNode * f, std::unordered_map<DdNode*, unsigned>& visited )
{
  assert( f );

  auto fr = Cudd_Regular( f );

  /* node visited before? */
  auto it = visited.find( fr );
  if ( it != visited.end() )
  {
    /* visited once more */
    if ( !Cudd_IsComplement( f ) )
    {
      it->second++;
    }
    return;
  }

  /* mark node as visited */
  visited.insert( {fr, (Cudd_IsComplement( f ) ? 0u : 1u)} );

  /* terminate? */
  if cuddIsConstant( fr ) { return; }

  /* recur */
  collect_nodes_and_count_ignore_complemented( cuddT( fr ), visited );
  collect_nodes_and_count_ignore_complemented( cuddE( fr ), visited );
}

std::vector<unsigned> level_sizes( DdManager * dd, const std::vector<DdNode*>& fs )
{
  using namespace std::placeholders;

  std::vector<unsigned> sizes( Cudd_ReadSize( dd ) );

  /* collect all nodes in the BDDs */
  std::unordered_set<DdNode*> visited;
  for ( const auto* f : fs )
  {
    collect_nodes( Cudd_Regular( f ), visited );
  }

  for ( const auto* node : visited )
  {
    if ( !cuddIsConstant( node ) )
    {
      sizes[node->index]++;
    }
  }

  return sizes;
}

std::vector<unsigned> level_sizes( const Cudd& manager, const std::vector<BDD>& fs )
{
  using namespace std::placeholders;
  using boost::adaptors::transformed;

  std::vector<DdNode*> fs_native( fs.size() );
  boost::copy( fs | transformed( std::bind( &BDD::getRegularNode, _1 ) ), fs_native.begin() );

  return level_sizes( manager.getManager(), fs_native );
}

unsigned maximum_fanout( DdManager* manager, const std::vector<DdNode*>& fs )
{
  using boost::adaptors::map_values;

  /* collect all nodes in the BDDs and count */
  std::unordered_map<DdNode*, unsigned> visited;
  for ( const auto* f : fs )
  {
    collect_nodes_and_count_ignore_complemented( f, visited );
  }

  /* erase constant nodes from visited list */
  visited.erase( DD_ONE( manager ) );
  visited.erase( Cudd_Not( DD_ONE( manager ) ) );

  if ( visited.empty() )
  {
    return 0u;
  }
  else
  {
    return *boost::max_element( visited | map_values );
  }
}

unsigned maximum_fanout( const Cudd& manager, const std::vector<BDD>& fs )
{
  using namespace std::placeholders;
  using boost::adaptors::transformed;

  std::vector<DdNode*> fs_native( fs.size() );
  boost::copy( fs | transformed( std::bind( &BDD::getNode, _1 ) ), fs_native.begin() );

  return maximum_fanout( manager.getManager(), fs_native );
}

void count_complement_edges_rec( DdManager* manager, DdNode* f, unsigned& count, std::unordered_set<DdNode*>& visited )
{
  if ( visited.find( f ) != visited.end() )
  {
    return;
  }

  if ( Cudd_IsComplement( f ) )
  {
    ++count;
  }

  if ( Cudd_IsConstant( f ) )
  {
    return;
  }

  const auto fr = Cudd_Regular( f );

  count_complement_edges_rec( manager, cuddT( fr ), count, visited );
  count_complement_edges_rec( manager, cuddE( fr ), count, visited );

  visited.insert( f );
}

unsigned count_complement_edges( DdManager* manager, const std::vector<DdNode*>& fs )
{
  std::unordered_set<DdNode*> visited;

  auto count = 0u;

  for ( const auto& f : fs )
  {
    count_complement_edges_rec( manager, f, count, visited );
  }

  return count;
}

unsigned count_complement_edges( const Cudd& manager, const std::vector<BDD>& fs )
{
  using namespace std::placeholders;
  using boost::adaptors::transformed;

  std::vector<DdNode*> fs_native( fs.size() );
  boost::copy( fs | transformed( std::bind( &BDD::getNode, _1 ) ), fs_native.begin() );

  return count_complement_edges( manager.getManager(), fs_native );
}

BDD make_eq_rec( const Cudd& manager, const std::vector<BDD>& vars, unsigned pos, int k, std::map<std::pair<unsigned, int>, BDD>& visited )
{
  /* terminal */
  if ( pos == vars.size() )
  {
    return ( k == 0 ) ? manager.bddOne() : manager.bddZero();
  }

  /* cannot be 1 anymore */
  if ( ( k < 0 ) || ( k > static_cast<int>( vars.size() - pos ) ) )
  {
    return manager.bddZero();
  }

  /* cached? */
  const auto it = visited.find( {pos, k} );
  if ( it != visited.end() ) { return it->second; }

  const auto f = ( vars[pos] & make_eq_rec( manager, vars, pos + 1, k - 1, visited ) ) |
                 ( ~vars[pos] & make_eq_rec( manager, vars, pos + 1, k, visited ) );

  visited.insert( {{pos, k}, f} );

  return f;
}

BDD make_eq( const Cudd& manager, const std::vector<BDD>& vars, unsigned k )
{
  std::map<std::pair<unsigned, int>, BDD> visited;
  return make_eq_rec( manager, vars, 0, (int)k, visited );
}

DdNode * bdd_copy_rec( DdManager* mgr_from, DdNode* from, DdManager* mgr_to, std::map<DdNode*, DdNode*>& visited, const std::vector<unsigned>& index_map )
{
  assert( !Cudd_IsComplement( from ) );

  if ( Cudd_IsConstant( from ) )
  {
    if ( from == DD_ONE( mgr_from ) )
    {
      return DD_ONE( mgr_to );
    }
    else
    {
      return Cudd_Not( DD_ONE( mgr_to ) );
    }
  }

  /* visited table */
  const auto it = visited.find( from );
  if ( it != visited.end() ) { return it->second; }

  auto * var = Cudd_bddIthVar( mgr_to, index_map.at( from->index ) );

  auto * high = Cudd_NotCond( bdd_copy_rec( mgr_from, Cudd_Regular( cuddT( from ) ), mgr_to, visited, index_map ), Cudd_IsComplement( cuddT( from ) ) );
  auto * low  = Cudd_NotCond( bdd_copy_rec( mgr_from, Cudd_Regular( cuddE( from ) ), mgr_to, visited, index_map ), Cudd_IsComplement( cuddE( from ) ) );

  /* create new node */
  auto* node = Cudd_bddIte( mgr_to, var, high, low );

  visited.insert( {from, node} );
  return node;
}

std::vector<DdNode*> bdd_copy( DdManager* mgr_from, const std::vector<DdNode*>& from, DdManager* mgr_to, std::vector<unsigned>& index_map )
{
  if ( index_map.empty() )
  {
    index_map.resize( Cudd_ReadSize( mgr_from ) );
    boost::iota( index_map, 0u );
  }

  std::map<DdNode*, DdNode*> visited;

  std::vector<DdNode*> ret;

  for ( auto* node : from )
  {
    ret.push_back( Cudd_NotCond( bdd_copy_rec( mgr_from, Cudd_Regular( node ), mgr_to, visited, index_map ), Cudd_IsComplement( node ) ) );
  }

  return ret;
}

std::vector<BDD> bdd_copy( const Cudd& mgr_from, const std::vector<BDD>& from, const Cudd& mgr_to, std::vector<unsigned>& index_map )
{
  using namespace std::placeholders;
  using boost::adaptors::transformed;

  std::vector<DdNode*> from_native( from.size() );
  boost::copy( from | transformed( std::bind( &BDD::getNode, _1 ) ), from_native.begin() );

  auto orig = bdd_copy( mgr_from.getManager(), from_native, mgr_to.getManager(), index_map );
  std::vector<BDD> ret;

  for ( auto* node : orig )
  {
    ret.push_back( BDD( mgr_to, node ) );
  }

  return ret;
}

bdd_function_t compute_characteristic( const bdd_function_t& bdd, bool inputs_first )
{
  const auto& mgr_from = bdd.first;
  const auto& from     = bdd.second;

  Cudd mgr;
  ntimes( mgr_from.ReadSize() + from.size(), [&]() { mgr.bddVar(); } );

  std::vector<unsigned> index_map( mgr_from.ReadSize() );
  boost::iota( index_map, inputs_first ? 0u : from.size() );

  const auto fs = bdd_copy( mgr_from, from, mgr, index_map );

  auto f = mgr.bddOne();

  const auto offset = inputs_first ? mgr_from.ReadSize() : 0u;
  for ( auto i = 0u; i < fs.size(); ++i )
  {
    f &= mgr.bddVar( offset + i ).Xnor( fs[i] );
  }

  return {mgr, {f}};
}

cube_vec_t bdd_to_cubes( DdManager* manager, DdNode* f )
{
  DdGen * gen;
  int * ddcube;
  CUDD_VALUE_TYPE value;

  cube_vec_t cubes;

  Cudd_ForeachCube( manager, f, gen, ddcube, value )
  {
    boost::dynamic_bitset<> bits( manager->size ), care( manager->size );
    for ( auto i = 0; i < manager->size; ++i )
    {
      switch ( ddcube[i] )
      {
      case 0:
        care[i] = true;
        break;
      case 1:
        bits[i] = true;
        care[i] = true;
        break;
      default:
        break;
      }
    }
    cubes.push_back( cube( bits, care ) );
  }

  return cubes;
}

cube_vec_t bdd_to_cubes( const Cudd& manager, BDD f )
{
  return bdd_to_cubes( manager.getManager(), f.getNode() );
}

/******************************************************************************
 * new BDD operations                                                         *
 ******************************************************************************/

DdNode* bdd_up_rec( DdManager* manager, DdNode* f )
{
  /* terminal case */
  if ( Cudd_IsConstant( f ) ) { return f; }

  const auto* F = Cudd_Regular( f );

  if ( F->ref != 1 )
  {
    auto* r = cuddCacheLookup1( manager, bdd_up_rec, f );
    if ( r )
    {
      return r;
    }
  }

  const auto top = manager->perm[F->index];

  auto* fl = Cudd_NotCond( cuddE( F ), Cudd_IsComplement( f ) );
  auto* fh = Cudd_NotCond( cuddT( F ), Cudd_IsComplement( f ) );
  auto* t1 = Cudd_bddOr( manager, fl, fh );
  if ( !t1 )
  {
    return nullptr;
  }
  cuddRef( t1 );

  auto* rh = bdd_up_rec( manager, t1 );
  if ( !rh )
  {
    Cudd_IterDerefBdd( manager, t1 );
    return nullptr;
  }
  cuddRef( rh );

  auto* rl = bdd_up_rec( manager, fl );
  if ( !rl )
  {
    Cudd_IterDerefBdd( manager, t1 );
    Cudd_IterDerefBdd( manager, rh );
  }
  cuddRef( rl );

  DdNode* r;
  if ( rl == rh )
  {
    r = rh;
  }
  else
  {
    if ( Cudd_IsComplement( rh ) )
    {
      r = cuddUniqueInter( manager, (int)top, Cudd_Not( rh ), Cudd_Not( rl ) );
      if ( !r )
      {
        Cudd_IterDerefBdd( manager, t1 );
        Cudd_IterDerefBdd( manager, rh );
        Cudd_IterDerefBdd( manager, rl );
        return nullptr;
      }
      r = Cudd_Not( r );
    }
    else
    {
      r = cuddUniqueInter( manager, (int)top, rh, rl );
      if ( !r )
      {
        Cudd_IterDerefBdd( manager, t1 );
        Cudd_IterDerefBdd( manager, rh );
        Cudd_IterDerefBdd( manager, rl );
        return nullptr;
      }
    }
  }
  cuddDeref( t1 );
  cuddDeref( rh );
  cuddDeref( rl );
  if ( F->ref != 1 )
  {
    cuddCacheInsert1( manager, bdd_up, f, r );
  }
  return r;
}

DdNode* bdd_up( DdManager *manager, DdNode *f )
{
  DdNode *res;

  do
  {
    manager->reordered = 0;
    res = bdd_up_rec( manager, f );
  }
  while ( manager->reordered == 1 );
  return res;
}

BDD bdd_up( Cudd& manager, const BDD& f )
{
  auto* r = bdd_up( f.manager(), f.getNode() );
  return BDD( manager, r );
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End: