/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file ored/model/modelparameter.hpp
    \brief class for holding model parameter data
    \ingroup models
*/

#pragma once

#include <ql/types.hpp>
#include <ored/model/lgmdata.hpp>
#include <ored/utilities/xmlutils.hpp>
#include <boost/optional.hpp>
#include <vector>

namespace ore {
namespace data {

/*! Abstract base class for holding model parameter data.
    \ingroup models
*/
class ModelParameter : public XMLSerializable {
public:
    //! Default constructor
    ModelParameter();

    //! Detailed constructor
    ModelParameter(bool calibrate,
        ParamType type,
        const std::vector<QuantLib::Time>& times,
        const std::vector<QuantLib::Real>& values);

    //! \name Inspectors
    //@{
    bool calibrate() const;
    ParamType type() const;
    const std::vector<QuantLib::Time>& times() const;
    const std::vector<QuantLib::Real>& values() const;
    //@}

    //! \name Serialisation
    //@{
    void fromXML(XMLNode* node) override;
    //@}

protected:
    //! Method used by toXML in derived classes to add the members here to a node.
    void append(XMLDocument& doc, XMLNode* node) const;

    //! Perform some checks on the parameters
    virtual void check() const;

private:
    bool calibrate_;
    ParamType type_;
    std::vector<QuantLib::Time> times_;
    std::vector<QuantLib::Real> values_;
};

/*! Volatility model parameter with optional volatility type.

    \note
    The volatility type is currently an LGM volatility type. We may want to broaden this in future.

    \ingroup models
*/
class VolatilityParameter : public ModelParameter {
public:
    //! Default constructor
    VolatilityParameter();

    //! Constructor for piecewise volatility with an explicit volatility type.
    VolatilityParameter(LgmData::VolatilityType volatilityType,
        bool calibrate,
        ParamType type,
        const std::vector<QuantLib::Time>& times,
        const std::vector<QuantLib::Real>& values);

    //! Constructor for constant volatility with an explicit volatility type.
    VolatilityParameter(LgmData::VolatilityType volatilityType,
        bool calibrate,
        QuantLib::Real value);

    //! Constructor for piecewise volatility without an explicit volatility type.
    VolatilityParameter(bool calibrate,
        ParamType type,
        const std::vector<QuantLib::Time>& times,
        const std::vector<QuantLib::Real>& values);

    //! Constructor for constant volatility without an explicit volatility type.
    VolatilityParameter(bool calibrate,
        QuantLib::Real value);

    //! \name Inspectors
    //@{
    const boost::optional<LgmData::VolatilityType>& volatilityType() const;
    //@}

    //! \name Serialisation
    //@{
    void fromXML(XMLNode* node) override;
    XMLNode* toXML(XMLDocument& doc) override;
    //@}

private:
    boost::optional<LgmData::VolatilityType> volatilityType_;
};

/*! Reversion model parameter with specified reversion type.

    \note
    The reversion type is currently an LGM reversion type. We may want to broaden this in future.

    \ingroup models
*/
class ReversionParameter : public ModelParameter {
public:
    //! Default constructor
    ReversionParameter();

    //! Constructor for piecewise reversion
    ReversionParameter(LgmData::ReversionType reversionType,
        bool calibrate,
        ParamType type,
        const std::vector<QuantLib::Time>& times,
        const std::vector<QuantLib::Real>& values);

    //! Constructor for constant reversion
    ReversionParameter(LgmData::ReversionType reversionType,
        bool calibrate,
        QuantLib::Real value);

    //! \name Inspectors
    //@{
    LgmData::ReversionType reversionType() const;
    //@}

    //! \name Serialisation
    //@{
    void fromXML(XMLNode* node) override;
    XMLNode* toXML(XMLDocument& doc) override;
    //@}

private:
    LgmData::ReversionType reversionType_;
};

}
}


#include "stl.h"

#include <boost/interprocess/file_mapping.hpp>
#include <boost/interprocess/mapped_region.hpp>
#include <fstream>

using namespace boost::interprocess;

std::vector<Triangle> LoadBinarySTL(std::string path) {
    file_mapping fm(path.c_str(), read_only);
    mapped_region mr(fm, read_only);
    uint8_t *src = (uint8_t *)mr.get_address();
    const int numBytes = mr.get_size();
    const int numTriangles = std::max(0, (numBytes - 84) / 50);
    const int numVertices = numTriangles * 3;
    std::vector<glm::vec3> positions(numVertices);
    auto dst = positions.data();
    src += 96;
    for (int i = 0; i < numTriangles; i++) {
        memcpy(dst, src, 36);
        src += 50;
        dst += 3;
    }
    std::vector<Triangle> triangles;
    triangles.reserve(numTriangles);
    for (int i = 0; i < positions.size(); i += 3) {
        triangles.emplace_back(
            positions[i+0],
            positions[i+1],
            positions[i+2]);
    }
    return triangles;
}

void SaveBinarySTL(std::string path, const std::vector<Triangle> &triangles) {
    const uint64_t numBytes = uint64_t(triangles.size()) * 50 + 84;

    {
        file_mapping::remove(path.c_str());
        std::filebuf fbuf;
        fbuf.open(path.c_str(),
            std::ios_base::in | std::ios_base::out | std::ios_base::trunc |
            std::ios_base::binary);
        fbuf.pubseekoff(numBytes - 1, std::ios_base::beg);
        fbuf.sputc(0);
    }

    file_mapping fm(path.c_str(), read_write);
    mapped_region mr(fm, read_write);
    uint8_t *dst = (uint8_t *)mr.get_address();

    const uint32_t count = triangles.size();
    memcpy(dst + 80, &count, 4);

    for (uint32_t i = 0; i < triangles.size(); i++) {
        const Triangle &t = triangles[i];
        const glm::vec3 normal = t.Normal();
        const uint64_t idx = 84 + i * 50;
        memcpy(dst + idx + 0, &normal, 12);
        memcpy(dst + idx + 12, &t.A(), 12);
        memcpy(dst + idx + 24, &t.B(), 12);
        memcpy(dst + idx + 36, &t.C(), 12);
    }
}


/*
 * Copyright Andrey Semashev 2020
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * https://www.boost.org/LICENSE_1_0.txt)
 */
/*!
 * \file rotl.hpp
 *
 * This header defines \c rotl algorithm, which rotates integer left.
 */

#ifndef BOOST_BIT_OPS_ROTATING_ROTL_HPP_INCLUDED_
#define BOOST_BIT_OPS_ROTATING_ROTL_HPP_INCLUDED_

#include <limits>
#include <boost/bit_ops/detail/config.hpp>
#include <boost/bit_ops/detail/type_traits/enable_if.hpp>
#include <boost/bit_ops/detail/type_traits/integral_constant.hpp>
#include <boost/bit_ops/detail/type_traits/is_integral.hpp>
#include <boost/bit_ops/detail/type_traits/is_unsigned.hpp>

#if defined(_MSC_VER)
#include <intrin.h>
#endif

namespace boost {
namespace bit_ops {

namespace detail {

#if defined(BOOST_BIT_OPS_DETAIL_HAS_ROTL8)

template< typename T >
inline T rotl(T value, unsigned int count, bit_ops::detail::integral_constant< unsigned int, 1u >) BOOST_NOEXCEPT
{
    return static_cast< T >(_rotl8(value, count));
}

#endif // defined(BOOST_BIT_OPS_DETAIL_HAS_ROTL8)

#if defined(BOOST_BIT_OPS_DETAIL_HAS_ROTL16)

template< typename T >
inline T rotl(T value, unsigned int count, bit_ops::detail::integral_constant< unsigned int, 2u >) BOOST_NOEXCEPT
{
    return static_cast< T >(_rotl16(value, count));
}

#endif // defined(BOOST_BIT_OPS_DETAIL_HAS_ROTL16)

#if defined(BOOST_BIT_OPS_DETAIL_HAS_ROTL)

template< typename T >
inline T rotl(T value, unsigned int count, bit_ops::detail::integral_constant< unsigned int, 4u >) BOOST_NOEXCEPT
{
    return static_cast< T >(_rotl(value, count));
}

#endif // defined(BOOST_BIT_OPS_DETAIL_HAS_ROTL)

#if defined(BOOST_BIT_OPS_DETAIL_HAS_ROTL64)

template< typename T >
inline T rotl(T value, unsigned int count, bit_ops::detail::integral_constant< unsigned int, 8u >) BOOST_NOEXCEPT
{
    return static_cast< T >(_rotl64(value, count));
}

#endif // defined(BOOST_BIT_OPS_DETAIL_HAS_ROTL64)

template< typename T, unsigned int N >
inline T rotl(T value, unsigned int count, bit_ops::detail::integral_constant< unsigned int, N >) BOOST_NOEXCEPT
{
    return static_cast< T >((value << count) | (value >> (std::numeric_limits< T >::digits - count)));
}

} // namespace detail

//! Returns \a value rotated left by \a count bits
template< typename T >
inline typename bit_ops::detail::enable_if<
    bit_ops::detail::is_integral< T >::value && bit_ops::detail::is_unsigned< T >::value,
    T
>::type rotl(T value, unsigned int count) BOOST_NOEXCEPT
{
    return bit_ops::detail::rotl(value, count, bit_ops::detail::integral_constant< unsigned int, sizeof(T) >());
}

} // namespace bit_ops
} // namespace boost

#endif // BOOST_BIT_OPS_ROTATING_ROTL_HPP_INCLUDED_


// test_equipartition.cpp - Test the equipartition algorithm
//
// Copyright (C) 2020- ECMWF.
//
// This software is licensed under the terms of the Apache Licence Version 2.0
// which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
//
// In applying this licence, ECMWF does not waive the privileges and immunities
// granted to it by virtue of its status as an intergovernmental organisation
// nor does it submit to any jurisdiction.
//
// Author:  Robin Hogan
// Email:   r.j.hogan@ecmwf.int

#include <adept_arrays.h>
#include <cmath>
#include <iostream>

#include "equipartition.h"

using namespace adept;

class TestEquipartition : public Equipartition {
public:
  TestEquipartition(int n) : npoints(n) {
    values = exp(linspace(-2,10.0,npoints));
  }
  ep_real calc_error(ep_real bound1, ep_real bound2) {
    total_comp_cost += (bound2-bound1);
    int ibound1 = std::ceil(bound1*(npoints-1));
    int ibound2 = std::floor(bound2*(npoints-1));
    return abs(sum(values(range(ibound1,ibound2)))
	       - (ibound2-ibound1+1)*values((ibound1+ibound2)/2));
  }

  int npoints;
  Vector values;
  ep_real total_comp_cost;

};

int main()
{
  adept::set_array_print_style(PRINT_STYLE_PLAIN);
  ep_real e_save;

  bool iverbose = false;

  int npoints = 1000000;

  TestEquipartition te(npoints);
  te.set_verbose(iverbose);
  te.set_partition_max_iterations(200);
  te.set_line_search_max_iterations(15);
  te.set_partition_tolerance(0.001);
  te.set_resolution(1.0/npoints);
  te.total_comp_cost = 0.0;

  for (int ic = 0; ic < 2; ++ic)
  {

    te.set_cubic_interpolation(ic == 1);
    std::cout << "TESTING EQUIPARTITION SPECIFYING N\n";
    int ni = 16;
    Vector bounds(ni+1);
    Vector error(ni);

    bounds = linspace(0.0, 1.0, ni+1);
    EpStatus status = te.equipartition_n(ni, bounds.data(), error.data());
    std::cout << "***STATUS*** " << ep_status_string(status) << "\n";

    std::cout << "bounds = " << bounds << "\n";
    std::cout << "error  = " << error  << "\n";
    std::cout << "  computational cost = " << te.total_comp_cost << "\n";
    ep_real mean_error, cost_fn;
    ep_real frac_std, frac_range;
    ep_stats(ni, error.data(), mean_error, cost_fn,
	     &frac_std, &frac_range);
    std::cout << "  mean error = " << mean_error
	      << "\n  cost function = " << cost_fn
	      << "\n  frac std = " << frac_std
	      << "\n  frac range = " << frac_range << "\n";
    e_save = error[0];
  }

  std::cout << "\n";

  if (0)
  {
    std::cout << "TESTING EQUIPARTITION SPECIFYING E\n";
    int ni_max = 100;
    std::vector<ep_real> bounds;
    std::vector<ep_real> error;
    int ni = ni_max;
    te.total_comp_cost = 0.0;
    EpStatus status = te.equipartition_e(e_save, 0.0, 1.0, ni,
					 bounds, error);
    std::cout << "***STATUS*** " << ep_status_string(status) << "\n";
    std::cout << "bounds = " << Vector(&bounds[0],dimensions(ni+1)) << "\n";
    std::cout << "error  = " << Vector(&error[0],dimensions(ni))  << "\n";
    std::cout << "  computational cost = " << te.total_comp_cost << "\n";
    ep_real mean_error, cost_fn;
    ep_real frac_std, frac_range;
    ep_stats(ni, &error[0], mean_error, cost_fn,
	     &frac_std, &frac_range);
    std::cout << "  mean error = " << mean_error
	      << "\n  cost function = " << cost_fn
	      << "\n  frac std = " << frac_std
	      << "\n  frac range = " << frac_range << "\n";
  }

}


#include "SimpleTriMesh.h"

#include <Eigen/StdVector>

#include <map>

void SimpleTriMesh::addVertex(double x, double y, double z)
{
	m_vertices.push_back(Eigen::Vector3d(x, y, z));
}
void SimpleTriMesh::addTri(int i1, int i2, int i3)
{
	m_triangles.push_back(Eigen::Vector3i(i1, i2, i3));
}

void SimpleTriMesh::setFromMatrices(const Eigen::MatrixXd &vertices, const Eigen::MatrixXi &tris)
{
	m_vertices.resize(vertices.cols());
	m_triangles.resize(tris.cols());

	for (int i = 0; i < (int)m_vertices.size(); i++)
	{
		m_vertices[i] = vertices.col(i);
	}
	for (int i = 0; i < (int)m_triangles.size(); i++)
	{
		m_triangles[i] = tris.col(i);
	}
}
void SimpleTriMesh::toMatrices(Eigen::MatrixXd &vertices, Eigen::MatrixXi &tris) const
{
	vertices.resize(3, m_vertices.size());
	tris.resize(3, m_triangles.size());

	for (int i = 0; i < (int)m_vertices.size(); i++)
	{
		vertices.col(i) = m_vertices[i];
	}
	for (int i = 0; i < (int)m_triangles.size(); i++)
	{
		tris.col(i) = m_triangles[i];
	}
}
class TriBaryCoords
{
public:
	TriBaryCoords(int triIdx, const Eigen::Vector2d &u)
		:m_triIdx(triIdx),
		m_u(u)
	{
	}

	int getTriIdx() const
	{
		return m_triIdx;
	}

	Eigen::Vector3d getN() const
	{
		Eigen::Vector3d N;
		N << 1.0 - m_u.sum(), m_u;
		return N;
	}

private:
	Eigen::Vector2d m_u;
	int m_triIdx;
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

void SimpleTriMesh::upsample(int nAdditionalPointsPerEdge)
{
	//the following implementation removes non used vertices

	int numPointsPerEdge = nAdditionalPointsPerEdge + 2;
	int numTrisPerOldFace = (numPointsPerEdge - 1)*(numPointsPerEdge - 1);
	int numTris = numTrisPerOldFace * this->m_triangles.size();

	std::vector<TriBaryCoords, Eigen::aligned_allocator<TriBaryCoords> > baryCoords;

	std::vector<int> oldVertexToBaryCoords(this->m_vertices.size(), -1);
	std::vector<std::map<int, int> > edgeToBaryCoords(this->m_vertices.size());

	auto getOldVertexIdx = [&oldVertexToBaryCoords, &baryCoords](int oldTriIdx, int v, const Eigen::Vector2d &u) {
		if (oldVertexToBaryCoords[v] == -1)
		{
			baryCoords.push_back(TriBaryCoords(oldTriIdx, u));
			oldVertexToBaryCoords[v] = baryCoords.size() - 1;
		}
		return oldVertexToBaryCoords[v];
	};
	auto getVertexIdxOnEdge = [numPointsPerEdge, &getOldVertexIdx, &baryCoords, &edgeToBaryCoords](
		int oldTriIdx, int v0, const Eigen::Vector2d &u0, int v1, const Eigen::Vector2d &u1, int col) {
		if (col == 0) return getOldVertexIdx(oldTriIdx, v0, u0);
		if (col == numPointsPerEdge - 1) return getOldVertexIdx(oldTriIdx, v1, u1);

		Eigen::Vector2d uCopy0 = u0;
		Eigen::Vector2d uCopy1 = u1;

		if (v0 > v1)
		{
			std::swap(v0, v1);
			Eigen::Vector2d ut = uCopy0;
			uCopy0 = uCopy1;
			uCopy1 = ut;
			col = numPointsPerEdge - col - 1;
		}

		if (edgeToBaryCoords[v0].find(v1) == edgeToBaryCoords[v0].end())
		{
			double lambda = col / double(numPointsPerEdge - 1);
			Eigen::Vector2d u = (1.0 - lambda) * uCopy0 + lambda * uCopy1;
			baryCoords.push_back(TriBaryCoords(oldTriIdx, u));
			edgeToBaryCoords[v0].emplace(v1, baryCoords.size() - 1);
		}

		return edgeToBaryCoords[v0].find(v1)->second;
	};

	auto getVertexIdx = [this, &getVertexIdxOnEdge, &baryCoords, numPointsPerEdge](int oldTriIdx, int row, int col) {
		const Eigen::Vector3i &tri = this->m_triangles[oldTriIdx];

		Eigen::Vector2d u0(0.0, 0.0);
		Eigen::Vector2d u1(1.0, 0.0);
		Eigen::Vector2d u2(0.0, 1.0);

		if (row == 0)
		{
			return getVertexIdxOnEdge(oldTriIdx, tri[0], u0, tri[1], u1, col);
		}
		if (col == 0)
		{
			return getVertexIdxOnEdge(oldTriIdx, tri[0], u0, tri[2], u2, row);
		}
		if (row - col == 0)
		{
			return getVertexIdxOnEdge(oldTriIdx, tri[1], u1, tri[2], u2, row);
		}

		int numPointsInRow = numPointsPerEdge - row;

		double lambda = row / double(numPointsPerEdge - 1);
		double lambda2 = col / double(numPointsInRow - 1);

		//Eigen::Vector2d p1 = (1.0 - lambda) * u0 + lambda * u2;
		//Eigen::Vector2d p2 = (1.0 - lambda) * u1 + lambda * u2;

		//Eigen::Vector2d u = (1.0 - lambda2) * p1 + lambda2 * p2;

		Eigen::Vector2d u(lambda2 * (1.0 - lambda), lambda);

		//if (numPointsInRow == 1) u = Eigen::Vector2d(0.0, lambda);

		baryCoords.push_back(TriBaryCoords(oldTriIdx, u));

		return (int)baryCoords.size() - 1;
	};

	Eigen::MatrixXi newFaces(3, numTris);

	for (int oldTriIdx = 0; oldTriIdx < this->m_triangles.size(); oldTriIdx++)
	{
		int ciTri = 0;
		for (int row = 0; row < numPointsPerEdge - 1; row++)
		{
			int numTrisInRow = numPointsPerEdge - 2 * row;
			for (int k = 0, baseVertex = 0; k < numTrisInRow; k++, baseVertex++)
			{
				Eigen::Vector3i newTri;
				newTri[0] = getVertexIdx(oldTriIdx, row, baseVertex);
				newTri[1] = getVertexIdx(oldTriIdx, row, baseVertex + 1);
				newTri[2] = getVertexIdx(oldTriIdx, row + 1, baseVertex);
				newFaces.col(oldTriIdx * numTrisPerOldFace + ciTri) = newTri;
				ciTri += 1;

				k++;
				if (k >= numTrisInRow) break;

				newTri[0] = getVertexIdx(oldTriIdx, row, baseVertex + 1);
				newTri[1] = getVertexIdx(oldTriIdx, row + 1, baseVertex + 1);
				newTri[2] = getVertexIdx(oldTriIdx, row + 1, baseVertex);
				newFaces.col(oldTriIdx * numTrisPerOldFace + ciTri) = newTri;
				ciTri += 1;
			}
		}
		assert(ciTri == numTrisPerOldFace);
	}

	Eigen::MatrixXd newVertices(3, baryCoords.size());
	for (int i = 0; i < baryCoords.size(); i++)
	{
		const Eigen::Vector3i &tri = this->m_triangles[baryCoords[i].getTriIdx()];
		Eigen::Vector3d N = baryCoords[i].getN();
		Eigen::Vector3d v = Eigen::Vector3d::Zero();
		for (int j = 0; j < 3; j++)
		{
			v += N[j] * this->m_vertices[tri[j]];
		}
		newVertices.col(i) = v;
	}

	setFromMatrices(newVertices, newFaces);
}

SimpleTriMesh SimpleTriMesh::icosphere()
{
	double t = (1.0 + std::sqrt(5.0)) / 2.0;
	double normalizationFactor = 1.0 / (1.0 + t*t);
	double u = t * normalizationFactor;
	double o = 1.0 * normalizationFactor;
	SimpleTriMesh mesh;

	mesh.addVertex(-o, u, 0);
	mesh.addVertex(o, u, 0);
	mesh.addVertex(-o, -u, 0);
	mesh.addVertex(o, -u, 0);

	mesh.addVertex(0, -o, u);
	mesh.addVertex(0, o, u);
	mesh.addVertex(0, -o, -u);
	mesh.addVertex(0, o, -u);

	mesh.addVertex(u, 0, -o);
	mesh.addVertex(u, 0, o);
	mesh.addVertex(-u, 0, -o);
	mesh.addVertex(-u, 0, o);

	mesh.addTri(0, 5, 1);
	mesh.addTri(0, 11, 5);
	mesh.addTri(0, 1, 7);
	mesh.addTri(0, 10, 11);
	mesh.addTri(0, 7, 10);

	mesh.addTri(3, 9, 4);
	mesh.addTri(3, 4, 2);
	mesh.addTri(3, 2, 6);
	mesh.addTri(3, 6, 8);
	mesh.addTri(3, 8, 9);

	mesh.addTri(1, 5, 9);
	mesh.addTri(7, 1, 8);
	mesh.addTri(10, 7, 6);
	mesh.addTri(5, 11, 4);
	mesh.addTri(11, 10, 2);

	mesh.addTri(4, 9, 5);
	mesh.addTri(9, 8, 1);
	mesh.addTri(6, 2, 10);
	mesh.addTri(2, 4, 11);
	mesh.addTri(8, 6, 7);

	return mesh;
}
SimpleTriMesh SimpleTriMesh::icosphere(int nSubdivisions)
{
	SimpleTriMesh mesh = SimpleTriMesh::icosphere();
	for (int i = 0; i < nSubdivisions; i++)
	{
		mesh.upsample(1);
		for (auto &vertex : mesh.vertices())
		{
			vertex.normalize();
		}
	}
	return mesh;
}


// test file for quaternion.hpp

//  (C) Copyright Hubert Holin 2001.
//  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 <iomanip>

#include <boost/mpl/list.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/test/unit_test_log.hpp>
#include <boost/test/test_case_template.hpp>

#include <boost/math/quaternion.hpp>

template<typename T>
struct string_type_name;

#define DEFINE_TYPE_NAME(Type)              \
template<> struct string_type_name<Type>    \
{                                           \
    static char const * _()                 \
    {                                       \
        return #Type;                       \
    }                                       \
}

DEFINE_TYPE_NAME(float);
DEFINE_TYPE_NAME(double);
DEFINE_TYPE_NAME(long double);

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
typedef boost::mpl::list<float,double,long double>  test_types;
#else
typedef boost::mpl::list<float,double>  test_types;
#endif

// Apple GCC 4.0 uses the "double double" format for its long double,
// which means that epsilon is VERY small but useless for
// comparisons. So, don't do those comparisons.
#if (defined(__APPLE_CC__) && defined(__GNUC__) && __GNUC__ == 4) || defined(BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS)
typedef boost::mpl::list<float,double>  near_eps_test_types;
#else
typedef boost::mpl::list<float,double,long double>  near_eps_test_types;
#endif

#if BOOST_WORKAROUND(__GNUC__, < 3)
    // gcc 2.x ignores function scope using declarations,
    // put them in the scope of the enclosing namespace instead:
using   ::std::sqrt;
using   ::std::atan;
using   ::std::log;
using   ::std::exp;
using   ::std::cos;
using   ::std::sin;
using   ::std::tan;
using   ::std::cosh;
using   ::std::sinh;
using   ::std::tanh;

using   ::std::numeric_limits;

using   ::boost::math::abs;
#endif  /* BOOST_WORKAROUND(__GNUC__, < 3) */

#ifdef  BOOST_NO_STDC_NAMESPACE
using   ::sqrt;
using   ::atan;
using   ::log;
using   ::exp;
using   ::cos;
using   ::sin;
using   ::tan;
using   ::cosh;
using   ::sinh;
using   ::tanh;
#endif  /* BOOST_NO_STDC_NAMESPACE */

#ifdef  BOOST_NO_ARGUMENT_DEPENDENT_LOOKUP
using   ::boost::math::real;
using   ::boost::math::unreal;
using   ::boost::math::sup;
using   ::boost::math::l1;
using   ::boost::math::abs;
using   ::boost::math::norm;
using   ::boost::math::conj;
using   ::boost::math::exp;
using   ::boost::math::pow;
using   ::boost::math::cos;
using   ::boost::math::sin;
using   ::boost::math::tan;
using   ::boost::math::cosh;
using   ::boost::math::sinh;
using   ::boost::math::tanh;
using   ::boost::math::sinc_pi;
using   ::boost::math::sinhc_pi;
#endif  /* BOOST_NO_ARGUMENT_DEPENDENT_LOOKUP */

// Provide standard floating point abs() overloads if older Microsoft
// library is used with _MSC_EXTENSIONS defined. This code also works
// for the Intel compiler using the Microsoft library.
#if defined(_MSC_EXTENSIONS) && BOOST_WORKAROUND(_MSC_VER, < 1310)
#if !((__INTEL__ && _WIN32) && BOOST_WORKAROUND(__MWERKS__, >= 0x3201))
inline float        abs(float v)
{
    return(fabs(v));
}

inline double        abs(double v)
{
    return(fabs(v));
}

inline long double    abs(long double v)
{
    return(fabs(v));
}
#endif /* !((__INTEL__ && _WIN32) && BOOST_WORKAROUND(__MWERKS__, >= 0x3201)) */
#endif /* defined(_MSC_EXTENSIONS) && BOOST_WORKAROUND(_MSC_VER, < 1310) */

// explicit (if ludicrous) instanciation
#if !BOOST_WORKAROUND(__GNUC__, < 3)
template    class ::boost::math::quaternion<int>;
#else
// gcc doesn't like the absolutely-qualified namespace
template class boost::math::quaternion<int>;
#endif /* !BOOST_WORKAROUND(__GNUC__) */

void    quaternion_manual_test()
{
    // tests for evaluation by humans

    // using default constructor
    ::boost::math::quaternion<float>        q0;

    ::boost::math::quaternion<float>        qa[2];

    // using constructor "H seen as R^4"
    ::boost::math::quaternion<double>       q1(1,2,3,4);

    ::std::complex<float>                   c0(5,6);

    // using constructor "H seen as C^2"
    ::boost::math::quaternion<float>        q2(c0);

    // using UNtemplated copy constructor
    ::boost::math::quaternion<float>        q3(q2);

    // using templated copy constructor
    ::boost::math::quaternion<long double>  q4(q3);

    // using UNtemplated assignment operator
    q3 = q0;
    qa[0] = q0;

    // using templated assignment operator
    q4 = q0;
    qa[1] = q1;

    float                                   f0(7);

    // using converting assignment operator
    q2 = f0;

    // using converting assignment operator
    q3 = c0;

    // using += (const T &)
    q2 += f0;

    // using += (const ::std::complex<T> &)
    q2 += c0;

    // using += (const quaternion<X> &)
    q2 += q3;

    // using -= (const T &)
    q3 -= f0;

    // using -= (const ::std::complex<T> &)
    q3 -= c0;

    // using -= (const quaternion<X> &)
    q3 -= q2;

    double                                  d0(8);
    ::std::complex<double>                  c1(9,10);

    // using *= (const T &)
    q1 *= d0;

    // using *= (const ::std::complex<T> &)
    q1 *= c1;

    // using *= (const quaternion<X> &)
    q1 *= q1;

    long double                             l0(11);
    ::std::complex<long double>             c2(12,13);

    // using /= (const T &)
    q4 /= l0;

    // using /= (const ::std::complex<T> &)
    q4 /= c2;

    // using /= (const quaternion<X> &)
    q4 /= q1;

    // using + (const T &, const quaternion<T> &)
    ::boost::math::quaternion<float>        q5 = f0+q2;

    // using + (const quaternion<T> &, const T &)
    ::boost::math::quaternion<float>        q6 = q2+f0;

    // using + (const ::std::complex<T> &, const quaternion<T> &)
    ::boost::math::quaternion<float>        q7 = c0+q2;

    // using + (const quaternion<T> &, const ::std::complex<T> &)
    ::boost::math::quaternion<float>        q8 = q2+c0;

    // using + (const quaternion<T> &,const quaternion<T> &)
    ::boost::math::quaternion<float>        q9 = q2+q3;

    // using - (const T &, const quaternion<T> &)
    q5 = f0-q2;

    // using - (const quaternion<T> &, const T &)
    q6 = q2-f0;

    // using - (const ::std::complex<T> &, const quaternion<T> &)
    q7 = c0-q2;

    // using - (const quaternion<T> &, const ::std::complex<T> &)
    q8 = q2-c0;

    // using - (const quaternion<T> &,const quaternion<T> &)
    q9 = q2-q3;

    // using * (const T &, const quaternion<T> &)
    q5 = f0*q2;

    // using * (const quaternion<T> &, const T &)
    q6 = q2*f0;

    // using * (const ::std::complex<T> &, const quaternion<T> &)
    q7 = c0*q2;

    // using * (const quaternion<T> &, const ::std::complex<T> &)
    q8 = q2*c0;

    // using * (const quaternion<T> &,const quaternion<T> &)
    q9 = q2*q3;

    // using / (const T &, const quaternion<T> &)
    q5 = f0/q2;

    // using / (const quaternion<T> &, const T &)
    q6 = q2/f0;

    // using / (const ::std::complex<T> &, const quaternion<T> &)
    q7 = c0/q2;

    // using / (const quaternion<T> &, const ::std::complex<T> &)
    q8 = q2/c0;

    // using / (const quaternion<T> &,const quaternion<T> &)
    q9 = q2/q3;

    // using + (const quaternion<T> &)
    q2 = +q0;

    // using - (const quaternion<T> &)
    q2 = -q3;

    // using == (const T &, const quaternion<T> &)
    f0 == q2;

    // using == (const quaternion<T> &, const T &)
    q2 == f0;

    // using == (const ::std::complex<T> &, const quaternion<T> &)
    c0 == q2;

    // using == (const quaternion<T> &, const ::std::complex<T> &)
    q2 == c0;

    // using == (const quaternion<T> &,const quaternion<T> &)
    q2 == q3;

    // using != (const T &, const quaternion<T> &)
    f0 != q2;

    // using != (const quaternion<T> &, const T &)
    q2 != f0;

    // using != (const ::std::complex<T> &, const quaternion<T> &)
    c0 != q2;

    // using != (const quaternion<T> &, const ::std::complex<T> &)
    q2 != c0;

    // using != (const quaternion<T> &,const quaternion<T> &)
    q2 != q3;

    BOOST_MESSAGE("Please input a quaternion...");

#ifdef BOOST_INTERACTIVE_TEST_INPUT_ITERATOR
    ::std::cin >> q0;

    if    (::std::cin.fail())
    {
        BOOST_MESSAGE("You have entered nonsense!");
    }
    else
    {
        BOOST_MESSAGE("You have entered the quaternion "<< q0 << " .");
    }
#else
    ::std::istringstream                bogus("(1,2,3,4)");

    bogus >> q0;

    BOOST_MESSAGE("You have entered the quaternion " << q0 << " .");
#endif

    BOOST_MESSAGE("For this quaternion:");

    BOOST_MESSAGE( "the value of the real part is "
                << real(q0));

    BOOST_MESSAGE( "the value of the unreal part is "
                << unreal(q0));

    BOOST_MESSAGE( "the value of the sup norm is "
                << sup(q0));

    BOOST_MESSAGE( "the value of the l1 norm is "
                << l1(q0));

    BOOST_MESSAGE( "the value of the magnitude (euclidian norm) is "
                << abs(q0));

    BOOST_MESSAGE( "the value of the (Cayley) norm is "
                << norm(q0));

    BOOST_MESSAGE( "the value of the conjugate is "
                << conj(q0));

    BOOST_MESSAGE( "the value of the exponential is "
                << exp(q0));

    BOOST_MESSAGE( "the value of the cube is "
                << pow(q0,3));

    BOOST_MESSAGE( "the value of the cosinus is "
                << cos(q0));

    BOOST_MESSAGE( "the value of the sinus is "
                << sin(q0));

    BOOST_MESSAGE( "the value of the tangent is "
                << tan(q0));

    BOOST_MESSAGE( "the value of the hyperbolic cosinus is "
                << cosh(q0));

    BOOST_MESSAGE( "the value of the hyperbolic sinus is "
                << sinh(q0));

    BOOST_MESSAGE( "the value of the hyperbolic tangent is "
                << tanh(q0));

#ifdef    BOOST_NO_TEMPLATE_TEMPLATES
    BOOST_MESSAGE("no template templates, can't compute cardinal functions");
#else    /* BOOST_NO_TEMPLATE_TEMPLATES */
    BOOST_MESSAGE( "the value of "
                << "the Sinus Cardinal (of index pi) is "
                << sinc_pi(q0));

    BOOST_MESSAGE( "the value of "
                << "the Hyperbolic Sinus Cardinal (of index pi) is "
                << sinhc_pi(q0));
#endif    /* BOOST_NO_TEMPLATE_TEMPLATES */

    BOOST_MESSAGE(" ");

    float                         rho = ::std::sqrt(8.0f);
    float                         theta = ::std::atan(1.0f);
    float                         phi1 = ::std::atan(1.0f);
    float                         phi2 = ::std::atan(1.0f);

    BOOST_MESSAGE( "The value of the quaternion represented "
                << "in spherical form by "
                << "rho = " << rho << " , theta = " << theta
                << " , phi1 = " << phi1 << " , phi2 = " << phi2
                << " is "
                << ::boost::math::spherical(rho, theta, phi1, phi2));

    float                         alpha = ::std::atan(1.0f);

    BOOST_MESSAGE( "The value of the quaternion represented "
                << "in semipolar form by "
                << "rho = " << rho << " , alpha = " << alpha
                << " , phi1 = " << phi1 << " , phi2 = " << phi2
                << " is "
                << ::boost::math::semipolar(rho, alpha, phi1, phi2));

    float                         rho1 = 1;
    float                         rho2 = 2;
    float                         theta1 = 0;
    float                         theta2 = ::std::atan(1.0f)*2;

    BOOST_MESSAGE( "The value of the quaternion represented "
                << "in multipolar form by "
                << "rho1 = " << rho1 << " , theta1 = " << theta1
                << " , rho2 = " << rho2 << " , theta2 = " << theta2
                << " is "
                << ::boost::math::multipolar(rho1, theta1, rho2, theta2));

    float                         t = 5;
    float                         radius = ::std::sqrt(2.0f);
    float                         longitude = ::std::atan(1.0f);
    float                         lattitude = ::std::atan(::std::sqrt(3.0f));

    BOOST_MESSAGE( "The value of the quaternion represented "
                << "in cylindrospherical form by "
                << "t = " << t << " , radius = " << radius
                << " , longitude = " << longitude << " , latitude = "
                << lattitude << " is "
                << ::boost::math::cylindrospherical(t, radius,
                        longitude, lattitude));

    float                         r = ::std::sqrt(2.0f);
    float                         angle = ::std::atan(1.0f);
    float                         h1 = 3;
    float                         h2 = 4;

    BOOST_MESSAGE( "The value of the quaternion represented "
                << "in cylindrical form by "
                << "r = " << r << " , angle = " << angle
                << " , h1 = " << h1 << " , h2 = " << h2
                << " is "
                << ::boost::math::cylindrical(r, angle, h1, h2));

    double                                   real_1(1);
    ::std::complex<double>                   complex_1(1);
    ::std::complex<double>                   complex_i(0,1);
    ::boost::math::quaternion<double>        quaternion_1(1);
    ::boost::math::quaternion<double>        quaternion_i(0,1);
    ::boost::math::quaternion<double>        quaternion_j(0,0,1);
    ::boost::math::quaternion<double>        quaternion_k(0,0,0,1);

    BOOST_MESSAGE(" ");

    BOOST_MESSAGE( "Real 1: " << real_1 << " ; "
                << "Complex 1: " << complex_1 << " ; "
                << "Quaternion 1: " << quaternion_1 << " .");

    BOOST_MESSAGE( "Complex i: " << complex_i << " ; "
                << "Quaternion i: " << quaternion_i << " .");

    BOOST_MESSAGE( "Quaternion j: " << quaternion_j << " .");

    BOOST_MESSAGE( "Quaternion k: " << quaternion_k << " .");

    BOOST_MESSAGE(" ");

    BOOST_MESSAGE( "i*i: " << quaternion_i*quaternion_i << " ; "
                << "j*j: " << quaternion_j*quaternion_j << " ; "
                << "k*k: " << quaternion_k*quaternion_k << " .");
    BOOST_MESSAGE( "i*j: " << quaternion_i*quaternion_j << " ; "
                << "j*i: " << quaternion_j*quaternion_i << " .");
    BOOST_MESSAGE( "j*k: " << quaternion_j*quaternion_k << " ; "
                << "k*j: " << quaternion_k*quaternion_j << " .");
    BOOST_MESSAGE( "k*i: " << quaternion_k*quaternion_i << " ; "
                << "i*k: " << quaternion_i*quaternion_k << " .");

    BOOST_MESSAGE(" ");
}

BOOST_TEST_CASE_TEMPLATE_FUNCTION(multiplication_test, T)
{
#if     BOOST_WORKAROUND(__GNUC__, < 3)
#else   /* BOOST_WORKAROUND(__GNUC__, < 3) */
    using ::std::numeric_limits;

    using ::boost::math::abs;
#endif  /* BOOST_WORKAROUND(__GNUC__, < 3) */

    BOOST_MESSAGE("Testing multiplication for "
        << string_type_name<T>::_() << ".");

    BOOST_REQUIRE_PREDICATE(::std::less_equal<T>(),
        (abs(::boost::math::quaternion<T>(1,0,0,0)*
             ::boost::math::quaternion<T>(1,0,0,0)-static_cast<T>(1)))
        (numeric_limits<T>::epsilon()));

    BOOST_REQUIRE_PREDICATE(::std::less_equal<T>(),
        (abs(::boost::math::quaternion<T>(0,1,0,0)*
             ::boost::math::quaternion<T>(0,1,0,0)+static_cast<T>(1)))
        (numeric_limits<T>::epsilon()));

    BOOST_REQUIRE_PREDICATE(::std::less_equal<T>(),
        (abs(::boost::math::quaternion<T>(0,0,1,0)*
             ::boost::math::quaternion<T>(0,0,1,0)+static_cast<T>(1)))
        (numeric_limits<T>::epsilon()));

    BOOST_REQUIRE_PREDICATE(::std::less_equal<T>(),
        (abs(::boost::math::quaternion<T>(0,0,0,1)*
             ::boost::math::quaternion<T>(0,0,0,1)+static_cast<T>(1)))
        (numeric_limits<T>::epsilon()));

    BOOST_REQUIRE_PREDICATE(::std::less_equal<T>(),
        (abs(::boost::math::quaternion<T>(0,1,0,0)*
             ::boost::math::quaternion<T>(0,0,1,0)-
             ::boost::math::quaternion<T>(0,0,0,1)))
        (numeric_limits<T>::epsilon()));

    BOOST_REQUIRE_PREDICATE(::std::less_equal<T>(),
        (abs(::boost::math::quaternion<T>(0,0,1,0)*
             ::boost::math::quaternion<T>(0,1,0,0)+
             ::boost::math::quaternion<T>(0,0,0,1)))
        (numeric_limits<T>::epsilon()));

    BOOST_REQUIRE_PREDICATE(::std::less_equal<T>(),
        (abs(::boost::math::quaternion<T>(0,0,1,0)*
             ::boost::math::quaternion<T>(0,0,0,1)-
             ::boost::math::quaternion<T>(0,1,0,0)))
        (numeric_limits<T>::epsilon()));

    BOOST_REQUIRE_PREDICATE(::std::less_equal<T>(),
        (abs(::boost::math::quaternion<T>(0,0,0,1)*
             ::boost::math::quaternion<T>(0,0,1,0)+
             ::boost::math::quaternion<T>(0,1,0,0)))
        (numeric_limits<T>::epsilon()));

    BOOST_REQUIRE_PREDICATE(::std::less_equal<T>(),
        (abs(::boost::math::quaternion<T>(0,0,0,1)*
             ::boost::math::quaternion<T>(0,1,0,0)-
             ::boost::math::quaternion<T>(0,0,1,0)))
        (numeric_limits<T>::epsilon()));

    BOOST_REQUIRE_PREDICATE(::std::less_equal<T>(),
        (abs(::boost::math::quaternion<T>(0,1,0,0)*
             ::boost::math::quaternion<T>(0,0,0,1)+
             ::boost::math::quaternion<T>(0,0,1,0)))
        (numeric_limits<T>::epsilon()));
}

BOOST_TEST_CASE_TEMPLATE_FUNCTION(exp_test, T)
{
#if     BOOST_WORKAROUND(__GNUC__, < 3)
#else   /* BOOST_WORKAROUND(__GNUC__, < 3) */
    using ::std::numeric_limits;

    using ::std::atan;

    using ::boost::math::abs;
#endif  /* BOOST_WORKAROUND(__GNUC__, < 3) */

    BOOST_MESSAGE("Testing exp for "
        << string_type_name<T>::_() << ".");

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(exp(::boost::math::quaternion<T>
             (0,4*atan(static_cast<T>(1)),0,0))+static_cast<T>(1)))
        (2*numeric_limits<T>::epsilon()));

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(exp(::boost::math::quaternion<T>
             (0,0,4*atan(static_cast<T>(1)),0))+static_cast<T>(1)))
        (2*numeric_limits<T>::epsilon()));

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(exp(::boost::math::quaternion<T>
             (0,0,0,4*atan(static_cast<T>(1))))+static_cast<T>(1)))
        (2*numeric_limits<T>::epsilon()));
}

BOOST_TEST_CASE_TEMPLATE_FUNCTION(cos_test, T)
{
#if     BOOST_WORKAROUND(__GNUC__, < 3)
#else   /* BOOST_WORKAROUND(__GNUC__, < 3) */
    using ::std::numeric_limits;

    using ::std::log;

    using ::boost::math::abs;
#endif  /* BOOST_WORKAROUND(__GNUC__, < 3) */

    BOOST_MESSAGE("Testing cos for "
        << string_type_name<T>::_() << ".");

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(static_cast<T>(4)*cos(::boost::math::quaternion<T>
             (0,log(static_cast<T>(2)),0,0))-static_cast<T>(5)))
        (4*numeric_limits<T>::epsilon()));

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(static_cast<T>(4)*cos(::boost::math::quaternion<T>
             (0,0,log(static_cast<T>(2)),0))-static_cast<T>(5)))
        (4*numeric_limits<T>::epsilon()));

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(static_cast<T>(4)*cos(::boost::math::quaternion<T>
             (0,0,0,log(static_cast<T>(2))))-static_cast<T>(5)))
        (4*numeric_limits<T>::epsilon()));
}

BOOST_TEST_CASE_TEMPLATE_FUNCTION(sin_test, T)
{
#if     BOOST_WORKAROUND(__GNUC__, < 3)
#else   /* BOOST_WORKAROUND(__GNUC__, < 3) */
    using ::std::numeric_limits;

    using ::std::log;

    using ::boost::math::abs;
#endif  /* BOOST_WORKAROUND(__GNUC__, < 3) */

    BOOST_MESSAGE("Testing sin for "
        << string_type_name<T>::_() << ".");

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(static_cast<T>(4)*sin(::boost::math::quaternion<T>
             (0,log(static_cast<T>(2)),0,0))
             -::boost::math::quaternion<T>(0,3,0,0)))
        (4*numeric_limits<T>::epsilon()));

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(static_cast<T>(4)*sin(::boost::math::quaternion<T>
             (0,0,log(static_cast<T>(2)),0))
             -::boost::math::quaternion<T>(0,0,3,0)))
        (4*numeric_limits<T>::epsilon()));

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(static_cast<T>(4)*sin(::boost::math::quaternion<T>
             (0,0,0,log(static_cast<T>(2))))
             -::boost::math::quaternion<T>(0,0,0,3)))
        (4*numeric_limits<T>::epsilon()));
}

BOOST_TEST_CASE_TEMPLATE_FUNCTION(cosh_test, T)
{
#if     BOOST_WORKAROUND(__GNUC__, < 3)
#else   /* BOOST_WORKAROUND(__GNUC__, < 3) */
    using ::std::numeric_limits;

    using ::std::atan;

    using ::boost::math::abs;
#endif  /* BOOST_WORKAROUND(__GNUC__, < 3) */

    BOOST_MESSAGE("Testing cosh for "
        << string_type_name<T>::_() << ".");

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(cosh(::boost::math::quaternion<T>
             (0,4*atan(static_cast<T>(1)),0,0))
             +static_cast<T>(1)))
        (4*numeric_limits<T>::epsilon()));

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(cosh(::boost::math::quaternion<T>
             (0,0,4*atan(static_cast<T>(1)),0))
             +static_cast<T>(1)))
        (4*numeric_limits<T>::epsilon()));

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(cosh(::boost::math::quaternion<T>
             (0,0,0,4*atan(static_cast<T>(1))))
             +static_cast<T>(1)))
        (4*numeric_limits<T>::epsilon()));
}

BOOST_TEST_CASE_TEMPLATE_FUNCTION(sinh_test, T)
{
#if     BOOST_WORKAROUND(__GNUC__, < 3)
#else   /* BOOST_WORKAROUND(__GNUC__, < 3) */
    using ::std::numeric_limits;

    using ::std::atan;

    using ::boost::math::abs;
#endif  /* BOOST_WORKAROUND(__GNUC__, < 3) */

    BOOST_MESSAGE("Testing sinh for "
        << string_type_name<T>::_() << ".");

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(sinh(::boost::math::quaternion<T>
             (0,2*atan(static_cast<T>(1)),0,0))
             -::boost::math::quaternion<T>(0,1,0,0)))
        (4*numeric_limits<T>::epsilon()));

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(sinh(::boost::math::quaternion<T>
             (0,0,2*atan(static_cast<T>(1)),0))
             -::boost::math::quaternion<T>(0,0,1,0)))
        (4*numeric_limits<T>::epsilon()));

    BOOST_CHECK_PREDICATE(::std::less_equal<T>(),
        (abs(sinh(::boost::math::quaternion<T>
             (0,0,0,2*atan(static_cast<T>(1))))
             -::boost::math::quaternion<T>(0,0,0,1)))
        (4*numeric_limits<T>::epsilon()));
}

boost::unit_test_framework::test_suite *    init_unit_test_suite(int, char *[])
{
    ::boost::unit_test::unit_test_log.
        set_threshold_level(::boost::unit_test::log_messages);

    boost::unit_test_framework::test_suite *    test =
        BOOST_TEST_SUITE("quaternion_test");

    BOOST_MESSAGE("Results of quaternion test.");
    BOOST_MESSAGE(" ");
    BOOST_MESSAGE("(C) Copyright Hubert Holin 2003-2005.");
    BOOST_MESSAGE("Distributed under the Boost Software License, Version 1.0.");
    BOOST_MESSAGE("(See accompanying file LICENSE_1_0.txt or copy at");
    BOOST_MESSAGE("http://www.boost.org/LICENSE_1_0.txt)");
    BOOST_MESSAGE(" ");

#define    BOOST_QUATERNION_COMMON_GENERATOR(fct)   \
    test->add(BOOST_TEST_CASE_TEMPLATE(fct##_test, test_types));

#define    BOOST_QUATERNION_COMMON_GENERATOR_NEAR_EPS(fct)   \
    test->add(BOOST_TEST_CASE_TEMPLATE(fct##_test, near_eps_test_types));

#define    BOOST_QUATERNION_TEST                      \
    BOOST_QUATERNION_COMMON_GENERATOR(multiplication) \
    BOOST_QUATERNION_COMMON_GENERATOR_NEAR_EPS(exp)            \
    BOOST_QUATERNION_COMMON_GENERATOR_NEAR_EPS(cos)            \
    BOOST_QUATERNION_COMMON_GENERATOR_NEAR_EPS(sin)            \
    BOOST_QUATERNION_COMMON_GENERATOR_NEAR_EPS(cosh)           \
    BOOST_QUATERNION_COMMON_GENERATOR_NEAR_EPS(sinh)

    BOOST_QUATERNION_TEST

#undef    BOOST_QUATERNION_TEST

#undef    BOOST_QUATERNION_COMMON_GENERATOR
#undef BOOST_QUATERNION_COMMON_GENERATOR_NEAR_EPS

#ifdef BOOST_QUATERNION_TEST_VERBOSE

    test->add(BOOST_TEST_CASE(quaternion_manual_test));

#endif    /* BOOST_QUATERNION_TEST_VERBOSE */

    return test;
}

#undef DEFINE_TYPE_NAME


#ifndef __PARTICLE_FILTER_HPP
#define __PARTICLE_FILTER_HPP

#include <boost/random/discrete_distribution.hpp>

#include "particle_filter_base.hpp"

template <size_t Size>
ParticleFilter<Size>::ParticleFilter(const PredictionModel<Size> &pm,
				     const LikelihoodModel<Size> &lm,
				     unsigned int nParticles,
				     const double *initState):
  pm(pm),
  lm(lm),
  particles(std::vector<Particle<Size> >(nParticles)),
  epoch(0)
{
  if (initState)
  {
    pm.init(initState, particles);
  }
  else
  {
    double *_initState = new double[Size];
    std::fill(_initState, _initState+Size, 0);
    pm.init(initState, particles);
    delete []_initState;
  }
}

template <size_t Size>
const std::vector<Particle<Size> > &ParticleFilter<Size>::getParticles() const
{
  return particles;
}

template <size_t Size>
const Particle<Size> &ParticleFilter<Size>::getBestParticle() const
{

  double bestLikelihood = particles.at(0).likelihood;
  int bestID = 0;

  for (typename std::vector<Particle<Size> >::const_iterator it=particles.begin()+1;
       it!=particles.end(); ++it)
  {
    const Particle<Size> &p = *it;
    if (p.likelihood>bestLikelihood)
    {
      bestLikelihood = p.likelihood;
      bestID = it-particles.begin();
    }
  }

  return particles.at(bestID);
}

template <size_t Size>
void ParticleFilter<Size>::step(double deltaT)
{
  // Create a copy of the current particles
  std::vector<Particle<Size> > tempParticles(particles.size());
  std::copy(particles.begin(), particles.end(), tempParticles.begin());

  // Particle filter iteration
  // - update the particles state using the provided state-update model
  pm.update(tempParticles, deltaT);

  // - compute the importance weights using the provided likelihood model
  lm.eval(tempParticles);

  // Normalize the particles weight (and build the list of normalized weights
  // used in the next selection step)
  double wSum = 0.;
  for (typename std::vector<Particle<Size> >::iterator it=tempParticles.begin();
       it!=tempParticles.end(); ++it)
  {
    const Particle<Size> &p = *it;
    wSum += p.likelihood;
  }

  std::vector<double> normW;
  for (typename std::vector<Particle<Size> >::iterator it=tempParticles.begin();
       it!=tempParticles.end(); ++it)
  {
    Particle<Size> &p = *it;
    p.likelihood /= wSum;
    normW.push_back(p.likelihood);
  }

  // - selection step
  // TODO: parameterize at which epoch execute selection
  //if (!(epoch%10))
  if (true)
  {
    boost::random::discrete_distribution<> D(normW.begin(), normW.end());

    std::vector<int> idx;
    for (int i=0; i<tempParticles.size(); i++) idx.push_back(D(rng));

    // Copy back the selected particles
    for (int i=0; i<idx.size(); i++) particles.at(i) = tempParticles.at(idx.at(i));
  }
  else
  {
    std::copy(tempParticles.begin(), tempParticles.end(), particles.begin());
  }

  epoch++;

  // Done
}

#endif // __PARTICLE_FILTER_HPP


/*
 * 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 https://github.com/jjackowski/duds/blob/master/LICENSE.
 * 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) 2017  Jeff Jackowski
 */
#include <duds/hardware/devices/instruments/LSM9DS1.hpp>
#include <duds/hardware/interface/linux/DevI2c.hpp>
#include <duds/hardware/interface/I2cErrors.hpp>
#include <duds/hardware/devices/displays/HD44780.hpp>
#include <duds/hardware/devices/displays/TextDisplayStream.hpp>
#include <duds/hardware/interface/linux/SysFsPort.hpp>
#include <duds/hardware/interface/ChipPinSelectManager.hpp>
#include <duds/general/IntegerBiDirIterator.hpp>
#include <iostream>
#include <thread>
#include <iomanip>
#include <cmath>
#include <boost/exception/diagnostic_information.hpp>
#include <Eigen/Geometry>
#include <boost/program_options.hpp>

bool quit = false;

namespace displays = duds::hardware::devices::displays;

// https://www.hindawi.com/journals/js/2010/967245/
// https://github.com/kriswiner/MPU6050/wiki/Simple-and-Effective-Magnetometer-Calibration

// ----------------------------------------------------------------------------
// simplistic calibration

const duds::hardware::devices::instruments::LSM9DS1::Settings calconfig = {
	/* .accelerometer = */ 0,
	/* .gyroscope = */ 0,
	/* .magnetometer = */ 1,
	/* .accelRange = */ duds::hardware::devices::instruments::LSM9DS1::AccelRange2g,
	/* .gyroRange = */ duds::hardware::devices::instruments::LSM9DS1::GyroRange4p276rps,
	/* .magRange = */ duds::hardware::devices::instruments::LSM9DS1::MagRange400uT,
	/* .gyroLowPower = */ 1,
	/* .gyroHighPass = */ 0,
	/* .magLowPower = */ 0,
	/* .xyMagMode = */ duds::hardware::devices::instruments::LSM9DS1::AxesHighPerformance,
	/* .zMagMode = */ duds::hardware::devices::instruments::LSM9DS1::AxesHighPerformance,
	/* .magTempComp = */ 0
};

void runcalibrationtest(
	duds::hardware::devices::instruments::LSM9DS1 &acclgyromag,
	displays::TextDisplayStream &tds,
	Eigen::Vector3d bias,
	Eigen::Vector3d scale
)
try {
	//std::vector<Eigen::Vector3d> magvecs(8000);
	Eigen::Vector3d minS(256, 256, 256), maxS(-256, -256, -256);
	duds::hardware::devices::instruments::LSM9DS1::ConvertedSample sample;
	std::chrono::milliseconds delay = std::chrono::milliseconds(8);
	acclgyromag.configure(0, 80.0f, calconfig);
	std::cout << "Begin calibration" << std::endl;
	tds << "Begin calibration\n";
	std::this_thread::sleep_for(std::chrono::seconds(2));
	std::cout << "\tnow!" << std::endl;
	tds << "  now!\n";
	for (int pos = 0; pos < 8192; ++pos) {
		while (1) {
			try {
				while (!acclgyromag.sample()) {
					std::this_thread::sleep_for(std::chrono::milliseconds(2));
				}
				break;
			} catch (duds::hardware::interface::I2cErrorNoDevice &) {
				acclgyromag.configure(0, 80.0f, calconfig);
				std::this_thread::sleep_for(std::chrono::milliseconds(32));
			}
		}
		acclgyromag.magnetometerQuantity(sample);
		//magvecs[pos](0) = sample.x();
		//magvecs[pos](1) = sample.y();
		//magvecs[pos](2) = sample.z();
		if (sample.x() > maxS(0)) {
			maxS(0) = sample.x();
		}
		if (sample.y() > maxS(1)) {
			maxS(1) = sample.y();
		}
		if (sample.z() > maxS(2)) {
			maxS(2) = sample.z();
		}
		if (sample.x() < minS(0)) {
			minS(0) = sample.x();
		}
		if (sample.y() < minS(1)) {
			minS(1) = sample.y();
		}
		if (sample.z() < minS(2)) {
			minS(2) = sample.z();
		}
		if ((pos % 256) == 0) {
			tds << "Sample " << pos << '\r';
			std::cout << "Sample " << pos << '\r';
			std::cout.flush();
		}
		std::this_thread::sleep_for(delay);
	}
	tds << displays::clear;
	bias = (maxS + minS) / 2.0;
	scale = (maxS - minS) / 2.0;
	double avgscl = (scale(0) + scale(1) + scale(2)) / 3.0;
	scale = scale.cwiseInverse() * avgscl;
	std::cout << "Calibration complete\n\tbias: " <<
	bias(0) << ", " << bias(1) << ", " << bias(2) << "\n\tscale: " <<
	scale(0) << ", " << scale(1) << ", " << scale(2) << std::endl;
} catch (...) {
	std::cerr << "Program failed in runcalibrationtest(): " <<
	boost::current_exception_diagnostic_information() << std::endl;
}

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

void makeHorizontal(
	Eigen::Vector3d &res,
	const Eigen::Vector3d &grav,
	const Eigen::Vector3d &mod,
	float &angle
) {
	static const Eigen::Vector3d z(0, 0, 1);
	Eigen::Vector3d g(grav.normalized());
	angle = std::acos(g.dot(z));
	Eigen::Vector3d axis = g.cross(z);
	axis.normalize();
	Eigen::Quaternion<double> q(Eigen::AngleAxisd(angle, axis));
	res = q * mod;
}

float heading(const Eigen::Vector3d &dir) {
	/*
	static const Eigen::Vector3d fwd(0, 1, 0);
	Eigen::Vector3d d(dir(0), dir(1), 0);
	d.normalize();
	Eigen::Vector3d a = d.cross(fwd);
	return std::asin(a.norm());
	*/
	float h = std::atan2(dir(0), dir(1));
	if (h < 0) {
		h += 2 * M_PI;
	}
	return h;
}

std::ostream &operator << (std::ostream &os, const Eigen::Vector3d &v) {
	Eigen::Vector3d n(v.normalized());
	n *= 99.0f;
	os << std::fixed << std::setprecision(0) << std::setw(3) << n(0) << ' ' <<
	std::setw(3) << n(1) << ' ' << std::setw(3) << n(2);
	return os;
}

void runtest(
	duds::hardware::devices::instruments::LSM9DS1 &acclgyromag,
	displays::TextDisplayStream &tds,
	const Eigen::Vector3d &bias,
	const Eigen::Vector3d &scale,
	int htx, int hty, int htz
)
try {
	static const Eigen::Vector3f z(0, 0, 1), htest(htx, hty, htz);
	duds::hardware::devices::instruments::LSM9DS1::RawSample rsA, rsM;
	duds::hardware::devices::instruments::LSM9DS1::ConvertedSample csA, csM;
	acclgyromag.start();
	std::cout << std::fixed;
	// doesn't account for time spent in loop
	std::chrono::milliseconds delay = std::chrono::milliseconds(
		(int)(1000.0f / /*acclgyromag.sampleRate()*/ 5.0f)
	);
	std::this_thread::sleep_for(delay);
	do {
		try {
			while (!acclgyromag.sample() && !quit) {
				std::this_thread::sleep_for(std::chrono::milliseconds(4));
			}
		} catch (...) {
			std::cerr << "Sample failure in runtest(): " <<
			boost::current_exception_diagnostic_information() << std::endl;
		}
		rsA = acclgyromag.rawAccelerometer();
		rsM = acclgyromag.rawMagnetometer();
		acclgyromag.accelerometerQuantity(csA);
		acclgyromag.magnetometerQuantity(csM);
		//Eigen::Vector3f mF(rsM.x, rsM.y, rsM.z);
		Eigen::Vector3d m(csM.x(), csM.y(), csM.z()), tmpM;
		tmpM = (m - bias); // .cwiseProduct(scale);
		m = tmpM.cwiseProduct(scale);
		//Eigen::Vector3f g(rsA.x, rsA.y, rsA.z);
		Eigen::Vector3d g(csA.x(), csA.y(), csA.z());
		Eigen::Vector3d mT;
		std::cout << "A: " << std::fixed << std::setprecision(4) << std::setw(8) << g.norm() << ' ' <<
		std::setw(6) << csA.x() << ", " <<
		std::setw(6) << csA.y() << ", " << std::setw(6) << csA.z();
		float th;
		makeHorizontal(mT, g, m, th);
		float head = heading(/*mT*/ m);
		std::cout << " th " << std::setw(5) << (th * 180.0f / M_PI);
		// magnetometer vector modified to have X-Y plane perpindicular to the
		// gravity vector
		std::cout << "   M: " << std::scientific << std::setprecision(6) << std::setw(9) << mT.norm() << ' ' <<
		std::setw(9) << mT(0) << ", " << std::setw(9) << mT(1) << ", " <<
		std::setw(9) << mT(2) << "  h: " << std::setw(5) << std::setprecision(1) << std::fixed <<
		(head * 180.0f / M_PI) << "  \r";// std::endl;
		/* unmodified magnetometer vector
		std::cout << "   Mag: " << std::setw(7) << m.norm() << ' ' <<
		std::setw(5) << rsM.x << ", " <<
		std::setw(5) << rsM.y << ", " << std::setw(5) << rsM.z << " \r";// std::endl;
		*/
		std::cout.flush();

		//float bozo;
		//makeHorizontal(mT, g, htest, bozo);

		// LCD out
		tds << displays::move(0,0) << "Up:" << std::fixed << std::setprecision(0) << std::setw(4) <<
		(th * 180.0f / M_PI) << " H:" << std::setprecision(0) << std::setw(4) <<
		(head * 180.0f / M_PI) << ',' << std::setprecision(0) << std::setw(4) <<
		(heading(mT) * 180.0f / M_PI) << displays::startLine << "Grav    " <<
		g << displays::startLine << "Mag     " << m << displays::startLine <<
		"Mag mod " << mT << displays::startLine;
		//"Testvec " << mT << displays::startLine;

		//tds << displays::move(0, 0);
		std::this_thread::sleep_for(delay);
	} while (!quit);
} catch (...) {
	std::cerr << "Program failed in runtest(): " <<
	boost::current_exception_diagnostic_information() << std::endl;
}

const duds::hardware::devices::instruments::LSM9DS1::Settings config = {
	/* .accelerometer = */ 1,
	/* .gyroscope = */ 0,
	/* .magnetometer = */ 1,
	/* .accelRange = */ duds::hardware::devices::instruments::LSM9DS1::AccelRange2g,
	/* .gyroRange = */ duds::hardware::devices::instruments::LSM9DS1::GyroRange4p276rps,
	/* .magRange = */ duds::hardware::devices::instruments::LSM9DS1::MagRange400uT,
	/* .gyroLowPower = */ 1,
	/* .gyroHighPass = */ 0,
	/* .magLowPower = */ 0,
	/* .xyMagMode = */ duds::hardware::devices::instruments::LSM9DS1::AxesHighPerformance,
	/* .zMagMode = */ duds::hardware::devices::instruments::LSM9DS1::AxesHighPerformance,
	/* .magTempComp = */ 0
};

typedef duds::general::IntegerBiDirIterator<unsigned int>  uintIterator;

int main(int argc, char *argv[])
try {
	std::string i2cpath;
	int htx = 1, hty = 0, htz = 0;
	{ // option parsing
		boost::program_options::options_description optdesc("Options");
		optdesc.add_options()
			( // help info
				"help,h",
				"Show this help message"
			)
			( // the I2C device file to use
				"i2cdev,i",
				boost::program_options::value<std::string>(&i2cpath)->
					default_value("/dev/i2c-1"),
				"Specify I2C device file"
			)
			(",x",boost::program_options::value<int>(&htx),"X test vec")
			(",y",boost::program_options::value<int>(&hty),"Y test vec")
			(",z",boost::program_options::value<int>(&htz),"Z test vec")
			//("bx",boost::program_options::value<int>(&htz),"X bias")
		;
		boost::program_options::variables_map vm;
		boost::program_options::store(
			boost::program_options::parse_command_line(argc, argv, optdesc),
			vm
		);
		boost::program_options::notify(vm);
		if (vm.count("help")) {
			std::cout << "Accelerometer and magnetometer test\n" << argv[0]
			<< " [options]\n" << optdesc << std::endl;
			return 0;
		}
	}

	// Setup for LSM9DS1
	std::unique_ptr<duds::hardware::interface::I2c> magI2c(
		new duds::hardware::interface::linux::DevI2c(i2cpath, 0x1E)
	);
	std::unique_ptr<duds::hardware::interface::I2c> accelI2c(
		new duds::hardware::interface::linux::DevI2c(i2cpath, 0x6B)
	);
	duds::hardware::devices::instruments::LSM9DS1 acclgyromag(accelI2c, magI2c);

	// Setup for LCD
	//                       LCD pins:  4  5   6   7  RS   E
	std::vector<unsigned int> gpios = { 5, 6, 19, 26, 20, 21 };
	std::shared_ptr<duds::hardware::interface::linux::SysFsPort> port =
		std::make_shared<duds::hardware::interface::linux::SysFsPort>(
			gpios, 0
		);
	// select pin
	std::unique_ptr<duds::hardware::interface::DigitalPinAccess> selacc =
		port->access(5); // gpio 21
	std::shared_ptr<duds::hardware::interface::ChipPinSelectManager> selmgr =
		std::make_shared<duds::hardware::interface::ChipPinSelectManager>(
			selacc
		);
	assert(!selacc);
	duds::hardware::interface::ChipSelect lcdsel(selmgr, 1);
	// set for LCD data
	gpios.clear();
	gpios.insert(gpios.begin(), uintIterator(0), uintIterator(5));
	duds::hardware::interface::DigitalPinSet lcdset(port, gpios);
	// display object
	std::shared_ptr<displays::HD44780> tmd =
		std::make_shared<displays::HD44780>(
			lcdset, lcdsel, 20, 4
		);
	tmd->initialize();
	displays::TextDisplayStream tds(tmd);

	Eigen::Vector3d bias(-7.147e-06, 4.1335e-05, 5.4474e-05);
	Eigen::Vector3d scale(1.00074, 0.977949, 1.02229);
	runcalibrationtest(acclgyromag, tds, bias, scale);

	acclgyromag.configure(5.0f, 5.0f, config);

	std::cout.precision(4);
	//std::cout << "Sampling frequency reported as " << acclgyromag.sampleRate() <<
	//"Hz" << std::endl;
	std::thread doit(
		runtest,
		std::ref(acclgyromag),
		std::ref(tds),
		std::ref(bias),
		std::ref(scale),
		htx,
		hty,
		htz
	);
	std::cin.get();
	quit = true;
	doit.join();
	std::cout << std::endl;
	return 0;
} catch (...) {
	std::cerr << "Program failed in main(): " <<
	boost::current_exception_diagnostic_information() << std::endl;
	return 1;
}


/* ************************************************************************
 * Copyright 2015 Advanced Micro Devices, Inc.
 *
 * 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.
 * ************************************************************************ */

#include <gtest/gtest.h>
#include <vector>
#include <string>
#include <boost/program_options.hpp>

#include "clSPARSE.hpp"
#include "resources/clsparse_environment.h"
#include "resources/csr_matrix_environment.h"
#include "resources/matrix_utils.h"

clsparseControl ClSparseEnvironment::control = NULL;
cl_command_queue ClSparseEnvironment::queue = NULL;
cl_context ClSparseEnvironment::context = NULL;

static cl_bool explicit_zeroes = true;

/** Just a simple test checking if the io functions for matrices are ok */

namespace po = boost::program_options;

std::string path;

void generateReference( const std::vector<float>& x,
                        const float alpha,
                        std::vector<float>& y,
                        const float beta )
{
    using CSRE = CSREnvironment;

    csrmv( CSRE::n_rows, CSRE::n_cols, CSRE::n_vals,
           CSRE::row_offsets, CSRE::col_indices, CSRE::f_values,
           x, alpha, y, beta );
}

TEST( MM_file, load )
{
    using CLSE = ClSparseEnvironment;

    // Read sparse data from file and construct a COO matrix from it
    int nnz, row, col;
    clsparseStatus fileError = clsparseHeaderfromFile( &nnz, &row, &col, path.c_str( ) );
    if( fileError != clsparseSuccess )
        throw std::runtime_error( "Could not read matrix market header from disk" );

    // Now initialise a CSR matrix from the COO matrix
    clsparseCooMatrix cooMatx;
    clsparseInitCooMatrix( &cooMatx );
    cooMatx.num_nonzeros = nnz;
    cooMatx.num_rows = row;
    cooMatx.num_cols = col;

    cl_int status;
    cooMatx.values = ::clCreateBuffer( CLSE::context, CL_MEM_READ_ONLY,
                                       cooMatx.num_nonzeros * sizeof( cl_float ), NULL, &status );
    cooMatx.colIndices = ::clCreateBuffer( CLSE::context, CL_MEM_READ_ONLY,
                                           cooMatx.num_nonzeros * sizeof( cl_int ), NULL, &status );
    cooMatx.rowIndices = ::clCreateBuffer( CLSE::context, CL_MEM_READ_ONLY,
                                           cooMatx.num_nonzeros * sizeof( cl_int ), NULL, &status );
    clsparseCooMatrixfromFile( &cooMatx, path.c_str( ), CLSE::control, explicit_zeroes );

    clsparseCsrMatrix csrMatx;
    clsparseInitCsrMatrix( &csrMatx );

    csrMatx.values = ::clCreateBuffer( CLSE::context, CL_MEM_READ_ONLY,
                                       cooMatx.num_nonzeros * sizeof( cl_float ), NULL, &status );
    csrMatx.colIndices = ::clCreateBuffer( CLSE::context, CL_MEM_READ_ONLY,
                                       cooMatx.num_nonzeros * sizeof( cl_int ), NULL, &status );
    csrMatx.rowOffsets = ::clCreateBuffer( CLSE::context, CL_MEM_READ_ONLY,
                                           ( cooMatx.num_rows + 1 ) * sizeof( cl_int ), NULL, &status );
    //clsparseScoo2csr_host( &csrMatx, &cooMatx, CLSE::control );
    clsparseScoo2csr( &cooMatx, &csrMatx, CLSE::control );

    clsparseCsrMetaSize( &csrMatx, CLSE::control );

    csrMatx.rowBlocks = ::clCreateBuffer( CLSE::context, CL_MEM_READ_ONLY,
                                          csrMatx.rowBlockSize * sizeof( cl_ulong ), NULL, &status );
    clsparseCsrMetaCompute( &csrMatx, CLSE::control );

    clsparseScalar alpha;
    clsparseScalar beta;
    cldenseVector x;
    cldenseVector y;
    clsparseInitScalar( &alpha );
    clsparseInitScalar( &beta );
    clsparseInitVector( &x );
    clsparseInitVector( &y );

    std::vector< cl_float > xHost( csrMatx.num_cols );
    std::vector< cl_float > yHost( csrMatx.num_rows );
    std::fill( xHost.begin( ), xHost.end( ), 1.0f );
    std::fill( yHost.begin( ), yHost.end( ), 2.0f );
    cl_float aHost = 1.0f;
    cl_float bHost = 1.0f;

    x.values = ::clCreateBuffer( CLSE::context,
                                CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
                                xHost.size( ) * sizeof( cl_float ), xHost.data( ), &status );
    x.num_values = yHost.size( );

    y.values = ::clCreateBuffer( CLSE::context,
                               CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
                               yHost.size( ) * sizeof( cl_float ), yHost.data( ), &status );
    y.num_values = yHost.size( );

    alpha.value = ::clCreateBuffer( CLSE::context,
                                   CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
                                   sizeof( cl_float ), &aHost, &status );

    beta.value = ::clCreateBuffer( CLSE::context,
                                  CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
                                  sizeof( cl_float ), &bHost, &status );

    clsparseScsrmv( &alpha, &csrMatx, &x, &beta, &y, CLSE::control );

    generateReference( xHost, aHost, yHost, bHost );

    std::vector<float> deviceResult( yHost.size( ) );
    ::clEnqueueReadBuffer( ClSparseEnvironment::queue,
                           y.values, CL_TRUE, 0,
                         deviceResult.size( )*sizeof( float ),
                         deviceResult.data( ), 0, NULL, NULL );

    for( int i = 0; i < yHost.size( ); ++i )
        ASSERT_NEAR( yHost[ i ], deviceResult[ i ], 5e-4 );

}

int main( int argc, char* argv[ ] )
{
    using CLSE = ClSparseEnvironment;
    using CSRE = CSREnvironment;

    po::options_description desc( "Allowed options" );

    desc.add_options( )
        ( "help,h", "Produce this message." )
        ( "path,p", po::value( &path )->required( ),
        "Path to matrix in mtx format." )
        ("no_zeroes,z", po::bool_switch()->default_value(false),
         "Disable reading explicit zeroes from the input matrix market file.");

    po::variables_map vm;
    try
    {
        po::store( po::parse_command_line( argc, argv, desc ), vm );
        po::notify( vm );
    } catch( po::error& error )
    {
        std::cerr << "Parsing command line options..." << std::endl;
        std::cerr << "Error: " << error.what( ) << std::endl;
        std::cerr << desc << std::endl;
        return false;
    }

    if (vm["no_zeroes"].as<bool>())
        explicit_zeroes = false;

    double alpha = 1.0;
    double beta = 1.0;

    ::testing::InitGoogleTest( &argc, argv );

    //order does matter!
    ::testing::AddGlobalTestEnvironment( new CLSE( ) );
    ::testing::AddGlobalTestEnvironment( new CSRE( path, alpha, beta,
        CLSE::queue, CLSE::context ) );

    return RUN_ALL_TESTS( );
}


#include <cmath>
#include <cstddef>
#include <vector>

#include <Eigen/Core>
#include <gtest/gtest.h>

#include "aslam/calibration/algorithms/permute.h"
#include "aslam/calibration/algorithms/matrixOperations.h"
#include "aslam/calibration/statistics/UniformDistribution.h"
#include "aslam/calibration/exceptions/OutOfBoundException.h"

TEST(AslamCalibrationTestSuite, testPermute) {
  using namespace aslam::calibration;

  UniformDistribution<double> dist(0, 100);
    const std::vector<double> input = {dist.getSample(), dist.getSample(),
      dist.getSample(), dist.getSample(), dist.getSample(), dist.getSample()};
  const std::vector<size_t> p = {1, 3, 0, 2, 5, 4};
  auto pinput = input;

  permute(pinput, p);

  ASSERT_EQ(pinput, std::vector<double>({input[p[0]], input[p[1]], input[p[2]],
    input[p[3]], input[p[4]], input[p[5]]}));
  ASSERT_THROW(permute(pinput, std::vector<size_t>({1, 3, 0})),
    OutOfBoundException<size_t>);
  ASSERT_THROW(permute(pinput, std::vector<size_t>({10, 3, 0, 2, 5, 4})),
    OutOfBoundException<size_t>);
}

TEST(AslamCalibrationTestSuite, testComputeSumLogDiagR) {
  using namespace aslam::calibration;

  Eigen::Matrix3d A;
  A << 1,0,0, 10,50,0, 0,20,3.3;

  EXPECT_NEAR(computeSumLogDiagR(A,0,2), std::log2(std::fabs(A(0,0))) +
              std::log2(std::fabs(A(1,1))) + std::log2(std::fabs(A(2,2))), 1e-8);
  EXPECT_NEAR(computeSumLogDiagR(A,0,1), std::log2(std::fabs(A(0,0))) +
              std::log2(std::fabs(A(1,1))), 1e-8);
  EXPECT_NEAR(computeSumLogDiagR(A,1,2), std::log2(std::fabs(A(1,1))) +
              std::log2(std::fabs(A(2,2))), 1e-8);
}


//
// Created by shen on 18-6-5.
//
#include <iostream>
#include <string>

#include <pcl/io/openni2_grabber.h>         //OpenNI采集头文件

#include <pcl/console/parse.h>
#include <boost/thread/thread.hpp>.
#include <pcl/common/common_headers.h>

#include <pcl/visualization/pcl_visualizer.h>

#include <pcl/filters/passthrough.h>                    //直通滤波器头文件
#include <pcl/filters/voxel_grid.h>                     //体素滤波器头文件
#include <pcl/filters/statistical_outlier_removal.h>    //统计滤波头文件
#include <pcl/filters/extract_indices.h>                //索引提取滤波器头文件
#include <pcl/filters/project_inliers.h>                //映射相关头文件

#include <pcl/segmentation/sac_segmentation.h>  //基于采样一致性分割的类的头文件
#include <pcl/ModelCoefficients.h>          //采样一致性模型相关类头文件

#include <pcl/sample_consensus/method_types.h>  //随机参数估计方法头文件
#include <pcl/sample_consensus/model_types.h>   //模型定义头文件

#include <pcl/surface/concave_hull.h>           //提取凸（凹）多边形的头文件.

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

using namespace std;
using namespace cv;

class VolumDetectViewer
{
public:
	boost::mutex cloud_mutex;
	pcl::visualization::PCLVisualizer viewer;

	// 直通滤波器对象
	pcl::PassThrough<pcl::PointXYZ> XpassFilter;
	pcl::PassThrough<pcl::PointXYZ> YpassFilter;

	// 下采样 VoxelGrid 滤波对象
	pcl::VoxelGrid<pcl::PointXYZ> VoxlFilter;

	// 统计滤波器
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statisFilter;

	// 分割对象
	pcl::SACSegmentation<pcl::PointXYZ> segPlane;

	// 提取索引滤波器
	pcl::ExtractIndices<pcl::PointXYZ> extractPlane;

	// 点云投影滤波对象
	pcl::ProjectInliers<pcl::PointXYZ> projec;

	VolumDetectViewer():viewer("PCLVIEW")
	{
		// 1. 初始化 new viewer
		//pcl::visualization::PCLVisualizer viewer("PCLVIEW");
		viewer.setBackgroundColor(0, 0, 0);
		viewer.addCoordinateSystem(0.6);
		viewer.initCameraParameters();
		viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");
		//viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.15, "cloud");

		// 2. 直通滤波设置
		// 滤波字段设为x轴方向.
		XpassFilter.setFilterFieldName("x");
		// 设定可接受范围，将不在范围内的点过滤掉或者保留（由setFilterLimitsNegative决定）.
		XpassFilter.setFilterLimits(-0.25, 0.05);  // TODO: X方向裁剪阈值需要调试 实际是0.44m.
		// 滤波字段设为y轴方向.
		YpassFilter.setFilterFieldName("y");
		// 设定可接受范围，将不在范围内的点过滤掉或者保留（由setFilterLimitsNegative决定）.
		YpassFilter.setFilterLimits(-0.25, 0.18);  // TODO: Y方向裁剪阈值需要调试 实际是0.69m.

		// 3. 体素滤波设置
		// 设置体素滤波时创建的体素体积为0.5*0.5*0.5cm的立方体
		VoxlFilter.setLeafSize(0.005f, 0.005f, 0.005f);

		// 4. 统计滤波初始设置
		statisFilter.setMeanK(50);  // 对每个点分析的临近点个数设为50
		statisFilter.setStddevMulThresh(1.0);   // 将标准差倍数设为1，意味着一个点的距离超出平均距离1个标准差以上，就会被标记为离群点，并被移除。

		// 5. 设置分割对象
		// 可选：设置模型系数需要优化
		segPlane.setOptimizeCoefficients(true);
		// 必选：设置分割的模型类型、所用的随机参数估计方法、距离阈值、迭代次数上限
		segPlane.setModelType(pcl::SACMODEL_PLANE);
		segPlane.setMethodType(pcl::SAC_RANSAC);
		segPlane.setDistanceThreshold(0.01);    // TODO: 距离阈值需要调试
		segPlane.setMaxIterations(1000);

		// 6. 设置投影模型为SACMODEL_PLANE
		projec.setModelType(pcl::SACMODEL_PLANE);

	}

	void cloud_cb_(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &Prev_cloud, bool * new_cloud_available)
	{
		cloud_mutex.lock();

		*Prev_cloud = *cloud;

		*new_cloud_available = true;

		cloud_mutex.unlock();
	}

	void run()
	{
		// 1. 新建OpenNI设备的捕获器 grabber
		pcl::Grabber* interface = new pcl::io::OpenNI2Grabber();

		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

		bool new_cloud_available = false;
		// 建立回调
		boost::function<void(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&)> f =
			boost::bind(&VolumDetectViewer::cloud_cb_, this, _1, cloud, &new_cloud_available);

		interface->registerCallback(f);

		interface->start();

		//// 等待第一帧
		//while (!new_cloud_available)
		//{
		//	boost::this_thread::sleep(boost::posix_time::milliseconds(1));
		//	new_cloud_available = false;
		//}

		while (!viewer.wasStopped())
		{

			if (new_cloud_available && cloud_mutex.try_lock())
			{
				new_cloud_available = false;

				// 1. 空间裁剪滤波
				pcl::PointCloud<pcl::PointXYZ>::Ptr pass_filtered(new pcl::PointCloud<pcl::PointXYZ>);

				XpassFilter.setInputCloud(cloud);
				XpassFilter.filter(*pass_filtered);
				YpassFilter.setInputCloud(pass_filtered);
				YpassFilter.filter(*pass_filtered);
				// 滤波信息
				string originSize = "originSize: " + to_string(cloud->points.size());
				string passfiltered ="pass_filtered: " + to_string(pass_filtered->points.size());

				// 2. 下采样进行数据压缩
				// 下采样
				pcl::PointCloud<pcl::PointXYZ>::Ptr voxel_filtered(new pcl::PointCloud<pcl::PointXYZ>);
				VoxlFilter.setInputCloud(pass_filtered);
				VoxlFilter.filter(*voxel_filtered);
				// 滤波信息
				string voxelfiltered = "voxel_filtered: " + to_string(voxel_filtered->points.size());

				// 3. 统计滤波 去除异常点
				pcl::PointCloud<pcl::PointXYZ>::Ptr statis_Filtered(new pcl::PointCloud<pcl::PointXYZ>);
				statisFilter.setInputCloud(voxel_filtered);
				statisFilter.filter(*statis_Filtered);
				// 滤波信息
				string statisFiltered = "statis_Filtered: " + to_string(statis_Filtered->points.size());

				// 4. 平面分割 获取地面
				// 创建分割时所需要的模型系数对象 coefficientsPlane
				// 创建存储模型内点的点索引集合对象 inliersPlane
				pcl::ModelCoefficients::Ptr coefficientsPlane(new pcl::ModelCoefficients);
				pcl::PointIndices::Ptr inliersPlane(new pcl::PointIndices());
				// 平面分割
				segPlane.setInputCloud(statis_Filtered);
				segPlane.segment(*inliersPlane, *coefficientsPlane);
				string PlaneSeg;
				if (inliersPlane->indices.size() == 0)
				{
					PlaneSeg = "No Plane detected";
				}
				else
				{
					PlaneSeg = "Plane Model: ax+by+cz+d=0, a = " + to_string(coefficientsPlane->values[0]) + ", b= " + to_string(coefficientsPlane->values[1]) +
						", c= " + to_string(coefficientsPlane->values[2]) + ", d= " + to_string(coefficientsPlane->values[3]);
				}
				// 拷贝一份平面点集,并着色
				pcl::PointCloud<pcl::PointXYZ>::Ptr PlaneCloud(new pcl::PointCloud<pcl::PointXYZ>);
				pcl::copyPointCloud(*statis_Filtered, inliersPlane->indices, *PlaneCloud);
				pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> Plane_color(PlaneCloud, 139, 139, 0);

				// 5. 根据索引提取除地面之外的点云
				pcl::PointCloud<pcl::PointXYZ>::Ptr extracted(new pcl::PointCloud<pcl::PointXYZ>);
				extractPlane.setInputCloud(statis_Filtered);
				extractPlane.setIndices(inliersPlane);
				extractPlane.setNegative(true); // 设置Negative是获取除了平面的点
				extractPlane.filter(*extracted);
				string extractedFromPlane = "extractedFromPlane Remains size: " + to_string(extracted->points.size());

				// 6. 平面分割 获取目标物上表面
				// 创建分割时所需要的模型系数对象 coefficientsTarget
				// 创建存储模型内点的点索引集合对象 inliersTarget
				pcl::ModelCoefficients::Ptr coefficientsTarget(new pcl::ModelCoefficients);
				pcl::PointIndices::Ptr inliersTarget(new pcl::PointIndices());
				// 平面分割
				segPlane.setInputCloud(extracted);
				segPlane.segment(*inliersTarget, *coefficientsTarget);
				string TargetSeg;
				if (inliersTarget->indices.size() == 0)
				{
					TargetSeg = "No Plane detected";
					//break;
				}
				else
				{
					TargetSeg = "Target Model: ax+by+cz+d=0, a = " + to_string(coefficientsTarget->values[0]) + ", b= " + to_string(coefficientsTarget->values[1]) +
						", c= " + to_string(coefficientsTarget->values[2]) + ", d= " + to_string(coefficientsTarget->values[3]);
				}
				// 拷贝一份平面点集,并着色
				pcl::PointCloud<pcl::PointXYZ>::Ptr TargetCloud(new pcl::PointCloud<pcl::PointXYZ>);
				pcl::copyPointCloud(*extracted, inliersTarget->indices, *TargetCloud);
				pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> Target_color(TargetCloud, 205, 201, 201);

				// 7. 计算高度差
				float height = abs(coefficientsPlane->values[3] / coefficientsPlane->values[2] -
					coefficientsTarget->values[3] / coefficientsPlane->values[2]);
				string heightStr = "Height: " + to_string(height);

				// 8. 投影平面模型系数
				pcl::PointCloud<pcl::PointXYZ>::Ptr target_projected(new pcl::PointCloud<pcl::PointXYZ>);

				projec.setIndices(inliersTarget);
				projec.setInputCloud(extracted);
				projec.setModelCoefficients(coefficientsTarget);
				projec.filter(*target_projected);
				pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_projected_color(target_projected, 255, 69, 0);

				// 9. 为投影的点创建凹包表征
				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull(new pcl::PointCloud<pcl::PointXYZ>);
				pcl::ConcaveHull<pcl::PointXYZ> chull;
				//polygons存储一系列顶点集，每组顶点集表示一个多边形 //应该取其中第0个顶点集
				vector<pcl::Vertices> polygons;
				chull.setInputCloud(target_projected);
				chull.setAlpha(0.1);
				// polygons对应的是前面point cloud_hull的索引
				chull.reconstruct(*cloud_hull, polygons);
				pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_hull_color(cloud_hull, 0, 245, 255);

				// 10. 获取二维轮廓供OpenCV处理
				// 生成二维轮廓
				vector<Point2f> hullContour;
				for (int j = 0; j < polygons[0].vertices.size(); ++j) {
					hullContour.push_back(Point2f(cloud_hull->points[polygons[0].vertices[j]].x,
						cloud_hull->points[polygons[0].vertices[j]].y));
				}

				RotatedRect minRect = minAreaRect(hullContour);

				// TODO：此处的长宽与实际需要进行换算

				float realWidth = minRect.size.width * 1.5;
				float realHeight = minRect.size.height * 1.5;
				float rectArea = realWidth* realHeight;
				string rectInfo = "length: " + to_string(realWidth) + ", width: " + to_string(realHeight);

				// 8. 计算凹包面积
				// 8.1 自带函数计算
				// 面积的计算方式是没有问题的
				float Hullarea = pcl::calculatePolygonArea(*cloud_hull) * (0.69*0.44) / (0.3*0.43);

				string AreaStr = "Rect Area: " + to_string(rectArea) + ", Hull Area: " + to_string(Hullarea);

				float Vol = rectArea * height;
				string VolStr = "Volume: " + to_string(Vol);

				// 更新点云显示和字符串显示
				viewer.removeAllPointClouds();
				viewer.removeAllShapes();

				// 添加圆球体
				//pcl::PointXYZ center(0.2, 0.2, 1.0);
				//viewer.addSphere(center,0.01,1.0,1.0,0, "sphere");

				// 添加方体
				//viewer.addCube(-0.1, 0.1, -0.1, 0.1, 0.9, 1.0,0.1,0.2,0.3, "cube");

				viewer.addText(PlaneSeg, 10, 1000, 20, 1, 1, 0, "PlaneSeg");
				viewer.addText(TargetSeg, 10, 950, 20, 1, 1, 0, "TargetSeg");
				viewer.addText(heightStr, 10, 900, 20, 1, 1, 0, "heightStr");
				viewer.addText(rectInfo, 10, 850, 20, 1, 1, 0, "rectInfo");
				viewer.addText(AreaStr, 10, 800, 20, 1, 1, 0, "AreaStr");
				viewer.addText(VolStr, 10, 750, 30, 1, 1, 0, "VolStr");

				viewer.addText(originSize, 10, 200, 15, 1, 1, 0, "originSize");
				viewer.addText(passfiltered, 10,180, 15, 1, 1, 0, "passfiltered");
				viewer.addText(voxelfiltered, 10, 160, 15, 1, 1, 0, "voxelfiltered");
				viewer.addText(statisFiltered, 10, 140, 15, 1, 1, 0, "statisFiltered");
				viewer.addText(extractedFromPlane, 10, 120, 15, 1, 1, 0, "extractedFromPlane");

				viewer.addPointCloud<pcl::PointXYZ>(statis_Filtered, "cloud");
				viewer.addPointCloud<pcl::PointXYZ>(PlaneCloud, Plane_color, "Plane");
				viewer.addPointCloud<pcl::PointXYZ>(TargetCloud, Target_color, "Target");
				viewer.addPointCloud<pcl::PointXYZ>(target_projected, target_projected_color, "target_projected");
				viewer.addPointCloud<pcl::PointXYZ>(cloud_hull, cloud_hull_color, "cloud_hull");

				viewer.spinOnce();

				cloud_mutex.unlock();
			}
		}

		interface->stop();
	}
};

int main()
{
	VolumDetectViewer v;
	v.run();
	return (0);
}


#include <boost/shared_ptr.hpp>

#include <opencv2/highgui/highgui.hpp>
#include <eigen3/Eigen/Eigen>
#include <eigen3/Eigen/Core>

#include "map_point.h"

unsigned long MapPoint::factory_id_ = 0;	// 类内的静态成员变量要单独初始化

MapPoint::MapPoint(long unsigned int id, const Eigen::Vector3d& pose, const Eigen::Vector3d& norm_observe_direct, const cv::Mat& descriptor):
id_(id), pose_(pose), norm_observe_direct_(norm_observe_direct), descriptor_(descriptor), observed_times_(1), matched_times_(1)
{

}

MapPoint::~MapPoint()
{

}

MapPoint::Ptr MapPoint::createMapPoint(const Eigen::Vector3d& pose, const Eigen::Vector3d& norm_observe_direct, const  cv::Mat& descriptor)
{
	return MapPoint::Ptr( new MapPoint(factory_id_++, pose, norm_observe_direct, descriptor) );
}


/*
 * Author(s):  Salvadore Gerace <sgerace@dotdecimal.com>
 * Date:       12/12/2013
 *
 * Copyright:
 * This work was developed as a joint effort between .decimal, Inc. and
 * Partners HealthCare under research agreement A213686; as such, it is
 * jointly copyrighted by the participating organizations.
 * (c) 2013 .decimal, Inc. All rights reserved.
 * (c) 2013 Partners HealthCare. All rights reserved.
 */

#ifndef CRADLE_GEOMETRY_BIN_COLLECTION_HPP
#define CRADLE_GEOMETRY_BIN_COLLECTION_HPP

#include <algorithm>
#include <limits>
#include <map>
#include <set>
#include <vector>
#include <boost/numeric/conversion/cast.hpp>
#include <cradle/common.hpp>
#include <cradle/geometry/common.hpp>

namespace cradle {

// --------------------------------------------------
// Public structures

const double EPSILON = 1.0e-8;

// struct used temporarily while building the optimized bin_collection
template<class ItemType, unsigned int N, class T>
struct bin_collection_item
{
    ItemType item;
    box<N, T> bounds;
    bin_collection_item() {}
    bin_collection_item(
        ItemType const& item, box<N,T> const& bounds) : item(item), bounds(bounds) {}
};

// Represents a collection of geometric items which have been bounded by axis-aligned
// bounding boxes in N-dimensional Euclidean space.
api(struct with(ItemType:unsigned;N:3;T:double))
template<class ItemType, unsigned N, class T>
struct bin_collection
{
    // Bounds of all elements in the collection.
    box<N, T> bounds;
    // Spacing of the bins.
    vector<N,unsigned> grid_size;
    // Array of index shifts used for storing bin items (used to locate first item in each
    // within the item array). (u)
    array<unsigned> offsets;
    // Array of item counts per bin. (u)
    array<unsigned> counts;
    // Array of actual items in the bins. (struct: ItemType)
    array<ItemType> bins;
};

// helper for converting from coords to an index into the flat list
template<unsigned int N>
unsigned
coords_to_index(vector<N,unsigned> const& coords, vector<N,unsigned> const& grid_size)
{
    // calculate the step size for moving through each dimension
    vector<N,unsigned> step_sizes;
    step_sizes[0] = 1;
    for (unsigned dim = 1; dim < N; ++dim)
    {
        step_sizes[dim] = step_sizes[dim - 1] * grid_size[dim - 1];
    }

    return dot(coords, step_sizes);
}

// check whether the given coordinates are in-bounds for the grid size
template<unsigned int N>
bool
coords_in_bounds(vector<N,unsigned> const& coords, vector<N,unsigned> const& grid_size)
{
    for (unsigned dim = 0; dim < N; ++dim)
    {
        if (coords[dim] >= grid_size[dim])
        {
            return false;
        }
    }
    return true;
}

// advances coords to the next coord for iterating through a range in a grid. returns true
// if there was a valid next coord
template<unsigned int N>
bool
go_to_next(
    vector<N,unsigned> & coords,
    vector<N,unsigned> const& coord_minimums,
    vector<N,unsigned> const& coord_maximums)
{
    unsigned i = 0;

    while (true)
    {
        if (coords[i] == coord_maximums[i])
        {
            coords[i] = coord_minimums[i];
            i++;
            if (i == N)
            {
                return false;
            }
        }
        else
        {
            coords[i] += 1;
            return true;
        }
    }
}
// advances backwards to the next coord for iterating through a range in a grid. returns
// true if there was a valid prev coord
template<unsigned int N>
bool
go_to_prev(
    vector<N,unsigned> & coords,
    vector<N,unsigned> const& coord_minimums,
    vector<N,unsigned> const& coord_maximums)
{
    unsigned i = 0;
    while (true)
    {
        if (coords[i] == coord_minimums[i])
        {
            coords[i] = coord_maximums[i];
            ++i;
            if (i == N)
            {
                return false;
            }
        }
        else
        {
            coords[i] -= 1;
            return true;
        }
    }
}

// used during make_bin_collection to place items in their bins
template<typename OutputIterator, class ItemType, unsigned int N, class T>
void fill(
    // iterator to beginning of container to fill
    OutputIterator& output,
    // first row/col/etc. to include in each dimension
    vector<N,unsigned> const& starts,
    // last row/col/etc. to include in each dimension
    vector<N,unsigned> const& ends,
    // how many rows/cols/etc. the bincollection has in each dimensions
    vector<N,unsigned> const& grid_size,
    // item to place in each bin in the given range
    bin_collection_item<ItemType,N,T> const& item )
{
    typedef vector<N,unsigned> Coords;

    // keep coords in valid range
    Coords clamped_starts;
    Coords clamped_ends;
    for (unsigned dim = 0; dim < N; ++dim)
    {
        clamped_starts[dim] = std::max(std::min(starts[dim], grid_size[dim] - 1), 0U);
        clamped_ends[dim]   = std::max(std::min(ends[dim],   grid_size[dim] - 1), 0U);
    }

    // early out on nothing to fill
    if (length2(grid_size) == 0)
    {
        return;
    }

    // step through the bins, adding item to them
    Coords current_bin_coords = clamped_starts;
    do
    {
        unsigned i = coords_to_index(current_bin_coords, grid_size);
        (output + i)->push_back(item);
    } while (go_to_next(current_bin_coords, clamped_starts, clamped_ends));
}

// convert a filled grid of bin_items to the optimized bin_collection struct
template<typename InputIterator, class ItemType, unsigned int N, class T>
bin_collection<ItemType, N, T>
optimize_to_bin_collection(
    InputIterator const& in_bins,
    vector<N,unsigned> grid_size,
    box<N, T> bounds)
{
    typedef bin_collection_item<ItemType, N, T> BinItem;
    typedef std::vector<BinItem> ItemList;

    bin_collection<ItemType, N, T> output;
    output.bounds = bounds;
    output.grid_size = grid_size;

    unsigned total_bins = 1;
    for (unsigned dim = 0; dim < N; ++dim)
    {
        total_bins *= grid_size[dim];
    }

    unsigned* offsets = allocate(&output.offsets, total_bins);
    unsigned* counts = allocate(&output.counts, total_bins);

    // figure out how long bin_array is and fill counts array
    unsigned bin_array_length = 0;
    for (unsigned i = 0; i < total_bins; ++i)
    {
        unsigned count = boost::numeric_cast<unsigned>((in_bins + i)->size());
        counts[i] = count;
        bin_array_length += count;
    }

    ItemType* bins = allocate(&output.bins, bin_array_length);

    // copy values into flattened bin array
    unsigned cur_offset = 0;
    for (unsigned i = 0; i < total_bins; ++i)
    {
        ItemList const& bin = *(in_bins + i);
        offsets[i] = cur_offset;
        for (auto iter = bin.begin(); iter != bin.end(); ++iter)
        {
            bins[cur_offset] = iter->item;
            ++cur_offset;
        }
    }

    return output;
}

template <typename InputIterator, typename GetBoundsFnType,
    class ItemType, unsigned int N, class T>
bin_collection<ItemType, N, T>
make_bin_collection(
    InputIterator const& in_iter_begin,
    InputIterator const& in_iter_end,
    GetBoundsFnType get_bounds_fn,
    unsigned resolution_multiplier = 2)
{
    typedef bin_collection_item<ItemType, N, T> BinItem;
    typedef std::vector<BinItem> ItemList;
    typedef std::vector<ItemList> ItemListList;
    typedef vector<N,unsigned> Coords;
    typedef vector<N,T> Vector;

    Vector bounds_min;
    Vector bounds_max;
    Vector size_sums;
    for (unsigned dim = 0; dim < N; ++dim)
    {
        bounds_min[dim] = (std::numeric_limits<T>::max)();
        bounds_max[dim] = std::numeric_limits<T>::lowest();
        size_sums[dim] = 0;
    }

    // create bin_items, keep track of bounds of things
    std::map<ItemType, BinItem> bin_items;
    unsigned num_items = 0;
    for (auto iter = in_iter_begin; iter != in_iter_end; ++iter)
    {
        ItemType const& val = *iter;
        bin_items[val] = BinItem(val, get_bounds_fn(val));
        auto const& b = bin_items[val].bounds;
        for (unsigned dim = 0; dim < N; ++dim)
        {
            if (b.corner[dim] < bounds_min[dim])
            {
                bounds_min[dim] = b.corner[dim];
            }
            if ((b.corner[dim] + b.size[dim]) > bounds_max[dim])
            {
                bounds_max[dim] = b.corner[dim] + b.size[dim];
            }
        }
        ++num_items;
        size_sums += b.size;
    }

    // make the big bins list
    Coords grid_size;
    box3d bounds(bounds_min, bounds_max - bounds_min);
    for (unsigned dim = 0; dim < N; ++dim)
    {
        grid_size[dim] =
            unsigned(size_sums[dim] > EPSILON ?
                std::ceil(double(resolution_multiplier) * double(num_items) *
                    bounds.size[dim] / size_sums[dim]) : 1);
    }
    ItemListList bins;
    unsigned num_bins = 1;
    for (unsigned dim = 0; dim < N; ++dim)
    {
        num_bins *= grid_size[dim];
    }
    bins.resize(num_bins);

    // add items to the bins
    for (auto iter = bin_items.begin(); iter != bin_items.end(); ++iter)
    {
        // start in each row or column or (...etc into more dimensions) of the range that
        // includes the item
        Coords starts;
        // end in each row or column or (...etc into more dimensions) of the range that
        // includes the item
        Coords ends;

        for (unsigned dim = 0; dim < N; ++dim)
        {
            auto& item_bounds = iter->second.bounds;
            auto item_min = item_bounds.corner[dim];
            auto item_max = item_bounds.corner[dim] + item_bounds.size[dim];
            auto mesh_min = bounds.corner[dim];
            auto mesh_max = bounds.corner[dim] + bounds.size[dim];

            starts[dim] =
                unsigned(grid_size[dim] * (item_min - mesh_min) / bounds.size[dim]);
            ends[dim] =
                unsigned(grid_size[dim] * (item_max - mesh_min) / bounds.size[dim]);
        }

        typename ItemListList::iterator bins_begin = bins.begin();
        cradle::fill(bins_begin, starts, ends, grid_size, iter->second);
    }

    return
        optimize_to_bin_collection<typename ItemListList::const_iterator, ItemType, N, T>(
            bins.begin(),
            grid_size,
            bounds);
}

template<class ItemType, unsigned int N, class T>
vector<N, unsigned>
get_coords_for_point(
    bin_collection<ItemType, N, T> const& bin_col,
    vector<N, T> const& point)
{
    typedef vector<N,T> Vector;
    typedef vector<N,unsigned> Coords;

    Vector point_as_offset_from_corner = point - bin_col.bounds.corner;
    Coords coords;
    for (unsigned dim = 0; dim < N; ++dim)
    {
        coords[dim] =
            unsigned(
                double(bin_col.grid_size[dim]) *
                (point[dim] - bin_col.bounds.corner[dim]) / bin_col.bounds.size[dim]);

        coords[dim] = cradle::clamp(coords[dim], 0U, bin_col.grid_size[dim]);
    }
    return coords;
}

// given the grid coordinates, get an iterator to the first item in that bin
template<class ItemType, unsigned int N, class T>
ItemType const*
get_bin_begin(
    bin_collection<ItemType, N, T> const& bin_col,
    vector<N, unsigned> const& coords)
{
    unsigned i = coords_to_index(coords, bin_col.grid_size);
    unsigned offset = bin_col.offsets[i];
    return bin_col.bins.begin() + offset;
}

// given the grid coordinates, get an iterator (just a pointer in this case) one past the
// last item in that bin
template<class ItemType, unsigned int N, class T>
ItemType const*
get_bin_end(
    bin_collection<ItemType, N, T> const& bin_col,
    vector<N, unsigned> const& coords)
{
    unsigned i = coords_to_index(coords, bin_col.grid_size);
    unsigned offset = bin_col.offsets[i];
    unsigned count = bin_col.counts[i];
    return bin_col.bins.begin() + offset + count;
}

// step over the bin collection
// starting in the bin containing {start_point}
// move along axis {axis}
// in the positive direction or negative direction based on {positive_direction}
// calling {fn} on every item found in those bins
// adding up their results and storing it in {*sum} if not null
// if any of those calls to {fn} return false,
// this function returns false instead of setting {*sum}.
//
// fn will be called with the following args:
// (ItemType const& item, SumType*)
// returning whether it was successful (true)
// and if successful, should set the value {*value} if not null.
template<typename FunctionType, typename SumType, class ItemType, unsigned int N, class T>
bool
do_sum(
    bin_collection<ItemType, N, T> const& bin_col,
    vector<N, T> const& start_point,
    unsigned axis,
    bool positive_direction,
    FunctionType fn,
    SumType* sum )
{
    typedef vector<N,unsigned> Coords;

    SumType tempsum = 0;

    // early out if start point is not even within the bin collection bounds
    if (!contains(bin_col.bounds, start_point))
    {
        return false;
    }

    Coords coords = get_coords_for_point(bin_col, start_point);
    Coords min = coords;
    Coords max = coords;
    if (positive_direction)
    {
        max[axis] = bin_col.grid_size[axis] - 1;
    }
    else
    {
        min[axis] = 0;
    }

    std::set<ItemType> items_checked;

    bool keep_going = coords_in_bounds(coords, bin_col.grid_size);
    while (keep_going)
    {
        SumType temp_result;

        auto end = get_bin_end(bin_col, coords);
        for (auto iter = get_bin_begin(bin_col, coords); iter != end; ++iter)
        {
            if (items_checked.find(*iter) == items_checked.end())
            {
                items_checked.insert(*iter);
                bool success = fn(*iter, &temp_result);
                if (!success)
                {
                    return false;
                }
                tempsum += temp_result;
            }
        }

        if (positive_direction)
        {
            keep_going = go_to_next(coords, min, max);
        }
        else
        {
            keep_going = go_to_prev(coords, min, max);
        }
    }

    *sum = tempsum;
    return true;
}

}

#endif


#include <gtest/gtest.h>
#include "filters.hpp"
#include <filters/filter_chain.h>
#include <math.h>
#include <Eigen/Dense>

#define GTEST_COUT std::cerr << "[   INFO   ] "

TEST(ThresholdFilter, active)
{
    EXPECT_EQ(0, 0);
}

/**
 * Test that error checking works for cases in
 * which the layers don't exist.
 */
TEST(ThresholdFilter, layers_dont_exist)
{
    ros::NodeHandle nh("~");

    grid_map::GridMap gridMap({"random_layer"});
    gridMap.setFrameId("random");
    gridMap.setGeometry(grid_map::Length(3, 3), 1);

    filters::FilterChain<grid_map::GridMap> filterChain("grid_map::GridMap");

    if (!filterChain.configure("mitre_fast_layered_map_filters_threshold", nh))
    {
        GTEST_FATAL_FAILURE_("Unable to configure filter chain."); // We should never get here
    }

    EXPECT_FALSE(filterChain.update(gridMap, gridMap));

    // Bayes filter needs current_probability and obstacle_detected layers
    gridMap.add("current_probability", 0);
    EXPECT_FALSE(filterChain.update(gridMap, gridMap));

    gridMap.add("nonground", 0);
    EXPECT_TRUE(filterChain.update(gridMap, gridMap));

}

// /***
//  * All of the tests below this point are just checking that the math
//  * comes out as expected. You can use this to refer to the expected performance
//  * given the settings.
//  */

// /**
//  * Ensure its outputting expected values when a reading is given as 1 or 0
//  */
TEST(BayesUpdate, binary)
{
    ros::NodeHandle nh("~");

    // NOTE: Current probability will be filled with nans, but the filter should take care of it
    grid_map::GridMap gridMap({"current_probability", "nonground"});
    gridMap.setFrameId("random");
    gridMap.setGeometry(grid_map::Length(3, 3), 1);

    filters::FilterChain<grid_map::GridMap> filterChain("grid_map::GridMap");

    if (!filterChain.configure("mitre_fast_layered_map_filters_threshold", nh))
    {
        GTEST_FATAL_FAILURE_("Unable to configure filter chain."); // We should never get here
    }

    // Initialize obstacle detected, only two cells have detection in first frame
    gridMap["current_probability"] <<
    0, 0.1, 0.2,
    0.3, 0.4, 0.5,
    0.6, 0.7, 0.8;

    // Create matrix of expected output to compare to
    Eigen::MatrixXf answerMat(3, 3);
    answerMat << 0, 0, 0,
        0, 100, 100,
        100, 100, 100;

    if (!filterChain.update(gridMap, gridMap))
    {
        GTEST_FATAL_FAILURE_("Unable to update grid map.");
    }

    for (grid_map::GridMapIterator it(gridMap); !it.isPastEnd(); ++it)
    {
        const grid_map::Index index(*it);

        EXPECT_FLOAT_EQ(answerMat(index(0), index(1)), gridMap.at("nonground", index)) << "Index: " << index(0) << " " << index(1);
    }

}