#include <cstdint>
#include <algorithm>
#include <fstream>
#include <functional>
#include <iomanip>
#include <iostream>
#include <random>
#include <sstream>
#include <string>
#include <unordered_map>
#include <utility>

#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/negative_binomial.hpp>
#include <boost/cast.hpp>
#include <boost/random/negative_binomial_distribution.hpp>

namespace {
    using ParamType = double;
    using ResultType = int;
    using DensityType = double;
    using RandEngine = std::mt19937;
    using SizeType = size_t;
    using SeedType = std::mt19937::result_type;

    struct ParamSet {
        ParamType size;
        ParamType prob;
    };

    struct FileSet {
        SizeType n;
        std::string filename;
    };

    const std::vector<ParamType> SizeSet {2, 4, 6, 8};
    const std::vector<ParamType> ProbSet {0.1, 0.15, 0.25, 0.5};
    const std::vector<FileSet> AllFileSet {
        {100, "nbinom100.csv"},
        {1000, "nbinom1k.csv"},
        {10000, "nbinom10k.csv"},
        {100000, "nbinom100k.csv"},
    };
}

void rnbinomCpp(const auto& paramSet, SizeType n, SeedType seed, std::ofstream& ofs) {
    const ParamType alpha = paramSet.size;
    // Notice that prob for R means 1 - prob in C++
    const ParamType prob = paramSet.prob;
    const ParamType beta = (1 - prob) / prob;
    std::mt19937 gen(seed);

    std::gamma_distribution<double> distGamma(alpha, beta);
    std::unordered_map<ResultType, SizeType> count;

    // Cannot use alpha as integers
    using StdAlpha = int;
    const auto stdAlpha = boost::numeric_cast<StdAlpha>(alpha);
    std::negative_binomial_distribution<StdAlpha> distNB(stdAlpha, beta);

    // Makes random values
    ResultType maxValue = 0;
    for (decltype(n) i=0; i<n; ++i) {
        std::poisson_distribution<ResultType> dist(distGamma(gen));
        const auto value = dist(gen);
        count[value] += 1;
        maxValue = std::max(maxValue, value);
    }

    // Can eliminate outliers
    const SizeType sizeLimit = n + 100; //  n / 100;
    SizeType totalCount = 0;
    for (ResultType value=0; value<=maxValue; ++value) {
        // Writes densities in a long format
        const auto binCount = count[value];
        totalCount += binCount;
        const DensityType density = static_cast<DensityType>(binCount) / static_cast<DensityType>(n);
        ofs << alpha << "," << beta << "," << prob << "," << value << "," << density << "\n";
        if (totalCount >= sizeLimit) {
            break;
        }
    }
    return;
}

void rnbinomCppAll(void) {
    constexpr SeedType seed = 123;

    for(const auto& fileSet :AllFileSet) {
        // Writes titles
        std::ofstream ofs(fileSet.filename);
        ofs << "alpha,beta,prob,x,density" << std::endl;

        for(const auto& size : SizeSet) {
            for(const auto& prob : ProbSet) {
                const ParamSet paramSet {size, prob};
                rnbinomCpp(paramSet, fileSet.n, seed, ofs);
            }
        }
    }
}

template <typename CdfGenerator>
std::vector<DensityType> GenerateCdf(ResultType count, CdfGenerator& cdfGenerator) {
    std::vector<DensityType> vec;
    for(ResultType i=0; i<count; ++i) {
        vec.push_back(cdfGenerator(i));
    }
    return vec;
}

template <typename Vec>
void PrintVec(const Vec& vec, std::ostream& os) {
    const auto size = vec.size();
    for(auto i = decltype(size){0}; i<size; ++i) {
        os << vec.at(i);
        if ((i + 1) < size) {
            os << ",";
        }
    }
    os << "\n";
    return;
}

void nbinomCppCdf(void) {
    constexpr DensityType nbSize = 2.0;
    constexpr DensityType nbProb = 0.25;

    boost::math::negative_binomial distNB(nbSize, nbProb);
    auto funcNB = [&distNB](ResultType i) {
        return boost::math::cdf(distNB, i);
    };

    auto cdfNB = std::bind(funcNB, std::placeholders::_1);
    constexpr ResultType count = 10;
    std::ostringstream osNB;
    const auto actualNB = GenerateCdf(count, cdfNB);
    PrintVec(actualNB, osNB);

    // R pnbinom(seq(0, 9, 1), size=2.0, prob=0.25)
    // [1] 0.0625000 0.1562500 0.2617188 0.3671875 0.4660645 0.5550537 0.6329193 0.6996613
    // [9] 0.7559748 0.8029027
    std::cout << osNB.str();

    constexpr DensityType shape = 1.0;
    constexpr DensityType scale = 2.0;

    boost::math::gamma_distribution<double> distGamma(shape, scale);
    auto funcGamma = [&distGamma](ResultType i) {
        return boost::math::cdf(distGamma, i);
    };

    auto cdfGamma = std::bind(funcGamma, std::placeholders::_1);
    const auto actualGamma = GenerateCdf(count, cdfGamma);

    // R pgamma(seq(0, 9, 1), shape=1.0, scale=2.0)
    // [1] 0.0000000 0.3934693 0.6321206 0.7768698 0.8646647 0.9179150 0.9502129 0.9698026
    // [9] 0.9816844 0.9888910
    std::ostringstream osGamma;
    PrintVec(actualGamma, osGamma);
    std::cout << osGamma.str() << std::endl;
}

void cppRandombinom(void) {
    constexpr int size = 2;
    constexpr double prob = 0.5;
    std::negative_binomial_distribution<int> dist(size, prob);
//  std::negative_binomial_distribution<double> dist(size, prob);
    std::mt19937 randGen;
    for(int i=0; i<10000; ++i) {
       std::cout << dist(randGen);
    }
}

void boostMathCppNbinom(void) {
    constexpr double size = 0.99;
    constexpr double prob = 0.5;
    boost::math::negative_binomial dist(size, prob);

    for(double p=0.1; p<0.91; p += 0.1) {
       std::cout << boost::math::cdf(dist, p);
    };
}

void boostRandombinomLarge(void) {
    constexpr int size = 2;
    constexpr double prob = 0.5;
    boost::random::negative_binomial_distribution<int, double> dist(size, prob);
//  boost::random::negative_binomial_distribution<double, double> dist(size, prob);
    std::mt19937 randGen;
    for(int i=0; i<10000; ++i) {
       std::cout << dist(randGen);
    }
}

// This code occurs a runtime assertion failure
void boostRandombinomSmall(void) {
#if 0
    constexpr int size = 0.4;
    constexpr double prob = 0.3;
    boost::random::negative_binomial_distribution<int, double> dist(size, prob);
    std::mt19937 randGen;
    for(int i=0; i<10000; ++i) {
       std::cout << dist(randGen);
    }
#endif
}

int main(int argc, char* argv[]) {
    std::cout << "BOOST_VERSION=" << BOOST_VERSION << "\n";
    cppRandombinom();
    boostMathCppNbinom();
    boostRandombinomLarge();
    boostRandombinomSmall();

    rnbinomCppAll();
    nbinomCppCdf();
    std::cout << "Everything is OK\n";
    return 0;
}

/*
Local Variables:
mode: c++
coding: utf-8-dos
tab-width: nil
c-file-style: "stroustrup"
End:
*/


//
// Copyright 2020 Debabrata Mandal <mandaldebabrata123@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_GIL_IMAGE_PROCESSING_ADAPTIVE_HISTOGRAM_EQUALIZATION_HPP
#define BOOST_GIL_IMAGE_PROCESSING_ADAPTIVE_HISTOGRAM_EQUALIZATION_HPP

#include <boost/gil/algorithm.hpp>
#include <boost/gil/histogram.hpp>
#include <boost/gil/image.hpp>
#include <boost/gil/image_processing/histogram_equalization.hpp>
#include <boost/gil/image_view_factory.hpp>

#include <cmath>
#include <map>
#include <vector>

namespace boost { namespace gil {

/////////////////////////////////////////
/// Adaptive Histogram Equalization(AHE)
/////////////////////////////////////////
/// \defgroup AHE AHE
/// \brief Contains implementation and description of the algorithm used to compute
///        adaptive histogram equalization of input images. Naming for the AHE functions
///        are done in the following way
///             <feature-1>_<feature-2>_.._<feature-n>ahe
///        For example, for AHE done using local (non-overlapping) tiles/blocks and
///        final output interpolated among tiles , it is called
///             non_overlapping_interpolated_clahe
///

namespace detail {

/// \defgroup AHE-helpers AHE-helpers
/// \brief AHE helper functions

/// \fn double actual_clip_limit
/// \ingroup AHE-helpers
/// \brief Computes the actual clip limit given a clip limit value using binary search.
///        Reference -  Adaptive Histogram Equalization and Its Variations
///                     (http://www.cs.unc.edu/techreports/86-013.pdf, Pg - 15)
///
template <typename SrcHist>
double actual_clip_limit(SrcHist const& src_hist, double cliplimit = 0.03)
{
    double epsilon       = 1.0;
    using value_t        = typename SrcHist::value_type;
    double sum           = src_hist.sum();
    std::size_t num_bins = src_hist.size();

    cliplimit = sum * cliplimit;
    long low = 0, high = cliplimit, middle = low;
    while (high - low >= 1)
    {
        middle      = (low + high + 1) >> 1;
        long excess = 0;
        std::for_each(src_hist.begin(), src_hist.end(), [&](value_t const& v) {
            if (v.second > middle)
                excess += v.second - middle;
        });
        if (std::abs(excess - (cliplimit - middle) * num_bins) < epsilon)
            break;
        else if (excess > (cliplimit - middle) * num_bins)
            high = middle - 1;
        else
            low = middle + 1;
    }
    return middle / sum;
}

/// \fn void clip_and_redistribute
/// \ingroup AHE-helpers
/// \brief Clips and redistributes excess pixels based on the actual clip limit value
///        obtained from the other helper function actual_clip_limit
///        Reference - Graphic Gems 4, Pg. 474
///        (http://cas.xav.free.fr/Graphics%20Gems%204%20-%20Paul%20S.%20Heckbert.pdf)
///
template <typename SrcHist, typename DstHist>
void clip_and_redistribute(SrcHist const& src_hist, DstHist& dst_hist, double clip_limit = 0.03)
{
    using value_t            = typename SrcHist::value_type;
    double sum               = src_hist.sum();
    double actual_clip_value = detail::actual_clip_limit(src_hist, clip_limit);
    // double actual_clip_value = clip_limit;
    long actual_clip_limit = actual_clip_value * sum;
    double excess          = 0;
    std::for_each(src_hist.begin(), src_hist.end(), [&](value_t const& v) {
        if (v.second > actual_clip_limit)
            excess += v.second - actual_clip_limit;
    });
    std::for_each(src_hist.begin(), src_hist.end(), [&](value_t const& v) {
        if (v.second >= actual_clip_limit)
            dst_hist[dst_hist.key_from_tuple(v.first)] = clip_limit * sum;
        else
            dst_hist[dst_hist.key_from_tuple(v.first)] = v.second + excess / src_hist.size();
    });
    long rem = long(excess) % src_hist.size();
    if (rem == 0)
        return;
    long period       = round(src_hist.size() / rem);
    std::size_t index = 0;
    while (rem)
    {
        if (dst_hist(index) >= clip_limit * sum)
        {
            index = (index + 1) % src_hist.size();
        }
        dst_hist(index)++;
        rem--;
        index = (index + period) % src_hist.size();
    }
}

}  // namespace detail

/// \fn void non_overlapping_interpolated_clahe
/// \ingroup AHE
/// @param src_view      Input   Source image view
/// @param dst_view      Output  Output image view
/// @param tile_width_x  Input   Tile width along x-axis to apply HE
/// @param tile_width_y  Input   Tile width along x-axis to apply HE
/// @param clip_limit    Input   Clipping limit to be applied
/// @param bin_width     Input   Bin widths for histogram
/// @param mask          Input   Specify if mask is to be used
/// @param src_mask      Input   Mask on input image to ignore specified pixels
/// \brief Performs local histogram equalization on tiles of size (tile_width_x, tile_width_y)
///        Then uses the clip limit to redistribute excess pixels above the limit uniformly to
///        other bins. The clip limit is specified as a fraction i.e. a bin's value is clipped
///        if bin_value >= clip_limit * (Total number of pixels in the tile)
///
template <typename SrcView, typename DstView>
void non_overlapping_interpolated_clahe(
    SrcView const& src_view,
    DstView const& dst_view,
    std::size_t tile_width_x                = 20,
    std::size_t tile_width_y                = 20,
    double clip_limit                       = 0.03,
    std::size_t bin_width                   = 1.0,
    bool mask                               = false,
    std::vector<std::vector<bool>> src_mask = {})
{
    gil_function_requires<ImageViewConcept<SrcView>>();
    gil_function_requires<MutableImageViewConcept<DstView>>();

    static_assert(
        color_spaces_are_compatible<
            typename color_space_type<SrcView>::type,
            typename color_space_type<DstView>::type>::value,
        "Source and destination views must have same color space");

    using source_channel_t = typename channel_type<SrcView>::type;
    using dst_channel_t    = typename channel_type<DstView>::type;
    using coord_t          = typename SrcView::x_coord_t;

    std::size_t const channels = num_channels<SrcView>::value;
    coord_t const width        = src_view.width();
    coord_t const height       = src_view.height();

    // Find control points

    std::vector<coord_t> sample_x;
    coord_t sample_x1 = tile_width_x / 2;
    coord_t sample_x2 = (tile_width_x + 1) / 2;
    coord_t sample_y1 = tile_width_y / 2;
    coord_t sample_y2 = (tile_width_y + 1) / 2;

    auto extend_left   = tile_width_x;
    auto extend_top    = tile_width_y;
    auto extend_right  = (tile_width_x - width % tile_width_x) % tile_width_x + tile_width_x;
    auto extend_bottom = (tile_width_y - height % tile_width_y) % tile_width_y + tile_width_y;

    auto new_width  = width + extend_left + extend_right;
    auto new_height = height + extend_top + extend_bottom;

    image<typename SrcView::value_type> padded_img(new_width, new_height);

    auto top_left_x     = tile_width_x;
    auto top_left_y     = tile_width_y;
    auto bottom_right_x = tile_width_x + width;
    auto bottom_right_y = tile_width_y + height;

    copy_pixels(src_view, subimage_view(view(padded_img), top_left_x, top_left_y, width, height));

    for (std::size_t k = 0; k < channels; k++)
    {
        std::vector<histogram<source_channel_t>> prev_row(new_width / tile_width_x),
            next_row((new_width / tile_width_x));
        std::vector<std::map<source_channel_t, source_channel_t>> prev_map(
            new_width / tile_width_x),
            next_map((new_width / tile_width_x));

        coord_t prev = 0, next = 1;
        auto channel_view = nth_channel_view(view(padded_img), k);

        for (std::ptrdiff_t i = top_left_y; i < bottom_right_y; ++i)
        {
            if ((i - sample_y1) / tile_width_y >= next || i == top_left_y)
            {
                if (i != top_left_y)
                {
                    prev = next;
                    next++;
                }
                prev_row = next_row;
                prev_map = next_map;
                for (std::ptrdiff_t j = sample_x1; j < new_width; j += tile_width_x)
                {
                    auto img_view = subimage_view(
                        channel_view, j - sample_x1, next * tile_width_y,
                        std::max<int>(
                            std::min<int>(tile_width_x + j - sample_x1, bottom_right_x) -
                                (j - sample_x1),
                            0),
                        std::max<int>(
                            std::min<int>((next + 1) * tile_width_y, bottom_right_y) -
                                next * tile_width_y,
                            0));

                    fill_histogram(
                        img_view, next_row[(j - sample_x1) / tile_width_x], bin_width, false,
                        false);

                    detail::clip_and_redistribute(
                        next_row[(j - sample_x1) / tile_width_x],
                        next_row[(j - sample_x1) / tile_width_x], clip_limit);

                    next_map[(j - sample_x1) / tile_width_x] =
                        histogram_equalization(next_row[(j - sample_x1) / tile_width_x]);
                }
            }
            bool prev_row_mask = 1, next_row_mask = 1;
            if (prev == 0)
                prev_row_mask = false;
            else if (next + 1 == new_height / tile_width_y)
                next_row_mask = false;
            for (std::ptrdiff_t j = top_left_x; j < bottom_right_x; ++j)
            {
                bool prev_col_mask = true, next_col_mask = true;
                if ((j - sample_x1) / tile_width_x == 0)
                    prev_col_mask = false;
                else if ((j - sample_x1) / tile_width_x + 1 == new_width / tile_width_x - 1)
                    next_col_mask = false;

                // Bilinear interpolation
                point_t top_left(
                    (j - sample_x1) / tile_width_x * tile_width_x + sample_x1,
                                    prev * tile_width_y + sample_y1);
                point_t top_right(top_left.x + tile_width_x, top_left.y);
                point_t bottom_left(top_left.x, top_left.y + tile_width_y);
                point_t bottom_right(top_left.x + tile_width_x, top_left.y + tile_width_y);

                long double x_diff = top_right.x - top_left.x;
                long double y_diff = bottom_left.y - top_left.y;

                long double x1 = (j - top_left.x) / x_diff;
                long double x2 = (top_right.x - j) / x_diff;
                long double y1 = (i - top_left.y) / y_diff;
                long double y2 = (bottom_left.y - i) / y_diff;

                if (prev_row_mask == 0)
                    y1 = 1;
                else if (next_row_mask == 0)
                    y2 = 1;
                if (prev_col_mask == 0)
                    x1 = 1;
                else if (next_col_mask == 0)
                    x2 = 1;

                long double numerator =
                    ((prev_row_mask & prev_col_mask) * x2 *
                         prev_map[(top_left.x - sample_x1) / tile_width_x][channel_view(j, i)] +
                     (prev_row_mask & next_col_mask) * x1 *
                         prev_map[(top_right.x - sample_x1) / tile_width_x][channel_view(j, i)]) *
                        y2 +
                    ((next_row_mask & prev_col_mask) * x2 *
                         next_map[(bottom_left.x - sample_x1) / tile_width_x][channel_view(j, i)] +
                     (next_row_mask & next_col_mask) * x1 *
                         next_map[(bottom_right.x - sample_x1) / tile_width_x][channel_view(j, i)]) *
                        y1;

                if (mask && !src_mask[i - top_left_y][j - top_left_x])
                {
                    dst_view(j - top_left_x, i - top_left_y) =
                        channel_convert<dst_channel_t>(
                            static_cast<source_channel_t>(channel_view(i, j)));
                }
                else
                {
                    dst_view(j - top_left_x, i - top_left_y) =
                        channel_convert<dst_channel_t>(static_cast<source_channel_t>(numerator));
                }
            }
        }
    }
}

}}  //namespace boost::gil

#endif


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

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

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/distance.hpp>

#include <boost/geometry/strategies/spherical/distance_haversine.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track.hpp>

#include <boost/geometry/strategies/concepts/distance_concept.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/segment.hpp>

// This test is GIS oriented.

template <typename Point, typename LatitudePolicy>
void test_distance(
            typename bg::coordinate_type<Point>::type const& lon1,
            typename bg::coordinate_type<Point>::type const& lat1,
            typename bg::coordinate_type<Point>::type const& lon2,
            typename bg::coordinate_type<Point>::type const& lat2,
            typename bg::coordinate_type<Point>::type const& lon3,
            typename bg::coordinate_type<Point>::type const& lat3,
            typename bg::coordinate_type<Point>::type const& radius,
            typename bg::coordinate_type<Point>::type const& expected,
            typename bg::coordinate_type<Point>::type const& tolerance)
{
    typedef bg::strategy::distance::cross_track
        <
            Point,
            Point
        > strategy_type;
    typedef typename bg::strategy::distance::services::return_type
        <
            strategy_type
        >::type return_type;

    BOOST_CONCEPT_ASSERT
        (
            (bg::concept::PointSegmentDistanceStrategy<strategy_type>)
        );

    Point p1, p2, p3;
    bg::assign_values(p1, lon1, LatitudePolicy::apply(lat1));
    bg::assign_values(p2, lon2, LatitudePolicy::apply(lat2));
    bg::assign_values(p3, lon3, LatitudePolicy::apply(lat3));

    strategy_type strategy;
    return_type d = strategy.apply(p1, p2, p3);

    BOOST_CHECK_CLOSE(radius * d, expected, tolerance);

    // Test specifying radius explicitly
    strategy_type strategy_radius(radius);
    d = strategy_radius.apply(p1, p2, p3);
    BOOST_CHECK_CLOSE(d, expected, tolerance);

    // Test the "default strategy" registration
    bg::model::referring_segment<Point const> segment(p2, p3);
    d = bg::distance(p1, segment);
    BOOST_CHECK_CLOSE(radius * d, expected, tolerance);
}

template <typename Point, typename LatitudePolicy>
void test_all()
{
    typename bg::coordinate_type<Point>::type const average_earth_radius = 6372795.0;

    // distance (Paris <-> Amsterdam/Barcelona),
    // with coordinates rounded as below ~87 km
    // is equal to distance (Paris <-> Barcelona/Amsterdam)
    typename bg::coordinate_type<Point>::type const p_to_ab = 86.798321 * 1000.0;
    test_distance<Point, LatitudePolicy>(2, 48, 4, 52, 2, 41, average_earth_radius, p_to_ab, 0.1);
    test_distance<Point, LatitudePolicy>(2, 48, 2, 41, 4, 52, average_earth_radius, p_to_ab, 0.1);
}

int test_main(int, char* [])
{
    test_all<bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree> >, geographic_policy >();

    // NYI: haversine for mathematical spherical coordinate systems
    // test_all<bg::model::point<double, 2, bg::cs::spherical<bg::degree> >, mathematical_policya >();

#if defined(HAVE_TTMATH)
    typedef ttmath::Big<1,4> tt;
    //test_all<bg::model::point<tt, 2, bg::cs::geographic<bg::degree> >, geographic_policy>();
#endif

    return 0;
}


#include <boost/math/special_functions/beta.hpp>


/**
 * @file Tools/Math/Rotation.hpp
 * Rotation related functionality
 * @author Alexis Tsogias
 */

#pragma once

#include <Eigen/Dense>
#include "nao_ik/bhuman/Angle.hpp"
#include "nao_ik/bhuman/Approx.hpp"

using namespace Eigen;

using Vector3a = Matrix<Angle, 3, 1>;
typedef Quaternion<float> Quaternionf;

namespace Rotation
{
  Quaternionf aroundX(float angle);
  Quaternionf aroundY(float angle);
  Quaternionf aroundZ(float angle);

  /**
   * The spherical linear interpolation (slerp) between the two rotations.
   * Where interpolate(0.0f, q1, q2) = q1 and interpolate(1.0f, q1, q2) = q2.
   * @param t interpolation factor. Range: [0.0f, 1.0f].
   */
  Quaternionf interpolate(float t, const Quaternionf& q1, const Quaternionf& q2);

  Quaternionf removeZRotation(const Quaternionf& rotation);
  Quaternionf splitOffZRotation(const Quaternionf& rotation, Quaternionf& zRot);

  namespace Euler
  {
    // Euler angles are expressed in z y x manner.

    Quaternionf fromAngles(float x, float y, float z);
    Quaternionf fromAngles(const Vector3a& rotation);
    Quaternionf fromAngles(const Vector3f& rotation);
    Vector3f getAngles(const Quaternionf& rot);
    float getXAngle(const Quaternionf& rot);
    float getYAngle(const Quaternionf& rot);
    float getZAngle(const Quaternionf& rot);
  }

  namespace Aldebaran
  {
    // Aldebaran uses these angles when calculating the orientation of the robot via the imu
    float getXAngle(const Quaternionf& rot);
    float getXAngle(const Matrix3f& rot);
    float getYAngle(const Quaternionf& rot);
    float getYAngle(const Matrix3f& rot);
  }

  namespace AngleAxis
  {
    // Do not missinterpret the vectors as Euler angles!
    // Vector3f pack(const AngleAxisf& angleAxis);
    AngleAxisf unpack(const Vector3f& angleAxisVec);
  }
}

inline Quaternionf Rotation::aroundX(float angle)
{
  return Quaternionf(AngleAxisf(angle, Vector3f::UnitX()));
}

inline Quaternionf Rotation::aroundY(float angle)
{
  return Quaternionf(AngleAxisf(angle, Vector3f::UnitY()));
}

inline Quaternionf Rotation::aroundZ(float angle)
{
  return Quaternionf(AngleAxisf(angle, Vector3f::UnitZ()));
}

inline Quaternionf Rotation::interpolate(float t, const Quaternionf& q1, const Quaternionf& q2)
{
  return q1.slerp(t, q2);
}

inline Quaternionf Rotation::removeZRotation(const Quaternionf& rotation)
{
  const Vector3f& z = Vector3f::UnitZ();
  const Vector3f zR = rotation.inverse() * z;
  const Vector3f c = zR.cross(z);
  const float sin = c.norm();
  const float cos = zR.dot(z);
  if(Approx::isZero(sin))
    if(cos < 0.f) // 180 degree rotation
      return rotation; // There's no unique decomposition.
    else
      return Quaternionf::Identity();
  else
  {
    const float angle = std::atan2(sin, cos);
    return Quaternionf(AngleAxisf(angle, c.normalized()));
  }
}

inline Quaternionf Rotation::splitOffZRotation(const Quaternionf& rotation, Quaternionf& zRot)
{
  const Quaternionf xyRot = removeZRotation(rotation);
  zRot = rotation * xyRot.inverse();
  return xyRot;
}

inline Quaternionf Rotation::Euler::fromAngles(float x, float y, float z)
{
  return AngleAxisf(z, Vector3f::UnitZ()) * AngleAxisf(y, Vector3f::UnitY()) * AngleAxisf(x, Vector3f::UnitX());
}

inline Quaternionf Rotation::Euler::fromAngles(const Vector3a& rotation)
{
  return fromAngles(rotation.x(), rotation.y(), rotation.z());
}

inline Quaternionf Rotation::Euler::fromAngles(const Vector3f& rotation)
{
  return fromAngles(rotation.x(), rotation.y(), rotation.z());
}

inline Vector3f Rotation::Euler::getAngles(const Quaternionf& rot)
{
  const Matrix3f mat = rot.normalized().toRotationMatrix();
  const float m20 = mat(2, 0);

  if(std::abs(m20) < 0.999999f)
  {
    const float m00 = mat(0, 0);
    const float m10 = mat(1, 0);
    const float m21 = mat(2, 1);
    const float m22 = mat(2, 2);

    const float y1 = -std::asin(m20);
    const float y2 = pi - y1;
    const float cy1 = std::cos(y1);
    const float cy2 = std::cos(y2);
    const float x1 = std::atan2(m21 / cy1, m22 / cy1);
    const float x2 = std::atan2(m21 / cy2, m22 / cy2);
    const float z1 = std::atan2(m10 / cy1, m00 / cy1);
    const float z2 = std::atan2(m10 / cy2, m00 / cy2);

    Vector3f v1(x1, y1, z1);
    Vector3f v2(x2, y2, z2);

    return v1.norm() < v2.norm() ? v1 : v2;
  }
  else
  {
    const float x = std::atan2(mat(0, 1), mat(0, 2)); // x = +-z + atan2(...) , but we set z = 0...
    const float y = -std::asin(m20);

    return Vector3f(x, y, 0.f);
  }
}

inline float Rotation::Euler::getXAngle(const Quaternionf& rot)
{
  return getAngles(rot).x();
}

inline float Rotation::Euler::getYAngle(const Quaternionf& rot)
{
  return getAngles(rot).y();
}

inline float Rotation::Euler::getZAngle(const Quaternionf& rot)
{
  return getAngles(rot).z();
}

inline float Rotation::Aldebaran::getXAngle(const Quaternionf& rot)
{
  //     | a b c |        | a d g |        | 0 |   | g |
  // A = | d e f |   At = | b e h |   At * | 0 | = | h | = v
  //     | g h i |        | c f i |        | 1 |   | i |

  // Project        | 0 |
  // v into  : v -> | h | = v'
  // yz plane       | i |

  // calculate angle      | 0 |       angleX = acos((v' * z) / (|v'| * |z|))
  // bewteen v' and : z = | 0 |,  <=> angleX = acos(i / |v'|)
  // z-axis of A          | 1 |   <=> angleX = acos(i / sqrt(h * h + i * i))

  const float tx = 2.f * rot.x();
  const float h = 2.f * rot.z() * rot.y() + tx * rot.w();
  const float i = 1.f - (tx * rot.x() + 2.f * rot.y() * rot.y());

  const float len = std::sqrt(h * h + i * i);
  return len > 1e-5f ? std::acos(i / len) * (h > 0.f ? 1.f : -1.f) : 0.f;
}

inline float Rotation::Aldebaran::getXAngle(const Matrix3f& rot)
{
  //     | a b c |        | a d g |        | 0 |   | g |
  // A = | d e f |   At = | b e h |   At * | 0 | = | h | = v
  //     | g h i |        | c f i |        | 1 |   | i |

  // Project        | 0 |
  // v into  : v -> | h | = v'
  // yz plane       | i |

  // calculate angle      | 0 |       angleX = acos((v' * z) / (|v'| * |z|))
  // bewteen v' and : z = | 0 |,  <=> angleX = acos(i / |v'|)
  // z-axis of A          | 1 |   <=> angleX = acos(i / sqrt(h * h + i * i))

  const float h = rot(2, 1);
  const float i = rot(2, 2);

  const float len = std::sqrt(h * h + i * i);
  return len > 1e-5f ? std::acos(i / len) * (h > 0.f ? 1.f : -1.f) : 0.f;
}

inline float Rotation::Aldebaran::getYAngle(const Quaternionf& rot)
{
  //     | a b c |        | a d g |        | 0 |   | g |
  // A = | d e f |   At = | b e h |   At * | 0 | = | h | = v
  //     | g h i |        | c f i |        | 1 |   | i |

  // Project        | g |
  // v into  : v -> | 0 | = v'
  // xz plane       | i |

  // calculate angle      | 0 |       angleX = acos((v' * z) / (|v'| * |z|))
  // bewteen v' and : z = | 0 |,  <=> angleX = acos(i / |v'|)
  // z-axis of A          | 1 |   <=> angleX = acos(i / sqrt(g * g + i * i))

  // calculate g and i

  const float ty = 2.f * rot.y();
  const float g = 2.f * rot.z() * rot.x() - ty * rot.w();
  const float i = 1.f - (2.f * rot.x() * rot.x() + ty * rot.y());

  const float len = std::sqrt(g * g + i * i);
  return len > 1e-5f ? std::acos(i / len) * (g > 0.f ? -1.f : 1.f) : 0.f;
}

inline float Rotation::Aldebaran::getYAngle(const Matrix3f& rot)
{
  //     | a b c |        | a d g |        | 0 |   | g |
  // A = | d e f |   At = | b e h |   At * | 0 | = | h | = v
  //     | g h i |        | c f i |        | 1 |   | i |

  // Project        | g |
  // v into  : v -> | 0 | = v'
  // xz plane       | i |

  // calculate angle      | 0 |       angleX = acos((v' * z) / (|v'| * |z|))
  // bewteen v' and : z = | 0 |,  <=> angleX = acos(i / |v'|)
  // z-axis of A          | 1 |   <=> angleX = acos(i / sqrt(g * g + i * i))

  const float g = rot(2, 0);
  const float i = rot(2, 2);

  const float len = std::sqrt(g * g + i * i);
  return len > 1e-5f ? std::acos(i / len) * (g > 0.f ? -1.f : 1.f) : 0.f;
}

// inline Vector3f Rotation::AngleAxis::pack(const AngleAxisf& angleAxis)
// {
//   const float angle = angleAxis.angle();
//   if(Approx::isZero(angle))
//     return Vector3f::Zero();
//   else
//     return angleAxis.axis().normalized(angle);
// }

inline AngleAxisf Rotation::AngleAxis::unpack(const Vector3f& angleAxisVec)
{
  const float angle = angleAxisVec.norm();
  if(Approx::isZero(angle))
    return AngleAxisf::Identity();
  else
    return AngleAxisf(angle, angleAxisVec.normalized());
}


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

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ACOSPI_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ACOSPI_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse cosine in \f$\pi\f$ multiples.

    @par Header <boost/simd/function/acospi.hpp>

    @par Decorators

       - pedantic_     is similar to Invpi<T>*pedantic_(acos)(x);

    @see acos, acospi, cosd

    @par Example:

      @snippet acospi.cpp acospi

    @par Possible output:

      @snippet acospi.txt acospi

  **/
  IEEEValue acospi(IEEEValue const & x);
} }
#endif

#include <boost/simd/function/scalar/acospi.hpp>
#include <boost/simd/function/simd/acospi.hpp>

#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2007 Ferdinando Ametrano

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file sabrvolsurface.hpp
    \brief SABR volatility (smile) surface
*/

#ifndef quantlib_sabr_vol_surface_hpp
#define quantlib_sabr_vol_surface_hpp

#include <ql/experimental/volatility/interestratevolsurface.hpp>
#include <ql/experimental/volatility/blackatmvolcurve.hpp>
#include <ql/quote.hpp>
#include <ql/termstructures/volatility/sabrinterpolatedsmilesection.hpp>
#include <boost/array.hpp>

namespace QuantLib {

    //! SABR volatility (smile) surface
    /*! blah blah
    */
    class SabrVolSurface : public InterestRateVolSurface {
      public:
        SabrVolSurface(
                const ext::shared_ptr<InterestRateIndex>&,
                const Handle<BlackAtmVolCurve>&,
                const std::vector<Period>& optionTenors,
                const std::vector<Spread>& atmRateSpreads,
                const std::vector<std::vector<Handle<Quote> > >& volSpreads);
        //@}
        // All virtual methods of base classes must be forwarded
        //! \name TermStructure interface
        //@{
        DayCounter dayCounter() const override;
        Date maxDate() const override;
        Time maxTime() const override;
        const Date& referenceDate() const override;
        Calendar calendar() const override;
        Natural settlementDays() const override;
        //@}
        //! \name VolatilityTermStructure interface
        //@{
        Real minStrike() const override;
        Real maxStrike() const override;
        //@}
        const Handle<BlackAtmVolCurve>& atmCurve() const;
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
        std::vector<Volatility> volatilitySpreads(const Period&) const;
        std::vector<Volatility> volatilitySpreads(const Date&) const;
      protected:
        boost::array<Real, 4> sabrGuesses(const Date&) const;
      public:
        //@}
        //! \name BlackVolSurface interface
        //@{
        ext::shared_ptr<SmileSection> smileSectionImpl(Time) const override;
        //@}
      protected:
        //@}
        //! \name LazyObject interface
        //@{
        void performCalculations () const;
        void update() override;
        //@}
      private:
        void registerWithMarketData();
        void checkInputs() const;
        void updateSabrGuesses(const Date& d, boost::array<Real, 4> newGuesses) const;
        Handle<BlackAtmVolCurve> atmCurve_;
        std::vector<Period> optionTenors_;
        std::vector<Time> optionTimes_;
        std::vector<Date> optionDates_;
        std::vector<Spread> atmRateSpreads_;
        std::vector<std::vector<Handle<Quote> > > volSpreads_;
        //
        bool isAlphaFixed_;
        bool isBetaFixed_;
        bool isNuFixed_;
        bool isRhoFixed_;
        bool vegaWeighted_;
        //
        mutable std::vector<boost::array<Real,4> > sabrGuesses_;
    };

    // inline

    inline DayCounter SabrVolSurface::dayCounter() const {
        return atmCurve_->dayCounter();
    }

    inline Date SabrVolSurface::maxDate() const {
        return atmCurve_->maxDate();
    }

    inline Time SabrVolSurface::maxTime() const {
        return atmCurve_->maxTime();
    }

    inline const Date& SabrVolSurface::referenceDate() const {
        return atmCurve_->referenceDate();
    }

    inline Calendar SabrVolSurface::calendar() const {
        return atmCurve_->calendar();
    }

    inline Natural SabrVolSurface::settlementDays() const {
        return atmCurve_->settlementDays();
    }

    inline Real SabrVolSurface::minStrike() const {
        return QL_MIN_REAL;
    }

    inline Real SabrVolSurface::maxStrike() const {
        return QL_MAX_REAL;
    }

    inline const Handle<BlackAtmVolCurve>& SabrVolSurface::atmCurve() const {
        return atmCurve_;
    }

    inline std::vector<Volatility>
    SabrVolSurface::volatilitySpreads(const Period& p) const {
        return volatilitySpreads(optionDateFromTenor(p));
    }
}

#endif


#include <boost/test/unit_test.hpp>

#include <crave/utils/Evaluator.hpp>
#include <crave/ir/UserConstraint.hpp>
#include <crave/ir/UserExpression.hpp>

#include <set>
#include <iostream>

// using namespace std;
using namespace crave;

BOOST_FIXTURE_TEST_SUITE(Evaluations_t, Context_Fixture)

BOOST_AUTO_TEST_CASE(logical_not_t1) {
  Variable<unsigned int> a;
  Evaluator evaluator;

  evaluator.assign(a, 0u);

  BOOST_REQUIRE(evaluator.evaluate(!(a != 0)));
  BOOST_REQUIRE(evaluator.result<bool>());

  evaluator.assign(a, 42u);

  BOOST_REQUIRE(evaluator.evaluate(!(a == 0)));
  BOOST_REQUIRE(evaluator.result<bool>());
}

BOOST_AUTO_TEST_CASE(logical_not_t2) {
  Variable<unsigned char> a;
  Variable<unsigned int> b;
  Evaluator eval;
  expression expr = make_expression(if_then_else(!(a % 2 == 0), b > 0 && b <= 50, b > 50 && b <= 100));

  eval.assign(a, 1u);

  BOOST_REQUIRE(!eval.evaluate(expr));

  eval.assign(b, 35u);

  BOOST_REQUIRE(eval.evaluate(expr));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(a, 2u);
  eval.assign(b, 75u);

  BOOST_REQUIRE(eval.evaluate(expr));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(a, 1u);

  BOOST_REQUIRE(eval.evaluate(expr));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(logical_and_t1) {
  Variable<bool> a;
  Variable<bool> b;
  Variable<bool> c;
  Evaluator eval;

  eval.assign(a, true);
  eval.assign(b, true);

  BOOST_REQUIRE(eval.evaluate(a && b));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(c, false);

  BOOST_REQUIRE(eval.evaluate(c == (a && b)));
  BOOST_REQUIRE(!eval.result<bool>());

  eval.assign(a, false);
  eval.assign(b, false);

  BOOST_REQUIRE(eval.evaluate(c == (a && b)));
  BOOST_REQUIRE(eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(logical_or_t1) {
  Variable<bool> a;
  Variable<bool> b;
  Variable<bool> c;
  Evaluator eval;

  eval.assign(a, false);
  eval.assign(b, false);

  BOOST_REQUIRE(eval.evaluate(a || b));
  BOOST_REQUIRE(!eval.result<bool>());

  eval.assign(c, false);

  BOOST_REQUIRE(eval.evaluate(c == (a || b)));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(a, true);
  eval.assign(c, true);

  BOOST_REQUIRE(eval.evaluate(c == (a || b)));
  BOOST_REQUIRE(eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(equal_t1) {
  Variable<unsigned int> a;
  Variable<unsigned int> b;
  Evaluator eval;

  eval.assign(a, 65535);

  BOOST_REQUIRE(eval.evaluate(a));
  BOOST_REQUIRE_EQUAL(eval.result<unsigned int>(), 65535);
  BOOST_REQUIRE(eval.evaluate(a == 65535));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(b, 5);

  BOOST_REQUIRE(eval.evaluate(a == b));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(not_equal_t1) {
  Variable<unsigned int> a;
  Variable<unsigned int> b;
  Evaluator eval;

  eval.assign(a, 25u);

  for (unsigned int i = 0; i < 50u; ++i) {
    eval.assign(b, i);
    BOOST_REQUIRE(eval.evaluate(a != b));

    if (i != 25u)
      BOOST_REQUIRE(eval.result<bool>());
    else
      BOOST_REQUIRE(!eval.result<bool>());
  }
}

BOOST_AUTO_TEST_CASE(less) {
  Variable<unsigned> a;
  Variable<unsigned> b;
  Evaluator eval;

  for (unsigned int i = 0u; i < 50u; ++i) {
    eval.assign(a, i);
    BOOST_REQUIRE(eval.evaluate(a < 50u));
    BOOST_REQUIRE(eval.result<bool>());

    eval.assign(b, i);
    BOOST_REQUIRE(eval.evaluate(b < 50u));
    BOOST_REQUIRE(eval.result<bool>());
  }
}

BOOST_AUTO_TEST_CASE(less_equal) {
  Variable<unsigned> a;
  Variable<unsigned> b;
  Evaluator eval;

  for (unsigned int i = 0u; i <= 50u; ++i) {
    eval.assign(a, i);
    BOOST_REQUIRE(eval.evaluate(a <= 50u));
    BOOST_REQUIRE(eval.result<bool>());

    eval.assign(b, i);
    BOOST_REQUIRE(eval.evaluate(b <= 50u));
    BOOST_REQUIRE(eval.result<bool>());
  }
}

BOOST_AUTO_TEST_CASE(greater) {
  Variable<unsigned> a;
  Variable<unsigned> b;
  Evaluator eval;

  for (int i = 50; i > 0; --i) {
    eval.assign(a, i);
    BOOST_REQUIRE(eval.evaluate(a > 0u));
    BOOST_REQUIRE(eval.result<bool>());

    eval.assign(b, i);
    BOOST_REQUIRE(eval.evaluate(b > 0u));
    BOOST_REQUIRE(eval.result<bool>());
  }
}

BOOST_AUTO_TEST_CASE(greater_equal) {
  Variable<unsigned> a;
  Variable<unsigned> b;
  Evaluator eval;

  for (int i = 50; i >= 0; --i) {
    eval.assign(a, i);
    BOOST_REQUIRE(eval.evaluate(a >= 0u));
    BOOST_REQUIRE(eval.result<bool>());

    eval.assign(b, i);
    BOOST_REQUIRE(eval.evaluate(b >= 0u));
    BOOST_REQUIRE(eval.result<bool>());
  }
}

BOOST_AUTO_TEST_CASE(neg_t1) {
  Variable<int> a;
  Variable<int> b;
  Evaluator eval;

  eval.assign(a, 1337);
  BOOST_REQUIRE(eval.evaluate(-a == 1337));
  BOOST_REQUIRE(!eval.result<bool>());

  eval.assign(b, -1337);
  BOOST_REQUIRE(eval.evaluate(a == -b));
  BOOST_REQUIRE(eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(neg_t2) {
  int a = 1337;
  Variable<int> b;
  Evaluator eval;

  eval.assign(b, -a);
  BOOST_REQUIRE(eval.evaluate(b));
  BOOST_REQUIRE_EQUAL(eval.result<int>(), -1337);
}

BOOST_AUTO_TEST_CASE(complement_t1) {
  Variable<int> a;
  Variable<int> b;
  Evaluator eval;

  eval.assign(a, 0);
  eval.assign(b, -1);
  BOOST_REQUIRE(eval.evaluate(~a == b));
  BOOST_REQUIRE(eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(complement_t2) {
  int a = 42;
  Variable<int> b;
  Evaluator eval;
  eval.assign(b, a);
  BOOST_REQUIRE(eval.evaluate(~b));
  BOOST_REQUIRE_EQUAL(eval.result<int>(), -43);
}

BOOST_AUTO_TEST_CASE(bitwise_and_t1) {
  Variable<unsigned int> a;
  Variable<unsigned int> b;
  Evaluator eval;

  eval.assign(a, 42);
  eval.assign(b, 1337);

  BOOST_REQUIRE(eval.evaluate(a & b));
  BOOST_REQUIRE_EQUAL(eval.result<unsigned int>(), 40);
}

BOOST_AUTO_TEST_CASE(bitwise_or_t1) {
  Variable<unsigned int> a;
  Variable<unsigned int> b;
  Evaluator eval;

  eval.assign(a, 42);
  eval.assign(b, 1337);

  BOOST_REQUIRE(eval.evaluate(a | b));
  BOOST_REQUIRE_EQUAL(eval.result<unsigned int>(), 1339);
}

BOOST_AUTO_TEST_CASE(xor_t1) {
  Variable<bool> a;
  Variable<bool> b;
  Evaluator eval;

  eval.assign(a, false);
  eval.assign(b, false);

  BOOST_REQUIRE(eval.evaluate(a ^ b));
  BOOST_REQUIRE_EQUAL(eval.result<bool>(), false);

  eval.assign(b, true);

  BOOST_REQUIRE(eval.evaluate(a ^ b));
  BOOST_REQUIRE_EQUAL(eval.result<bool>(), true);
}

BOOST_AUTO_TEST_CASE(xor_t2) {
  Variable<unsigned int> a;
  Variable<unsigned int> b;
  Evaluator eval;

  eval.assign(a, 65535);
  eval.assign(b, 4080);

  BOOST_REQUIRE(eval.evaluate(a ^ b));
  BOOST_REQUIRE_EQUAL(eval.result<unsigned int>(), 61455);
}

BOOST_AUTO_TEST_CASE(shiftleft) {
  Variable<unsigned> a;
  Variable<char> b;
  Evaluator eval;

  int count = 0;
  while (++count < 256) {
    eval.assign(a, count);
    eval.assign(b, count % (sizeof(unsigned) << 3u));

    BOOST_REQUIRE(eval.evaluate(a << b));
    BOOST_REQUIRE_EQUAL(eval.result<unsigned>(), count << (count % (sizeof(unsigned) << 3u)));
  }
}

BOOST_AUTO_TEST_CASE(shiftright) {
  Variable<unsigned> a;
  Variable<char> b;
  Evaluator eval;

  int count = 0;
  while (256 > ++count) {
    eval.assign(a, count + 256);
    eval.assign(b, count % 8);

    BOOST_REQUIRE(eval.evaluate(a >> b));
    BOOST_REQUIRE_EQUAL(eval.result<unsigned>(), (count + 256) >> (count % 8));
  }
}

BOOST_AUTO_TEST_CASE(plus_minus) {
  Variable<unsigned int> a;
  Variable<unsigned int> b;
  Evaluator eval;

  unsigned int cnt = 0;
  while (cnt++ < 300) {
    eval.assign(a, cnt * cnt);
    eval.assign(b, cnt + cnt);

    BOOST_REQUIRE(eval.evaluate(a + b));
    BOOST_REQUIRE_EQUAL(eval.result<unsigned>(), (cnt * cnt) + (cnt + cnt));

    BOOST_REQUIRE(eval.evaluate(a - b));
    BOOST_REQUIRE_EQUAL(eval.result<unsigned>(), (cnt * cnt) - (cnt + cnt));
  }
}

BOOST_AUTO_TEST_CASE(mult_mod) {
  Variable<int> a;
  Variable<int> b;
  Evaluator eval;

  for (int i = -3; i <= 3; i++) {
    for (int j = -3; j <= 3; j++) {
      eval.assign(a, i);
      eval.assign(b, j);

      BOOST_REQUIRE(eval.evaluate(a * b % 6));
      BOOST_REQUIRE_EQUAL(eval.result<int>(), i * j % 6);
    }
  }

  eval.assign(b, 0);

  BOOST_REQUIRE(!eval.evaluate(a % b));
}

BOOST_AUTO_TEST_CASE(divide) {
  Variable<short> a;
  Variable<short> b;
  Evaluator eval;

  unsigned int cnt = 1;
  while (cnt++ < 256) {
    eval.assign(a, cnt * cnt);
    eval.assign(b, cnt + cnt);

    BOOST_REQUIRE(eval.evaluate(a / b));
    BOOST_REQUIRE_EQUAL(eval.result<short>(), (cnt * cnt) / (cnt + cnt));

    BOOST_REQUIRE(eval.evaluate(a % b));
    BOOST_REQUIRE_EQUAL(eval.result<short>(), (cnt * cnt) % (cnt + cnt));
  }

  eval.assign(b, 0u);
  BOOST_REQUIRE(!eval.evaluate(a / b));
}

BOOST_AUTO_TEST_CASE(element_inside_set) {
  std::set<unsigned> s;
  s.insert(1);
  s.insert(7);
  s.insert(9);

  Variable<unsigned> x;
  Evaluator eval;

  eval.assign(x, 1);

  BOOST_REQUIRE(eval.evaluate(inside(x, s)));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(x, 5);

  BOOST_REQUIRE(eval.evaluate(inside(x, s)));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(element_inside_vec) {
  std::vector<unsigned> v;
  v.push_back(1);
  v.push_back(7);
  v.push_back(9);

  Variable<unsigned> x;
  Evaluator eval;

  eval.assign(x, 7u);

  BOOST_REQUIRE(eval.evaluate(inside(x, v)));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(x, 5u);

  BOOST_REQUIRE(eval.evaluate(inside(x, v)));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(element_inside_array) {
  unsigned a[3];
  a[0] = 1;
  a[1] = 7;
  a[2] = 9;

  Variable<unsigned> x;
  Evaluator eval;

  eval.assign(x, 9);

  BOOST_REQUIRE(eval.evaluate(inside(x, a)));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(x, 5u);

  BOOST_REQUIRE(eval.evaluate(inside(x, a)));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(element_inside_list) {
  std::list<unsigned> l;
  l.push_back(1);
  l.push_back(7);
  l.push_back(9);

  Variable<unsigned> x;
  Evaluator eval;

  eval.assign(x, 7u);

  BOOST_REQUIRE(eval.evaluate(inside(x, l)));
  BOOST_REQUIRE(eval.result<bool>());

  eval.assign(x, 5u);

  BOOST_REQUIRE(eval.evaluate(inside(x, l)));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(element_not_inside) {
  Evaluator eval;
  {
    std::set<unsigned> s;
    Variable<unsigned> x;
    eval.assign(x, 1u);

    BOOST_REQUIRE(eval.evaluate(inside(x, s)));
    BOOST_REQUIRE(!eval.result<bool>());

    s.insert(1u);

    BOOST_REQUIRE(eval.evaluate(inside(x, s)));
    BOOST_REQUIRE(eval.result<bool>());
  }
  {
    std::vector<unsigned> v;

    Variable<unsigned> x;
    eval.assign(x, 1u);

    BOOST_REQUIRE(eval.evaluate(inside(x, v)));
    BOOST_REQUIRE(!eval.result<bool>());

    v.push_back(1u);

    BOOST_REQUIRE(eval.evaluate(inside(x, v)));
    BOOST_REQUIRE(eval.result<bool>());
  }
  {
    std::vector<unsigned> l;
    Variable<unsigned> x;
    eval.assign(x, 1u);

    BOOST_REQUIRE(eval.evaluate(inside(x, l)));
    BOOST_REQUIRE(!eval.result<bool>());

    l.push_back(1u);

    BOOST_REQUIRE(eval.evaluate(inside(x, l)));
    BOOST_REQUIRE(eval.result<bool>());
  }
}

BOOST_AUTO_TEST_CASE(if_then_else_t1) {
  Variable<unsigned int> a;
  Variable<unsigned int> b;
  Evaluator eval;
  expression expr = make_expression(if_then_else(a<5, b> 0 && b <= 50, b > 50 && b <= 100));

  for (int i = 0; i < 10; ++i) {
    eval.assign(a, i);
    eval.assign(b, 25);
    BOOST_REQUIRE(eval.evaluate(expr));

    if (i < 5) {
      BOOST_REQUIRE(eval.result<bool>());
    } else {
      BOOST_REQUIRE(!eval.result<bool>());
    }
    eval.assign(b, 75);
    BOOST_REQUIRE(eval.evaluate(expr));

    if (i < 5) {
      BOOST_REQUIRE(!eval.result<bool>());
    } else {
      BOOST_REQUIRE(eval.result<bool>());
    }
  }
}

BOOST_AUTO_TEST_CASE(if_then_t1) {
  Variable<unsigned int> a;
  Variable<unsigned int> b;
  Evaluator eval;
  expression expr = make_expression(if_then(a<5, b> 0 && b <= 100));

  for (int i = 0; i < 10; ++i) {
    eval.assign(a, i);
    eval.assign(b, 25u);
    BOOST_REQUIRE(eval.evaluate(expr));
    BOOST_REQUIRE(eval.result<bool>());

    eval.assign(b, 705u);
    BOOST_REQUIRE(eval.evaluate(expr));

    if (i < 5) {
      BOOST_REQUIRE(!eval.result<bool>());
    } else {
      BOOST_REQUIRE(eval.result<bool>());
    }
  }
}

BOOST_AUTO_TEST_CASE(equal_t2) {
  Variable<unsigned int> a;
  Variable<unsigned int> b;
  Evaluator eval;

  eval.assign(a, 1);
  eval.assign(b, 2);

  BOOST_REQUIRE(eval.evaluate(a == b));
  BOOST_REQUIRE(!eval.result<bool>());
}

BOOST_AUTO_TEST_CASE(equal_t3) {
  Variable<unsigned int> a;
  Variable<unsigned int> b;
  Variable<unsigned int> c;
  Evaluator eval;

  eval.assign(a, 1);
  eval.assign(b, 2);
  eval.assign(c, 3);

  BOOST_REQUIRE(eval.evaluate(a + b == c));
  BOOST_REQUIRE(eval.result<bool>());
}

BOOST_AUTO_TEST_SUITE_END()  // Evaluations


/*
 * Copyright 2019,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#include <iostream>
#include <sot/core/debug.hh>

#ifndef WIN32
#include <unistd.h>
#endif

using namespace std;

#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
#include <sot/core/madgwickahrs.hh>
#include <sstream>

using namespace dynamicgraph;
using namespace dynamicgraph::sot;

#define BOOST_TEST_MODULE test - filter - differentiator

#include <boost/test/output_test_stream.hpp>
#include <boost/test/unit_test.hpp>

using boost::test_tools::output_test_stream;

BOOST_AUTO_TEST_CASE(test_filter_differentiator) {
  sot::MadgwickAHRS *aFilter = new MadgwickAHRS("MadgwickAHRS");

  double timestep = 0.001, beta = 0.01;
  aFilter->init(timestep);
  aFilter->set_beta(beta);

  srand(0);
  dynamicgraph::Vector acc(3);
  dynamicgraph::Vector angvel(3);
  acc(0) = 0.3;
  acc(1) = 0.2;
  acc(2) = 0.3;
  aFilter->m_accelerometerSIN = acc;
  angvel(0) = 0.1;
  angvel(1) = -0.1;
  angvel(2) = 0.3;
  aFilter->m_gyroscopeSIN = angvel;
  aFilter->m_imu_quatSOUT.recompute(0);
  output_test_stream output;
  ostringstream anoss;
  aFilter->m_imu_quatSOUT.get(output);
  aFilter->m_imu_quatSOUT.get(anoss);

  BOOST_CHECK(output.is_equal("1 "
                              "5.5547e-05 "
                              "-5.83205e-05 "
                              "0.00015"));
}


//---------------------------------------------------------------------------//
//!
//! \file   Utility_GeneralSphericalSpatialCoordinateConversionPolicy.hpp
//! \author Alex Robinson
//! \brief  General spherical spatial coordinate conversion policy declaration
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_GENERAL_SPHERICAL_SPATIAL_COORDINATE_CONVERSION_POLICY_HPP
#define UTILITY_GENERAL_SPHERICAL_SPATIAL_COORDINATE_CONVERSION_POLICY_HPP

// Boost Includes
#include <boost/serialization/array_wrapper.hpp>

// FRENSIE Includes
#include "Utility_SphericalSpatialCoordinateConversionPolicy.hpp"

namespace Utility{

//! The general spherical spatial coordinate conversion policy class
class GeneralSphericalSpatialCoordinateConversionPolicy : public SphericalSpatialCoordinateConversionPolicy
{

public:

  //! Constructor
  GeneralSphericalSpatialCoordinateConversionPolicy( const double origin[3],
                                                     const double axis[3] );

  //! Destructor
  ~GeneralSphericalSpatialCoordinateConversionPolicy()
  { /* ... */ }

  //! Convert the spatial coordinates to cartesian coordinates
  void convertToCartesianSpatialCoordinates(
                                      const double primary_spatial_coord,
                                      const double secondary_spatial_coord,
                                      const double tertiary_spatial_coord,
                                      double& x_spatial_coord,
                                      double& y_spatial_coord,
                                      double& z_spatial_coord ) const override;

  //! Convert the cartesian coordinates to the spatial coordinate system
  void convertFromCartesianSpatialCoordinates(
                               const double x_spatial_coord,
                               const double y_spatial_coord,
                               const double z_spatial_coord,
                               double& primary_spatial_coord,
                               double& secondary_spatial_coord,
                               double& tertiary_spatial_coord ) const override;

  //! Convert the spatial coordinates to cartesian coordinates
  using SphericalSpatialCoordinateConversionPolicy::convertToCartesianSpatialCoordinates;

  //! Convert the cartesian coordinates to the spatial coordinate system
  using SphericalSpatialCoordinateConversionPolicy::convertFromCartesianSpatialCoordinates;

private:

  // The default constructor should not be used - if the axis and origin
  // correspond to the global coordinate system use the basic conversion
  // policy
  GeneralSphericalSpatialCoordinateConversionPolicy()
  { /* ... */ }

  // We have C-arrays as members - hide the copy constructor and assignment
  // operator
  GeneralSphericalSpatialCoordinateConversionPolicy( const GeneralSphericalSpatialCoordinateConversionPolicy& that );
  GeneralSphericalSpatialCoordinateConversionPolicy& operator=( const GeneralSphericalSpatialCoordinateConversionPolicy& that );

  // Save the policy to an archive
  template<typename Archive>
  void save( Archive& ar, const unsigned version ) const;

  // Load the policy from an archive
  template<typename Archive>
  void load( Archive& ar, const unsigned version );

  BOOST_SERIALIZATION_SPLIT_MEMBER();

  // Declare the boost serialization access object as a friend
  friend class boost::serialization::access;

  // The origin of the spherical coordinate system w.r.t. the global Cartesian
  // coordinate system
  double d_origin[3];

  // The z-axis (unit vector) of the spherical coordinate system w.r.t. the
  // global Cartesian coordinate system
  double d_axis[3];
};

// Save the policy to an archive
template<typename Archive>
void GeneralSphericalSpatialCoordinateConversionPolicy::save( Archive& ar, const unsigned version ) const
{
  // Save the base class
  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP( SphericalSpatialCoordinateConversionPolicy );

  // Save the local data
  ar & boost::serialization::make_nvp( "d_origin", boost::serialization::make_array( d_origin, 3 ) );
  ar & boost::serialization::make_nvp( "d_axis", boost::serialization::make_array( d_axis, 3 ) );
}

// Load the policy from an archive
template<typename Archive>
void GeneralSphericalSpatialCoordinateConversionPolicy::load( Archive& ar, const unsigned version )
{
  // Load the base class
  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP( SphericalSpatialCoordinateConversionPolicy );

  // Load the local data
  ar & boost::serialization::make_nvp( "d_origin", boost::serialization::make_array( d_origin, 3 ) );
  ar & boost::serialization::make_nvp( "d_axis", boost::serialization::make_array( d_axis, 3 ) );
}

} // end Utility namespace

BOOST_SERIALIZATION_CLASS_VERSION( GeneralSphericalSpatialCoordinateConversionPolicy, Utility, 0 );
BOOST_SERIALIZATION_CLASS_EXPORT_STANDARD_KEY( GeneralSphericalSpatialCoordinateConversionPolicy, Utility );
EXTERN_EXPLICIT_CLASS_SAVE_LOAD_INST( Utility, GeneralSphericalSpatialCoordinateConversionPolicy );

#endif // end UTILITY_GENERAL_SPHERICAL_SPATIAL_COORDINATE_CONVERSION_POLICY_HPP

//---------------------------------------------------------------------------//
// end Utility_GeneralSphericalSpatialCoordinateConversionPolicy.hpp
//---------------------------------------------------------------------------//


#pragma once

//#define BOOST_RESULT_OF_USE_DECLTYPE
#include <type_traits>
#include <array>

#include<Eigen/Core>
#include<Eigen/StdVector>

#ifdef NDEBUG
#define BOOST_DISABLE_ASSERTS
#endif

#include <boost/multi_array.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/iterator_facade.hpp>

#include "tuple_tools.hpp"

namespace gftools {

/// container_base is a base class, that provides an interface of multidimensional array
/// with arithmetic operations (element-wise addition, multiplication, etc)
/// it is a wrapper over boost::multi_array and has 3 types : type of the stored numbers, rank and boost::multi_array type
/// TODO ValueType is a duplication here
template <typename ValueType, size_t N, typename BoostContainerType>
struct container_base;

/// container is a multidimensional array with math operations, that allocates and stores memory
template <typename ValueType, size_t N>
struct container;

/// container_view is a "view" of a container_base that allows to access elements with a different storage order
template<typename ContainerType, size_t M = ContainerType::N_>
using container_view = container_base<typename ContainerType::value_type, ContainerType::N_, typename ContainerType::boost_t::template array_view<M>::type>;

/// container_ref is a container without allocation of memory, i.e. it serves as an interface to an outside chunk of memory with all container operations
template <typename ValueType, size_t N>
using container_ref = container_base<ValueType, N, boost::multi_array_ref<ValueType,N>>;

/// container_traits is a helper ''traits'' class that provides type definitions
template <typename, bool View=false> struct container_traits;

template <typename ValueType, size_t N, typename BoostContainerType>
struct container_base
{
    /// total rank (number of dimensions) of the container_base
    constexpr static size_t N_ = N;
    /// a helper flag that is true, when the container_base is a view
    constexpr static bool is_view_ = (N != BoostContainerType::dimensionality);

    // typedefs
    /// typedef for stored values
    typedef ValueType value_type;
    /// typedef for a result of const operator[] operation (i.e. can be an array or a number)
    typedef typename container_traits<container_base, is_view_>::type under_type;
    /// typedef for a result of operator[] operation (i.e. can be an array or a number)
    typedef typename container_traits<container_base, is_view_>::ref_type under_ref_type;
    /// typedef for a resukt of operator[] of underlying boost::multi_array type
    typedef typename container_traits<container_base, is_view_>::boost_under_type boost_under_type;
    /// typedef wrapped boost::multi_array
    typedef BoostContainerType boost_t;
    /// typedef for a flattened array (Eigen::Array type)
    typedef Eigen::Array<ValueType, Eigen::Dynamic, 1> EigenArray;
    /// typedef for an Eigen::Map object that provides arithmetic operations on flattened arrays
    typedef Eigen::Map<EigenArray> EigenMap;
    /// typedef for a matrix with value_type values
    typedef Eigen::Matrix<ValueType,Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixType;
    /// typedef for a flattened Eigen::Vector, that provides vector*matrix operations
    typedef Eigen::Matrix<ValueType,Eigen::Dynamic, 1> VectorType;

    // helper type structues
    /// TODO : move to traits
    /// Is2d defines true_type if the objectr is 2d -> allows matrix operations
    template <size_t N2 = N, typename DT = boost_t>
    using Is2d = typename std::enable_if<N2==2,DT>::type;
    // helper typedefs for math operations (to be able to multiply container and a ref, container and container, container and a number etc)
    /// BaseRefIfContainer defines container_base type if the template parameter T is a container_base
    template <typename T>
    using BaseRefIfContainer = typename std::enable_if<T::N_>=1, container_base<ValueType,N,boost_t>&>::type;
    /// ContaierIfContainer defines container type if the template parameter T is a container_base
    template <typename T>
    using ContainerIfContainer = typename std::enable_if<T::N_>=1, container<ValueType,N>>::type;
    /// BaseRefIfValue defines container_base if the template parameter T is a number
    template <typename T>
    using BaseRefIfValue = typename std::enable_if<std::is_convertible<T,ValueType>::value, container_base<ValueType,N,boost_t>&>::type;
    /// BaseRefIfValue defines container if the template parameter T is a number
    template <typename T>
    using ContainerIfValue = typename std::enable_if<std::is_convertible<T,ValueType>::value, container<ValueType,N>>::type;

    // iterators
    /// typedef for an operation that converts a operator[] of wrapped multi_array into operator[] of container
    typedef std::function<under_type(boost_under_type)> action_type;
    /// iterator (operates as a wrapper over multi_array)
    typedef boost::transform_iterator<action_type,typename boost_t::iterator> iterator;
    /// const_iterator
    typedef boost::transform_iterator<action_type,typename boost_t::iterator> const_iterator;

    // constructors
    /// Constructor from the boost::multi_array (view, ref) type
    container_base(const boost_t &in):storage_(in){};
    /// Copy constructor
    container_base(const container_base<ValueType,N,boost_t> &rhs):storage_(rhs.storage_){};
    /// copy constructor from a different container_base
    template <typename BT2>
        container_base(const container_base<ValueType,N,BT2> &rhs):storage_(rhs.boost_container_()){};
    /// move constructor (C++11)
    container_base(container_base<ValueType,N,boost_t> &&rhs):storage_(std::forward<boost_t>(rhs.storage_)){ }
    /// swap operation
    void swap(container_base& rhs) {std::swap(storage_,rhs.storage_);}

    // assigments
    /// assignment operator from a differenet container base
    template <typename BT2>
        container_base& operator=(const container_base<ValueType,N,BT2> &rhs){storage_ = rhs.boost_container_(); return (*this);};
    /// assignment operator
    container_base& operator=(const container_base<ValueType,N,boost_t> &rhs){storage_ = rhs.storage_; return (*this);};
    /// move assignment (C++11)
    container_base& operator=(container_base<ValueType,N,boost_t> &&rhs){std::swap(storage_,rhs.storage_); return (*this);};
    /// assign from a number
    template <typename T>
        typename std::enable_if<std::is_convertible<T,ValueType>::value,container_base&>::type operator=(T rhs);// { Base(*this) = rhs; return (*this);}
    /// assign from matrix (2d containers only)
    template<size_t N2 = N, typename U = Is2d<N2>>
    container_base<ValueType,N,boost_t>& operator=(MatrixType rhs);

    // access operations
    /// access rank N-1 object
    auto operator[](size_t i) -> under_ref_type { return under_ref_type(storage_[i]); }
    /// const access rank N-1 object
    auto operator[](size_t i) const -> under_ref_type const { return under_ref_type(storage_[i]); }
    /// return value ref from an array of indices
    ValueType& operator()(std::array<size_t, N> indices){return storage_(indices);}
    /// return value const-ref from an array of indices
    const ValueType& operator() (std::array<size_t, N> indices) const {return storage_(indices);}

    // view modifiers
    /// copy into a matrix
    /// TODO consider viewing using Eigen::Map instead of copying
    template<size_t N2 = N, typename U = Is2d<N2>>
    MatrixType as_matrix() const;
    /// copy into a diagonal matrix
    MatrixType as_diagonal_matrix() const;
    /// copy flattened container into a vector
    VectorType as_vector() const;

    // global operations
    /// return a reference to this container as a flattened array
    container_base<ValueType,1,boost::multi_array_ref<ValueType,1>> flatten();
    /// conjugate copy for complex number objects
    template<typename T=ValueType> typename std::enable_if< std::is_same<T, complex_type>::value, container<ValueType,N>>::type conj() const;
    /// conjugate copy non-complex number objects (does nothing)
    template<typename T=ValueType> typename std::enable_if<!std::is_same<T, complex_type>::value, container<ValueType,N>>::type conj() const;
    /// return transposed copy (2d only)
    template<size_t N2 = N, typename U = Is2d<N2>>
    container<ValueType,N> transpose() const { return container<ValueType, N>( this->as_matrix().transpose()); }
    /// return hermite conjugate copy (2d only)
    template<size_t N2 = N, typename U = Is2d<N2>>
    container<ValueType,N> hermite_conj() const { return this->transpose().conj(); }

    /// Sum of all values in the container.
    ValueType sum() const;
    /// return the total size (sum of all dimensions) of the container
    int size() const;
    /// returns the shape of the container
    std::array<size_t,N> shape() const;
    /// returns the squared norm difference between 2 containers
    template <typename V2, typename DT> double diff(const container_base<V2,N,DT>& r, bool norm = true) const;

    /// returns a pointer to the first element in the container
    ValueType* data() { return storage_.origin(); }
    /// returns a const pointer to the first element in the container
    const ValueType* data() const { return storage_.origin(); }
    /** Make the object streamable. */
    template <typename V1, size_t M, typename B>
    friend std::ostream& operator<<(std::ostream& lhs, const container_base<V1, M, B> &in);

    // iterator accessors
    /// Begin const iterator./
    const_iterator begin() const;
    /// Begin iterator./
    iterator begin();
    /// End const iterator.
    const_iterator end() const;
    /// End iterator.
    iterator end();

    // Mathematical operations
    template <typename R>
        BaseRefIfContainer<R> operator+=(const R &rhs);
    template <typename R>
        ContainerIfContainer<R> operator+(const R &rhs) const;
    template <typename R2>
        BaseRefIfValue<R2> operator+=(const R2& rhs);
    template <typename R2>
        ContainerIfValue<R2> operator+(const R2& rhs) const;

    template <typename R>
        BaseRefIfContainer<R> operator-=(const R &rhs);
    template <typename R>
        ContainerIfContainer<R> operator-(const R &rhs) const;
    template <typename R2>
        BaseRefIfValue<R2> operator-=(const R2& rhs);
    template <typename R2>
        ContainerIfValue<R2> operator-(const R2& rhs) const;

    template <typename R>
        BaseRefIfContainer<R> operator*=(const R &rhs);
    template <typename R>
        ContainerIfContainer<R> operator*(const R &rhs) const;
    template <typename R2>
        BaseRefIfValue<R2> operator*=(const R2& rhs);
    template <typename R2>
        ContainerIfValue<R2> operator*(const R2& rhs) const;

    template <typename R>
        BaseRefIfContainer<R> operator/=(const R &rhs);
    template <typename R>
        ContainerIfContainer<R> operator/(const R &rhs) const;
    template <typename R2>
        BaseRefIfValue<R2> operator/=(const R2& rhs);
    template <typename R2>
        ContainerIfValue<R2> operator/(const R2& rhs) const;

    friend container<ValueType,N> operator* (const ValueType & lhs, const container_base<ValueType,N,boost_t> & rhs) {return rhs*lhs;};
    friend container<ValueType,N> operator+ (const ValueType & lhs, const container_base<ValueType,N,boost_t> & rhs) {return rhs+lhs;};
    friend container<ValueType,N> operator- (const ValueType & lhs, const container_base<ValueType,N,boost_t> & rhs) {return rhs*(-1.0)+lhs;};
    friend container<ValueType,N> operator/ (const ValueType & lhs, const container_base<ValueType,N,boost_t> & rhs) {
        container<ValueType,N> out(rhs); out=lhs; return out/rhs;};

    /// An exception provided for incorrect indices
    class ex_wrong_index : public std::exception { virtual const char* what() const throw(){return "Index out of bounds";}};

    /// return underlying boost container
    /// underscore is added to discourage from using the method
    boost_t& boost_container_() const {return storage_; }
protected:
    /// allow other container t access internal storage
    friend struct container<ValueType,N>;
    /// wrapped boost multi_array
    // it is mutable, because Eigen::Map operates with * pointers
    // FIXME - remove mutable
    mutable boost_t storage_;
};

template <typename ValueType, size_t N>
struct container : container_base<ValueType,N,typename boost::multi_array<ValueType, N>> {
    typedef boost::multi_array<ValueType, N> boost_t;
    typedef container_base<ValueType,N,boost_t> Base;
    using Base::storage_;
    typedef typename Base::MatrixType MatrixType;

    /// IsNotContainer defines true_type if the template parameter T is not a container_base type
    template <typename T>
    using IsNotContainer = typename std::enable_if<!(T::N_>=1)>::type;

    /// construct container from a given shape of ints (initialize with zeros)
    container(std::array<int,N> shape):container_base<ValueType,N,typename boost::multi_array<ValueType, N>>(boost::multi_array<ValueType, N>(shape)) {};
    /// construct container from a given shape of size_t (initialize with zeros)
    explicit container(std::array<size_t,N> shape):container_base<ValueType,N,typename boost::multi_array<ValueType, N>>(boost::multi_array<ValueType, N>(shape)) {};
    /// construct container from an initializer list of ints, aka container<double, 2> a({{1,2}})
    container(std::initializer_list<int> shape):container_base<ValueType,N,typename boost::multi_array<ValueType, N>>(boost::multi_array<ValueType, N>(std::array<int, N>(shape))) {};

    /// construct from a different container_base
    // using value here is safe with a move constructor
    template <typename CT>
        container(container_base<ValueType,N,CT> in) : container_base<ValueType,N,typename boost::multi_array<ValueType, N>>(in.storage_) {};
    container(container const&) = default;
    container(container &&) = default;
    container& operator=(container &&) = default;
    container& operator=(container const&) = default;

    /// construct container from given variable amount of numbers, aka container<double, 4> a(1,2,4,2)
    template<typename ...ShapeArgs,
        typename = typename std::enable_if<sizeof...(ShapeArgs) == N
               && (std::is_convertible<std::tuple<ShapeArgs...>, typename tuple_tools::repeater<int,N>::tuple_type>::value // Arguments have to be strictly ints
               || std::is_convertible<std::tuple<ShapeArgs...>, typename tuple_tools::repeater<size_t,N>::tuple_type>::value) // or size_t
        ,int>::type>
        container(ShapeArgs...in):container_base<ValueType,N,typename boost::multi_array<ValueType, N>>(boost::multi_array<ValueType, N>(std::array<int,N>({{static_cast<int>(in)...}}))) {
            static_assert(sizeof...(in) == N,"arg mismatch");
        };
    /// construct 2d container from matrix
    template<size_t N2 = N, typename U = typename std::enable_if<N2==2, bool>::type>
        container<ValueType,N> (MatrixType rhs);

    // inherit math from base
    using Base::operator+=;
    using Base::operator-=;
    using Base::operator*=;
    using Base::operator/=;
    using Base::operator=;
    /// assign from a number
    /*template <typename T> typename std::enable_if<std::is_convertible<T,ValueType>::value,container&>::type operator=(T rhs) {
        typename std::add_lvalue_reference<Base>::type(*this) = rhs; return (*this);}
    */
};

/// type traits for container/container_base of rank>1
template <typename ValueType, size_t N, typename BoostContainerType>
struct container_traits<container_base<ValueType,N,BoostContainerType>,false>
{
    /// typedef boost multi_array type
    typedef BoostContainerType boost_t;
    /// typedef for a result of const operator[] operation (i.e. can be an array or a number)
    typedef typename boost_t::reference boost_under_type;
    /// typedef for a result of operator[] operation (i.e. can be an array or a number)
    typedef container_base<ValueType,N-1,boost_under_type> type;
    /// typedef for a resukt of operator[] of underlying boost::multi_array type
    typedef container_base<ValueType,N-1,boost_under_type> ref_type;
};

/// type traits for container/container_base of rank==1
template <typename ValueType, typename BoostContainerType>
struct container_traits<container_base<ValueType,1,BoostContainerType>,false>
{
    typedef BoostContainerType boost_t;
    typedef typename boost_t::reference boost_under_type;
    typedef ValueType type;
    typedef ValueType& ref_type;
};

/// type traits for container views
template <typename ValueType, size_t N, typename BoostViewType>
struct container_traits<container_base<ValueType,N,BoostViewType>,true>
{
    typedef BoostViewType boost_t;
    typedef typename boost_t::reference boost_under_type;
    typedef ValueType type;
    typedef ValueType& ref_type;
};

}; // end of namespace gftools

#include "container.hxx"


#include <test_common.h>
#include <igl/per_face_normals.h>
#include <Eigen/Geometry>

class per_face_normals : public ::testing::TestWithParam<std::string> {};

TEST_P(per_face_normals, dot)
{
  Eigen::MatrixXd V,N;
  Eigen::MatrixXi F;
  // Load example mesh: GetParam() will be name of mesh file
  test_common::load_mesh(GetParam(), V, F);
  igl::per_face_normals(V,F,N);
  ASSERT_EQ(F.rows(),N.rows());
  for(int f = 0;f<N.rows();f++)
  {
    for(int c = 0;c<3;c++)
    {
      // Every half-edge dot the normal should be 0
      ASSERT_LT(
        std::abs((V.row(F(f,c))-V.row(F(f,(c+1)%3))).dot(N.row(f))),
        1e-12);
    }
  }
  // ASSERT_EQ(a,b);
  // ASSERT_TRUE(a==b);
  // ASSERT_NEAR(a,b,1e-15)
  // ASSERT_LT(a,1e-12);
}

INSTANTIATE_TEST_CASE_P
(
  all_meshes,
  per_face_normals,
  ::testing::ValuesIn(test_common::all_meshes()),
  test_common::string_test_name
);


#include "ship.hpp"

#include <boost/units/io.hpp>

std::ostream& operator<<(std::ostream& os, const Vector2d<Units::Force>& value)
{
    os << "(" << value(0) << ", " << value(1) << ")";
    return os;
}

#include <catch2/catch.hpp>

TEST_CASE("Test Ship with only one thruster", "[engine]")
{
    using namespace boost::units::si;

    SECTION("Basic ship with a thruster pointing straight up")
    {
        Ship ship({{Thruster{1 * newtons}, {{0 * meters, -1 * meters}}, pi / 4 * radians}}, 1000 * kilograms);
        CHECK(ship.numbeOfThrusters() == 1);
        ship.setThrust(0, 0.);
        CHECK(ship.thrust() == Vector2d<Units::Force>{{0 * newtons, 0 * newtons}});
        ship.setThrust(0, 1.);
        CHECK(ship.thrust() == Vector2d<Units::Force>{{0 * newtons, 1 * newtons}});
    }
}


#include <boost/math/distributions/exponential.hpp>


/**
 * @file tests/main_tests/emst_test.cpp
 * @author Manish Kumar
 *
 * Test RUN_BINDING() of emst_main.cpp.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#define BINDING_TYPE BINDING_TYPE_TEST

#include <mlpack/core.hpp>
#include <mlpack/methods/emst/emst_main.cpp>
#include <mlpack/core/util/mlpack_main.hpp>
#include "main_test_fixture.hpp"

#include "../catch.hpp"

#include <boost/math/special_functions/round.hpp>

using namespace mlpack;

BINDING_TEST_FIXTURE(EMSTTestFixture);

/**
 * Make sure that Output has 3 Dimensions and
 * check the number of output edges.
 */
TEST_CASE_METHOD(EMSTTestFixture, "EMSTOutputDimensionTest",
                 "[EMSTMainTest][BindingTests]")
{
  arma::mat x;
  if (!data::Load("test_data_3_1000.csv", x))
    FAIL("Cannot load test dataset test_data_3_1000.csv!");

  // Input random data points.
  SetInputParam("input", std::move(x));
  SetInputParam("leaf_size", (int) 2);

  RUN_BINDING();

  // Now check that the output has 3 dimensions.
  REQUIRE(params.Get<arma::mat>("output").n_rows == 3);
  // Check number of output points.
  REQUIRE(params.Get<arma::mat>("output").n_cols == 999);
}

/**
 * Check Naive algorithm Output has 3 Dimensions and
 * check the number of output edges.
 */
TEST_CASE_METHOD(EMSTTestFixture, "EMSTNaiveOutputDimensionTest",
                 "[EMSTMainTest][BindingTests]")
{
  arma::mat x;
  if (!data::Load("test_data_3_1000.csv", x))
    FAIL("Cannot load test dataset test_data_3_1000.csv!");

  // Input random data points.
  SetInputParam("input", std::move(x));
  SetInputParam("naive", true);

  RUN_BINDING();

  // Now check that the output has 3 dimensions.
  REQUIRE(params.Get<arma::mat>("output").n_rows == 3);
  // Check number of output points.
  REQUIRE(params.Get<arma::mat>("output").n_cols == 999);
}

/**
 * Ensure that we can't specify an invalid leaf size.
 */
TEST_CASE_METHOD(EMSTTestFixture, "EMSTInvalidLeafSizeTest",
                 "[EMSTMainTest][BindingTests]")
{
  arma::mat x;
  if (!data::Load("test_data_3_1000.csv", x))
    FAIL("Cannot load test dataset test_data_3_1000.csv!");

  // Input random data points.
  SetInputParam("input", std::move(x));
  SetInputParam("leaf_size", (int) -1); // Invalid leaf size.

  Log::Fatal.ignoreInput = true;
  REQUIRE_THROWS_AS(RUN_BINDING(), std::runtime_error);
  Log::Fatal.ignoreInput = false;
}

/**
 * Check that all elements of first two output rows are close to integers.
 */
TEST_CASE_METHOD(EMSTTestFixture, "EMSTFirstTwoOutputRowsIntegerTest",
                 "[EMSTMainTest][BindingTests]")
{
  arma::mat x;
  if (!data::Load("test_data_3_1000.csv", x))
    FAIL("Cannot load test dataset test_data_3_1000.csv!");

  // Input random data points.
  SetInputParam("input", std::move(x));
  SetInputParam("leaf_size", (int) 2);

  for (size_t i = 0; i < params.Get<arma::mat>("output").n_cols; ++i)
  {
    REQUIRE(params.Get<arma::mat>("output")(0, i) ==
        Approx(boost::math::iround(params.Get<arma::mat>("output")(0, i))).
        epsilon(1e-7));
    REQUIRE(params.Get<arma::mat>("output")(1, i) ==
        Approx(boost::math::iround(params.Get<arma::mat>("output")(1, i))).
        epsilon(1e-7));
  }
}


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

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>
#include <algorithm>
#include <vector>
#include "gtest/gtest.h"

#include "theia/alignment/alignment.h"
#include "theia/math/util.h"
#include "theia/util/random.h"
#include "theia/util/util.h"
#include "theia/sfm/pose/dls_pnp.h"
#include "theia/sfm/pose/test_util.h"
#include "theia/sfm/types.h"

namespace theia {
namespace {
using Eigen::AngleAxisd;
using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector2d;
using Eigen::Vector3d;

void TestDlsPnpWithNoise(const std::vector<Vector3d>& world_points,
                         const double projection_noise_std_dev,
                         const Quaterniond& expected_rotation,
                         const Vector3d& expected_translation,
                         const double max_reprojection_error,
                         const double max_rotation_difference,
                         const double max_translation_difference) {
  InitRandomGenerator();

  const int num_points = world_points.size();

  Matrix3x4d expected_transform;
  expected_transform << expected_rotation.toRotationMatrix(),
      expected_translation;

  std::vector<Vector2d> feature_points;
  feature_points.reserve(num_points);
  for (int i = 0; i < num_points; i++) {
    // Reproject 3D points into camera frame.
    feature_points.push_back(
        (expected_transform * world_points[i].homogeneous())
            .eval().hnormalized());
  }

  if (projection_noise_std_dev) {
    // Adds noise to both of the rays.
    for (int i = 0; i < num_points; i++) {
      AddNoiseToProjection(projection_noise_std_dev, &feature_points[i]);
    }
  }

  // Run DLS PnP.
  std::vector<Quaterniond> soln_rotation;
  std::vector<Vector3d> soln_translation;
  DlsPnp(feature_points, world_points, &soln_rotation, &soln_translation);

  // Check solutions and verify at least one is close to the actual solution.
  const int num_solutions = soln_rotation.size();
  EXPECT_GT(num_solutions, 0);
  bool matched_transform = false;
  for (int i = 0; i < num_solutions; i++) {
    // Check that reprojection errors are small.
    Matrix3x4d soln_transform;
    soln_transform <<
        soln_rotation[i].toRotationMatrix(), soln_translation[i];

    for (int j = 0; j < num_points; j++) {
      const Vector2d reprojected_point =
          (soln_transform * world_points[j].homogeneous()).eval().hnormalized();
      const double reprojection_error =
          (feature_points[j] - reprojected_point).squaredNorm();
      ASSERT_LE(reprojection_error, max_reprojection_error);
    }

    // Check that the solution is accurate.
    const double rotation_difference =
        expected_rotation.angularDistance(soln_rotation[i]);
    const bool matched_rotation =
        (rotation_difference < max_rotation_difference);
    const double translation_difference =
        (expected_translation - soln_translation[i]).squaredNorm();
    const bool matched_translation =
        (translation_difference < max_translation_difference);

    if (matched_translation && matched_rotation) {
      matched_transform = true;
    }
  }
  EXPECT_TRUE(matched_transform);
}

void BasicTest() {
  const std::vector<Vector3d> points_3d = { Vector3d(-1.0, 3.0, 3.0),
                                            Vector3d(1.0, -1.0, 2.0),
                                            Vector3d(-1.0, 1.0, 2.0),
                                            Vector3d(2.0, 1.0, 3.0) };
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  const double kNoise = 0.0;
  const double kMaxReprojectionError = 1e-4;
  const double kMaxAllowedRotationDifference = 1e-5;
  const double kMaxAllowedTranslationDifference = 1e-8;

  TestDlsPnpWithNoise(points_3d,
                      kNoise,
                      soln_rotation,
                      soln_translation,
                      kMaxReprojectionError,
                      kMaxAllowedRotationDifference,
                      kMaxAllowedTranslationDifference);
}

TEST(DlsPnp, Basic) {
  BasicTest();
}

TEST(DlsPnp, NoiseTest) {
    const std::vector<Vector3d> points_3d = { Vector3d(-1.0, 3.0, 3.0),
                                              Vector3d(1.0, -1.0, 2.0),
                                              Vector3d(-1.0, 1.0, 2.0),
                                              Vector3d(2.0, 1.0, 3.0),
                                              Vector3d(-1.0, -3.0, 2.0),
                                              Vector3d(1.0, -2.0, 1.0),
                                              Vector3d(-1.0, 4.0, 2.0),
                                              Vector3d(-2.0, 2.0, 3.0)
    };
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  const double kNoise = 1.0 / 512.0;
  const double kMaxReprojectionError = 5e-3;
  const double kMaxAllowedRotationDifference = DegToRad(0.25);
  const double kMaxAllowedTranslationDifference = 1e-2;

  TestDlsPnpWithNoise(points_3d,
                      kNoise,
                      soln_rotation,
                      soln_translation,
                      kMaxReprojectionError,
                      kMaxAllowedRotationDifference,
                      kMaxAllowedTranslationDifference);
}

TEST(DlsPnp, ManyPoints) {
// Sets some test rotations and translations.
  static const Vector3d kAxes[] = {
      Vector3d(0.0, 0.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 0.0).normalized(),
      Vector3d(1.0, 0.0, 0.0).normalized(),
      Vector3d(1.0, 0.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 1.0).normalized(),
      Vector3d(1.0, 1.0, 1.0).normalized(),
      Vector3d(0.0, 1.0, 1.0).normalized(),
      Vector3d(1.0, 1.0, 1.0).normalized()
  };

  static const double kRotationAngles[THEIA_ARRAYSIZE(kAxes)] = {
      DegToRad(7.0),
      DegToRad(12.0),
      DegToRad(15.0),
      DegToRad(20.0),
      DegToRad(11.0),
      DegToRad(0.0),  // Tests no rotation.
      DegToRad(5.0),
      DegToRad(0.0)  // Tests no rotation and no translation.
  };

  static const Vector3d kTranslations[THEIA_ARRAYSIZE(kAxes)] = {
      Vector3d(1.0, 1.0, 1.0),
      Vector3d(3.0, 2.0, 13.0),
      Vector3d(4.0, 5.0, 11.0),
      Vector3d(1.0, 2.0, 15.0),
      Vector3d(3.0, 1.5, 18.0),
      Vector3d(1.0, 7.0, 11.0),
      Vector3d(0.0, 0.0, 0.0),  // Tests no translation.
      Vector3d(0.0, 0.0, 0.0)  // Tests no translation and no rotation.
  };

  static const int num_points[3] = { 100, 500, 1000 };
  const double kNoise = 1.0 / 512.0;
  const double kMaxReprojectionError = 1e-2;
  const double kMaxAllowedRotationDifference = DegToRad(0.3);
  const double kMaxAllowedTranslationDifference = 5e-3;

  for (int i = 0; i < THEIA_ARRAYSIZE(kAxes); i++) {
    const Quaterniond soln_rotation(AngleAxisd(kRotationAngles[i], kAxes[i]));
    for (int j = 0; j < THEIA_ARRAYSIZE(num_points); j++) {
      std::vector<Vector3d> points_3d;
      points_3d.reserve(num_points[j]);
      for (int k = 0; k < num_points[j]; k++) {
        points_3d.push_back(Vector3d(RandDouble(-5.0, 5.0),
                                     RandDouble(-5.0, 5.0),
                                     RandDouble(2.0, 10.0)));
      }

      TestDlsPnpWithNoise(points_3d,
                          kNoise,
                          soln_rotation,
                          kTranslations[i],
                          kMaxReprojectionError,
                          kMaxAllowedRotationDifference,
                          kMaxAllowedTranslationDifference);
    }
  }
}

TEST(DlsPnp, NoRotation) {
    const std::vector<Vector3d> points_3d = { Vector3d(-1.0, 3.0, 3.0),
                                              Vector3d(1.0, -1.0, 2.0),
                                              Vector3d(-1.0, 1.0, 2.0),
                                              Vector3d(2.0, 1.0, 3.0),
                                              Vector3d(-1.0, -3.0, 2.0),
                                              Vector3d(1.0, -2.0, 1.0),
                                              Vector3d(-1.0, 4.0, 2.0),
                                              Vector3d(-2.0, 2.0, 3.0)
    };
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(0.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  const double kNoise = 1.0 / 512.0;
  const double kMaxReprojectionError = 5e-3;
  const double kMaxAllowedRotationDifference = DegToRad(0.25);
  const double kMaxAllowedTranslationDifference = 5e-4;

  TestDlsPnpWithNoise(points_3d,
                      kNoise,
                      soln_rotation,
                      soln_translation,
                      kMaxReprojectionError,
                      kMaxAllowedRotationDifference,
                      kMaxAllowedTranslationDifference);
}

TEST(DlsPnp, NoTranslation) {
      const std::vector<Vector3d> points_3d = { Vector3d(-1.0, 3.0, 3.0),
                                              Vector3d(1.0, -1.0, 2.0),
                                              Vector3d(-1.0, 1.0, 2.0),
                                              Vector3d(2.0, 1.0, 3.0),
                                              Vector3d(-1.0, -3.0, 2.0),
                                              Vector3d(1.0, -2.0, 1.0),
                                              Vector3d(-1.0, 4.0, 2.0),
                                              Vector3d(-2.0, 2.0, 3.0)
    };
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(0.0, 0.0, 0.0);
  const double kNoise = 1.0 / 512.0;
  const double kMaxReprojectionError = 1e-2;
  const double kMaxAllowedRotationDifference = DegToRad(0.2);
  const double kMaxAllowedTranslationDifference = 5e-3;

  TestDlsPnpWithNoise(points_3d,
                      kNoise,
                      soln_rotation,
                      soln_translation,
                      kMaxReprojectionError,
                      kMaxAllowedRotationDifference,
                      kMaxAllowedTranslationDifference);
}

TEST(DlsPnp, OrthogonalRotation) {
    const std::vector<Vector3d> points_3d = { Vector3d(-1.0, 3.0, 3.0),
                                              Vector3d(1.0, -1.0, 2.0),
                                              Vector3d(-1.0, 1.0, 2.0),
                                              Vector3d(2.0, 1.0, 3.0),
                                              Vector3d(-1.0, -3.0, 2.0),
                                              Vector3d(1.0, -2.0, 1.0),
                                              Vector3d(-1.0, 4.0, 2.0),
                                              Vector3d(-2.0, 2.0, 3.0)
    };
  const Quaterniond soln_rotation = Quaterniond(
      AngleAxisd(DegToRad(90.0), Vector3d(0.0, 0.0, 1.0)));
  const Vector3d soln_translation(1.0, 1.0, 1.0);
  const double kNoise = 1.0 / 512.0;
  const double kMaxReprojectionError = 5e-3;
  const double kMaxAllowedRotationDifference = DegToRad(0.25);
  const double kMaxAllowedTranslationDifference = 5e-3;

  TestDlsPnpWithNoise(points_3d,
                      kNoise,
                      soln_rotation,
                      soln_translation,
                      kMaxReprojectionError,
                      kMaxAllowedRotationDifference,
                      kMaxAllowedTranslationDifference);
}

}  // namespace
}  // namespace theia


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2012.
// 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 <mpllibs/metamonad/list.hpp>

#include <mpllibs/metamonad/mzero.hpp>
#include <mpllibs/metamonad/mplus.hpp>

#include <boost/test/unit_test.hpp>

#include "common.hpp"

#include <boost/mpl/equal.hpp>
#include <boost/mpl/assert.hpp>

BOOST_AUTO_TEST_CASE(test_list_monad_plus)
{
  using mpllibs::metamonad::list_tag;
  using mpllibs::metamonad::mplus;
  using mpllibs::metamonad::mzero;

  using boost::mpl::equal;
  using boost::mpl::list;

  typedef mzero<list_tag>::type zero;

  // test mzero
  BOOST_MPL_ASSERT((equal<zero, list<> >));

  // test mzero + mzero
  BOOST_MPL_ASSERT((equal<zero, mplus<list_tag, zero, zero>::type>));

  // test mzero + x
  BOOST_MPL_ASSERT((
    equal<list<int13>, mplus<list_tag, zero, list<int13> >::type>
  ));

  // test x + mzero
  BOOST_MPL_ASSERT((
    equal<list<int13>, mplus<list_tag, list<int13>, zero>::type>
  ));

  // test x + y
  BOOST_MPL_ASSERT((
    equal<list<int11, int13>, mplus<list_tag, list<int11>, list<int13> >::type>
  ));
}


#include "ValueWithError.hpp"
#include "precompiled.hpp"
#include "common.hpp"

#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wglobal-constructors"
#endif // __clang__

#if defined __GNUC__ \
            && ( __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 7) ) \
            && !defined __clang__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif // __GNUC__

#include <boost/test/unit_test.hpp>

using namespace error_propagation;

namespace {

  typedef ValueWithError<double,CompareWithinErrorIntervalsPolicy> VD;
  typedef ValueWithError<float,CompareWithinErrorIntervalsPolicy> PVF;

  struct CompareWithinErrorFixture{ };

} // anonymous namespace

BOOST_FIXTURE_TEST_SUITE(Test_CompareWithinErrorIntervalsPolicy,CompareWithinErrorFixture)

BOOST_AUTO_TEST_CASE(operator_equal)
{
  // no overlap
  BOOST_CHECK(!CompareWithinErrorIntervalsPolicy::Equal(VD(1.0,0.4),VD(2.0,0.4)));
  // point overlap
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::Equal(VD(1.0,1.0), VD(2.0,0.4)));
  // range overlap
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::Equal(VD(1.0,1.3), VD(2.0,0.4)));

  // test overloads
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::Equal(VD(1.0), 1.0));
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::Equal(VD(1.0,0.4), 1.0));
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::Equal(1.0, VD(1.0,0.4)));
}
// not equal defined in terms of equal therefore skipping for now

BOOST_AUTO_TEST_CASE(operator_greaterOrEqualThan)
{
  // no overlap
  BOOST_CHECK(!CompareWithinErrorIntervalsPolicy::GreaterOrEqual(VD(1.0,0.4), VD(2.0,0.4)));
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::GreaterOrEqual(VD(2.0,0.4), VD(1.0,0.4)));
  // point overlap
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::GreaterOrEqual(VD(1.0,1.0), VD(2.0,0.4)));
  // range overlap
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::GreaterOrEqual(VD(1.0,1.3), VD(2.0,0.4)));

  // test overloads
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::GreaterOrEqual(VD(1.0), 1.0));
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::GreaterOrEqual(VD(2.0), 1.0));
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::GreaterOrEqual(VD(1.0,0.4), 1.0));
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::GreaterOrEqual(VD(2.0,0.4), 1.0));
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::GreaterOrEqual(1.0, VD(1.0,0.4)));
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::GreaterOrEqual(2.0, VD(1.0,0.4)));
}

BOOST_AUTO_TEST_CASE(operator_greaterThan)
{
  // no overlap
  BOOST_CHECK(!CompareWithinErrorIntervalsPolicy::GreaterThan(VD(1.0,0.4), VD(2.0,0.4)));
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::GreaterThan(VD(2.0,0.4), VD(1.0,0.4)));
  // point overlap
  BOOST_CHECK(!CompareWithinErrorIntervalsPolicy::GreaterThan(VD(1.0,1.0), VD(2.0,0.4)));
  // range overlap
  BOOST_CHECK(!CompareWithinErrorIntervalsPolicy::GreaterThan(VD(1.0,1.3), VD(2.0,0.4)));

  // test overloads
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::GreaterThan(VD(2.0), 1.0));
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::GreaterThan(VD(2.0,0.4), 1.0));
  BOOST_CHECK(CompareWithinErrorIntervalsPolicy::GreaterThan(2.0, VD(1.0,0.4)));
}

BOOST_AUTO_TEST_SUITE_END() // Test_CompareWithinErrorIntervalsPolicy

#ifdef __clang__
#pragma clang diagnostic pop
#endif // __clang__

#if defined __GNUC__ \
            && ( __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 7) ) \
            && !defined __clang__
#pragma GCC diagnostic pop
#endif // __GNUC__