/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110207    B. Romgens        File created.
 *      110215    K. Kumar          Minor modifications to layout, comments
 *                                  and variable-naming.
 *      110411    K. Kumar          Added unit test for
 *                                  convertCartesianToSpherical() function.
 *      110701    K. Kumar          Updated failing tests with relative errors.
 *      110708    K. Kumar          Added unit tests for computeSampleMean()
 *                                  and computeSampleVariance() functions.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      111111    K. Kumar          Strange error with convertCylindricalToCartesian function;
 *                                  achieved precision of results is less than machine precision,
 *                                  fixed by using slightly larger precision tolerance.
 *      120127    D. Dirkx          Moved unit to separate file from basic mathematics test; moved
 *                                  tests to separate functions; moved to Tudat Core.
 *      120127    K. Kumar          Transferred unit tests over to Boost unit test framework.
 *      120128    K. Kumar          Changed BOOST_CHECK to BOOST_CHECK_CLOSE_FRACTION and
 *                                  BOOST_CHECK_SMALL for unit test comparisons.
 *      120118    D. Gondelach      Added unit tests for convertCylindricalToCartesian.
 *                                  Removed unit test for old convertCylindricalToCartesian
 *                                  function.
 *      120214    K. Kumar          Branched from old Tudat trunk for new coordinate conversions.
 *      120512    K. Kumar          Boostified unit test.
 *      120514    P. Musegaas       Fixed small error.
 *      120926    E. Dekens         Added spherical gradient to Cartesian conversion.
 *      120928    M. Ganeff         Made const-correct and.
 *      131023    T. Roegiers       Added unit tests for convertSphericalToCartesianState and for
 *                                  convertCartesianToSphericalState.
 *      140114    E. Brandon        Minor changes during code check.
 *
 *    References
 *      Mathworks. gravitysphericalharmonic, implement spherical harmonic representation of
 *        planetary gravity. Help documentation of MATLAB R2012a, 2012.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>
#include <limits>

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

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Mathematics/BasicMathematics/coordinateConversions.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_coordinate_conversions )

//! Test if spherical-to-Cartesian conversion is working correctly.
BOOST_AUTO_TEST_CASE( testSphericalToCartesianConversion )
{
    using std::sqrt;
    using mathematical_constants::PI;
    using coordinate_conversions::convertSphericalToCartesian;

    // Test 1: test conversion of: ( 0.0, 0.0, 0.0 ).
    {
        Eigen::Vector3d sphericalCoordinates( 0.0, 0.0, 0.0 );

        // Convert spherical coordinates to Cartesian coordinates.
        Eigen::Vector3d cartesianCoordinates = convertSphericalToCartesian( sphericalCoordinates );

        // Expected cartesian coordinates.
        Eigen::Vector3d expectedCartesianCoordinates( 0.0, 0.0, 0.0 );

        // Check if converted Cartesian coordinates are correct.
        TUDAT_CHECK_MATRIX_BASE( cartesianCoordinates, expectedCartesianCoordinates )
                BOOST_CHECK_SMALL( cartesianCoordinates.coeff( row, col ),
                                   std::numeric_limits< double >::min( ) );
    }

    // Test 2: test conversion of: ( 1.0, pi/6, pi/6 ), from Stewart (2003), Exercise 12.7.15.
    {
        Eigen::Vector3d sphericalCoordinates( 1.0, PI / 6.0, PI / 6.0 );

        // Convert spherical coordinates to Cartesian coordinates.
        Eigen::Vector3d cartesianCoordinates = convertSphericalToCartesian( sphericalCoordinates );

        // Expected cartesian coordinates.
        Eigen::Vector3d expectedCartesianCoordinates(
                    sqrt( 3.0 ) / 4.0, 1.0 / 4.0, sqrt( 3.0 ) / 2.0 );

        // Check if converted Cartesian coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    cartesianCoordinates, expectedCartesianCoordinates, 1.0e-15 );
    }

    // Test 3: test conversion of: ( 2.0, pi/4, pi/3 ), from Stewart (2003), Exercise 12.7.17.
    {
        Eigen::Vector3d sphericalCoordinates( 2.0, PI / 4.0, PI / 3.0 );

        // Convert spherical coordinates to Cartesian coordinates.
        Eigen::Vector3d cartesianCoordinates = convertSphericalToCartesian( sphericalCoordinates );

        // Expected cartesian coordinates.
        Eigen::Vector3d expectedCartesianCoordinates(
                    0.5 * sqrt( 2.0 ), 0.5 * sqrt( 6.0 ), sqrt( 2.0 ) );

        // Check if converted Cartesian coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( cartesianCoordinates, expectedCartesianCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 4: test conversion of: ( 2.0, pi/3, pi/4 , from Stewart (2003), Section 12.7,
    //         Example 4.
    {
        Eigen::Vector3d sphericalCoordinates( 2.0, PI / 3.0, PI / 4.0 );

        // Convert spherical coordinates to Cartesian coordinates.
        Eigen::Vector3d cartesianCoordinates = convertSphericalToCartesian( sphericalCoordinates );

        // Expected cartesian coordinates.
        Eigen::Vector3d expectedCartesianCoordinates( sqrt( 1.5 ), sqrt( 1.5 ), 1.0 );

        // Check if converted Cartesian coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( cartesianCoordinates, expectedCartesianCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }
}

//! Test if Cartesian-to-spherical conversion is working correctly.
BOOST_AUTO_TEST_CASE( testCartesianToSphericalConversion )
{
    using std::acos;
    using std::atan2;
    using std::pow;
    using std::sqrt;
    using coordinate_conversions::convertCartesianToSpherical;
    using mathematical_constants::PI;

    // Test 1: Test conversion of: ( 0.0, 0.0, 0.0 ).
    {
        Eigen::Vector3d cartesianCoordinates = Eigen::Vector3d::Zero( );

        // Expected vector in spherical coordinates.
        Eigen::Vector3d expectedSphericalCoordinates = Eigen::Vector3d::Zero( );

        // Result vector in spherical coordinates.
        Eigen::Vector3d sphericalCoordinates = convertCartesianToSpherical( cartesianCoordinates );

        // Check if converted spherical coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( sphericalCoordinates, expectedSphericalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: Test conversion of: ( 1.0, sqrt( 3 ), 2 * sqrt(3) ), from Stewart (2003),
    // Exercise 12.7.19.
    {
        Eigen::Vector3d cartesianCoordinates( 1.0, sqrt( 3. ), 2.0 * sqrt( 3. ) );

        // Expected vector in spherical coordinates.
        Eigen::Vector3d expectedSphericalCoordinates( 4.0, PI / 6.0, PI / 3.0 );

        // Result vector in spherical coordinates.
        Eigen::Vector3d sphericalCoordinates = convertCartesianToSpherical( cartesianCoordinates );

        // Check if converted spherical coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( sphericalCoordinates, expectedSphericalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 3: Test conversion of: ( 0.0, -1.0, -1.0 ), from Stewart (2003), Exercise 12.7.21.
    {
        Eigen::Vector3d cartesianCoordinates( 0.0, -1.0, -1.0 );

        // Expected vector in spherical coordinates.
        Eigen::Vector3d expectedSphericalCoordinates( sqrt( 2.0 ), 3.0 * PI / 4.0, -PI / 2.0 );

        // Result vector in spherical coordinates.
        Eigen::Vector3d sphericalCoordinates = Eigen::Vector3d::Zero( );

        // Compute conversions.
        sphericalCoordinates = convertCartesianToSpherical( cartesianCoordinates );

        // Check if converted spherical coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( sphericalCoordinates, expectedSphericalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 4: Test conversion of: ( 0.0, 2 sqrt( 3 ), -2.0 ), from Stewart (2003), Section 12.7,
    // Example 5.
    {
        Eigen::Vector3d cartesianCoordinates( 0.0, 2.0 * sqrt( 3. ), -2.0 );

        // Expected vector in spherical coordinates.
        Eigen::Vector3d expectedSphericalCoordinates( 4.0, 2.0 * PI / 3.0, PI / 2.0 );

        // Result vector in spherical coordinates.
        Eigen::Vector3d sphericalCoordinates = convertCartesianToSpherical( cartesianCoordinates );

        // Check if converted spherical coordinates are correct.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( sphericalCoordinates, expectedSphericalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }
}

// Test conversion from cylindrical (r, theta, z) to Cartesian (x, y, z) coordinates.
BOOST_AUTO_TEST_CASE( testCylindricalToCartesianPositionCoordinateConversion )
{
    using mathematical_constants::PI;
    using coordinate_conversions::xCartesianCoordinateIndex;
    using coordinate_conversions::yCartesianCoordinateIndex;
    using coordinate_conversions::zCartesianCoordinateIndex;

    // Test 1: test conversion of ( 0.0, pi, 1.2 ).
    {
        // Set cylindrical coordinates.
        const Eigen::Vector3d cylindricalCoordinates( 0.0, PI, 1.2 );

        // Set expected Cartesian coordinates.
        const Eigen::Vector3d expectedCartesianCoordinates( 0.0, 0.0, 1.2 );

        // Convert cylindrical to Cartesian coordinates.
        const Eigen::Vector3d computedCartesianCoordinates
                = coordinate_conversions::
                convertCylindricalToCartesian( cylindricalCoordinates );

        // Check if computed Cartesian coordinates match expected values.
        BOOST_CHECK_SMALL( computedCartesianCoordinates( xCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( computedCartesianCoordinates( yCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianCoordinates( zCartesianCoordinateIndex ),
                                    computedCartesianCoordinates( zCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: test conversion of ( 2.3, -pi/2, -3.5 ).
    {
        // Set cylindrical coordinates.
        const Eigen::Vector3d cylindricalCoordinates( 2.3, -PI/2, -3.5 );

        // Set expected Cartesian coordinates.
        const Eigen::Vector3d expectedCartesianCoordinates( 2.3 * std::cos( -PI / 2.0 ),
                                                            2.3 * std::sin( -PI / 2.0 ),
                                                            -3.5 );

        // Convert cylindrical to Cartesian coordinates.
        const Eigen::Vector3d convertedCartesianCoordinates
                = coordinate_conversions::
                convertCylindricalToCartesian( cylindricalCoordinates );

        // Check if converted Cartesian coordinates match expected values.
        BOOST_CHECK_SMALL( convertedCartesianCoordinates ( xCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    expectedCartesianCoordinates.segment( yCartesianCoordinateIndex, 2 ),
                    convertedCartesianCoordinates.segment( yCartesianCoordinateIndex, 2 ),
                    std::numeric_limits< double >::epsilon( ) );
    }
}

// Test conversion from cylindrical (r, theta, z, Vr, Vtheta, Vz) to Cartesian
// (x, y, z, xdot, ydot, zdot) state.
BOOST_AUTO_TEST_CASE( testCylindricalToCartesianPositionAndVelocityCoordinateConversion )
{
    using mathematical_constants::PI;
    using coordinate_conversions::xCartesianCoordinateIndex;
    using coordinate_conversions::yCartesianCoordinateIndex;
    using coordinate_conversions::zCartesianCoordinateIndex;

    // Test 1: test conversion of (2.1, pi/2.0, 1.2, 5.4, 4.5, -3.9).
    {
        // Set Cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const basic_mathematics::Vector6d cylindricalState =
                ( basic_mathematics::Vector6d( ) << 2.1, PI / 2.0, 1.2, 5.4, 4.5, -3.9 ).finished( );

        // Set expected Cartesian state (x, y, z, xdot, ydot, zdot).
        const basic_mathematics::Vector6d expectedCartesianState =
                ( basic_mathematics::Vector6d( )
                  << 2.1 * std::cos( PI / 2.0 ),
                  2.1 * std::sin( PI / 2.0 ),
                  1.2,
                  5.4 * std::cos( PI / 2.0 ) - 4.5 * std::sin( PI / 2.0 ),
                  5.4 * std::sin( PI / 2.0 ) + 4.5 * std::cos( PI / 2.0 ),
                  -3.9 ).finished( );

        // Convert cylindrical to Cartesian state (x, y, z, xdot, ydot, zdot).
        const basic_mathematics::Vector6d convertedCartesianState
                = coordinate_conversions::
                convertCylindricalToCartesianState( cylindricalState );

        // Check if converted Cartesian state match expected state.
        BOOST_CHECK_SMALL( convertedCartesianState ( xCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    expectedCartesianState.segment( yCartesianCoordinateIndex, 5 ),
                    convertedCartesianState.segment( yCartesianCoordinateIndex, 5 ),
                    std::numeric_limits< double >::epsilon( ) );

    }

    // Test 2: test conversion of (0.0, 8.2*pi/3.0, -2.5, -5.8, 0.0, 1.7).
    {
        // Set Cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const basic_mathematics::Vector6d cylindricalState = ( basic_mathematics::Vector6d( )
                                                   << 0.0, 8.2 * PI / 3.0,
                                                   -2.5, -5.8, 0.0, 1.7 ).finished( );

        // Set expected Cartesian state (x, y, z, xdot, ydot, zdot).
        const basic_mathematics::Vector6d expectedCartesianState = ( basic_mathematics::Vector6d( )
                                                         << 0.0,
                                                         0.0,
                                                         -2.5,
                                                         -5.8 * std::cos( 8.2 * PI / 3.0 ),
                                                         -5.8 * std::sin( 8.2 * PI / 3.0 ),
                                                         1.7 ).finished( );

        // Convert cylindrical to Cartesian state (x, y, z, xdot, ydot, zdot).
        const basic_mathematics::Vector6d convertedCartesianState
                = coordinate_conversions::
                convertCylindricalToCartesianState( cylindricalState );

        // Check if converted Cartesian state match expected state.
        BOOST_CHECK_SMALL( convertedCartesianState( xCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCartesianState( yCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    expectedCartesianState.segment( zCartesianCoordinateIndex, 4 ),
                    convertedCartesianState.segment( zCartesianCoordinateIndex, 4 ),
                    std::numeric_limits< double >::epsilon( ) );
    }
}

// Test conversion from Cartesian (x, y, z) to cylindrical (r, theta, z) coordinates.
BOOST_AUTO_TEST_CASE( testCartesianToCylindricalPositionCoordinateConversion )
{
    using mathematical_constants::PI;
    using coordinate_conversions::rCylindricalCoordinateIndex;
    using coordinate_conversions::thetaCylindricalCoordinateIndex;
    using coordinate_conversions::zCylindricalCoordinateIndex;

    // Test 1: test conversion of ( 0.0, 0.0, 1.0 ).
    {
        // Set Cartesian coordinates (x, y, z).
        const Eigen::Vector3d cartesianCoordinates( 0.0, 0.0, 1.0 );

        // Set expected cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d expectedCylindricalCoordinates( 0.0, 0.0, 1.0 );

        // Convert Cartesian to cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d convertedCylindricalCoordinates
                = coordinate_conversions::
                convertCartesianToCylindrical( cartesianCoordinates );

        // Check if converted cylindrical coordinates match expected values.
        BOOST_CHECK_SMALL( convertedCylindricalCoordinates( rCylindricalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCylindricalCoordinates( thetaCylindricalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCylindricalCoordinates( zCylindricalCoordinateIndex ),
                                    convertedCylindricalCoordinates( zCylindricalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: test conversion of ( 0.0, 2.0, 1.0 ).
    {
        // Set Cartesian coordinates (x, y, z).
        const Eigen::Vector3d cartesianCoordinates( 0.0, 2.0, 1.0 );

        // Set expected cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d expectedCylindricalCoordinates( 2.0, PI / 2.0, 1.0 );

        // Convert Cartesian to cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d convertedCylindricalCoordinates
                = coordinate_conversions::
                convertCartesianToCylindrical( cartesianCoordinates );

        // Check if converted cylindrical coordinates match expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedCylindricalCoordinates,
                                           convertedCylindricalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 3: test conversion of ( 0.0, -2.0, -1.0 ).
    {
        // Set Cartesian coordinates (x, y, z).
        const Eigen::Vector3d cartesianCoordinates( 0.0, -2.0, -1.0 );

        // Set expected cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d expectedCylindricalCoordinates( 2.0, 3.0 * PI / 2.0, -1.0 );

        // Convert Cartesian to cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d convertedCylindricalCoordinates
                = coordinate_conversions::
                convertCartesianToCylindrical( cartesianCoordinates );

        // Check if converted cylindrical coordinates match expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedCylindricalCoordinates,
                                           convertedCylindricalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

    // Test 4: test conversion of ( -5.0, -8.0, 5.0 ).
    {
        // Set Cartesian coordinates (x, y, z).
        const Eigen::Vector3d cartesianCoordinates( -5.0, -8.0, 5.0 );

        // Set expected cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d expectedCylindricalCoordinates( std::sqrt( 25.0 + 64.0 ),
                                                              std::atan2( -8.0,-5.0 ) + 2.0 * PI,
                                                              5.0 );

        // Convert Cartesian to cylindrical coordinates (r, theta, z).
        const Eigen::Vector3d convertedCylindricalCoordinates
                = coordinate_conversions::
                convertCartesianToCylindrical( cartesianCoordinates );

        // Check if converted cylindrical coordinates match expected values.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedCylindricalCoordinates,
                                           convertedCylindricalCoordinates,
                                           std::numeric_limits< double >::epsilon( ) );
    }

}

// Test conversion from Cartesian (x, y, z, xdot, ydot, zdot) to cylindrical
// (r, theta, z, Vr, Vtheta, Vz) state.
BOOST_AUTO_TEST_CASE( testCartesianToCylindricalPositionAndVelocityCoordinateConversion )
{
    using mathematical_constants::PI;
    using coordinate_conversions::rCylindricalCoordinateIndex;
    using coordinate_conversions::thetaCylindricalCoordinateIndex;
    using coordinate_conversions::zCylindricalCoordinateIndex;
    using coordinate_conversions::rDotCylindricalCoordinateIndex;
    using coordinate_conversions::vThetaCylindricalCoordinateIndex;
    using coordinate_conversions::zDotCylindricalCoordinateIndex;

    // Test 1: test conversion of ( 0.0, 0.0, 1.0, 5.0, 6.0, -9.0 ).
    {
        // Set Cartesian state (x,y,z,xdot,ydot,zdot).
        const basic_mathematics::Vector6d cartesianState = ( basic_mathematics::Vector6d( )
                                                 << 0.0, 0.0, 1.0, 5.0, 6.0, -9.0 ).finished( );

        // Set expected cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const basic_mathematics::Vector6d expectedCylindricalState =
                ( basic_mathematics::Vector6d( )
                                                           << 0.0, 0.0, 1.0,
                                                           std::sqrt( 25.0 + 36.0 ),
                                                           0.0, -9.0 ).finished( );

        // Convert Cartesian to cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const basic_mathematics::Vector6d convertedCylindricalState
                = coordinate_conversions::
                convertCartesianToCylindricalState( cartesianState );

        // Check that converted cylindrical state matches expected state.
        BOOST_CHECK_SMALL( convertedCylindricalState( rCylindricalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCylindricalState( thetaCylindricalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCylindricalState( zCylindricalCoordinateIndex ),
                                    convertedCylindricalState( zCylindricalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCylindricalState( rDotCylindricalCoordinateIndex ),
                                    convertedCylindricalState( rDotCylindricalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCylindricalState( vThetaCylindricalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCylindricalState( zDotCylindricalCoordinateIndex ),
                                    convertedCylindricalState( zDotCylindricalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: test conversion of ( 2.0, 0.0, -5.0, -4.0, 6.0, -6.0 ).
    {
        // Set Cartesian state (x,y,z,xdot,ydot,zdot).
        const basic_mathematics::Vector6d cartesianState = ( basic_mathematics::Vector6d( )
                                                 << 2.0, 0.0, -5.0, -4.0, 6.0, -6.0 ).finished( );

        // Set expected cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const basic_mathematics::Vector6d expectedCylindricalState = ( basic_mathematics::Vector6d( )
                                                           << 2.0, 0.0, -5.0,
                                                           -4.0, 6.0, -6.0 ).finished( );

        // Convert Cartesian to cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const basic_mathematics::Vector6d convertedCylindricalState
                = coordinate_conversions::
                convertCartesianToCylindricalState( cartesianState );

        // Check that converted cylindrical state matches expected state.
        BOOST_CHECK_CLOSE_FRACTION( expectedCylindricalState( rCylindricalCoordinateIndex ),
                                    convertedCylindricalState( rCylindricalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCylindricalState( thetaCylindricalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                    expectedCylindricalState.segment( zCylindricalCoordinateIndex, 4 ),
                    convertedCylindricalState.segment( zCylindricalCoordinateIndex, 4 ),
                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 3: test conversion of ( -7.0, -4.0, 3.0, 5.0, -3.0, 7.0 ).
    {
        // Set Cartesian state (x,y,z,xdot,ydot,zdot).
        const basic_mathematics::Vector6d cartesianState = ( basic_mathematics::Vector6d( )
                                                 << -7.0, -4.0, 3.0, 5.0, -3.0, 7.0 ).finished( );

        // Set expected cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const basic_mathematics::Vector6d expectedCylindricalState = ( basic_mathematics::Vector6d( )
                                                           << std::sqrt( 49.0 + 16.0 ),
                                                           std::atan2( -4.0, -7.0 ) + 2.0 * PI,
                                                           3.0,
                                                           ( -7.0 * 5.0 + ( -4.0 ) * -3.0 )
                                                           / std::sqrt( 49.0 + 16.0 ),
                                                           ( -7.0 * -3.0 - ( -4.0 ) * 5.0 )
                                                           / sqrt( 49.0 + 16.0 ), 7.0 ).finished( );

        // Convert Cartesian to cylindrical state (r, theta, z, Vr, Vtheta, Vz).
        const basic_mathematics::Vector6d convertedCylindricalState
                = coordinate_conversions::
                convertCartesianToCylindricalState( cartesianState );

        // Check that converted cylindrical state matches expected state.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedCylindricalState, convertedCylindricalState,
                                           std::numeric_limits< double >::epsilon( ) );
    }
}

// Test conversion from spherical to Cartesian gradient.
BOOST_AUTO_TEST_CASE( test_SphericalToCartesianGradientConversion )
{
    // Define an arbitrary spherical gradient.
    const Eigen::Vector3d sphericalGradient( -2.438146967844150, 1.103964186650749e4,
                                             5.932496087870976e1 );

    // Define an arbitrary spherical position vector.
    const Eigen::Vector3d cartesianCoordinates( -7.0e6, 8.5e6, -6.5e6 );

    // Compute Cartesian gradient.
    const Eigen::Vector3d cartesianGradient = coordinate_conversions::
            convertSphericalToCartesianGradient( sphericalGradient, cartesianCoordinates );

    // Define expected Cartesian gradient. These values are obtained from the computations of
    // 'gx', 'gy' and 'gz' in the MATLAB function 'gravitysphericalharmonics'. This function is
    // described by Mathworks [2012].
    const Eigen::Vector3d expectedCartesianGradient( 1.334464111786447, -1.620429182163669,
                                                     1.240151677089496 );

    // Check if the computed Cartesian gradient matches the expected value.
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedCartesianGradient, cartesianGradient, 1e-15 );
}

// Test conversion from Cartesian (x, y, z, xdot, ydot, zdot) to Spherical (radius, azimuth,
// elevation, radial velocity, azimuthal velocity, elevational velocity) state.
BOOST_AUTO_TEST_CASE( testCartesianToSphericalStateConversion )
{
    using mathematical_constants::PI;
    using coordinate_conversions::radiusSphericalCoordinateIndex;
    using coordinate_conversions::azimuthSphericalCoordinateIndex;
    using coordinate_conversions::elevationSphericalCoordinateIndex;
    using coordinate_conversions::radialVelocitySphericalCoordinateIndex;
    using coordinate_conversions::azimuthVelocitySphericalCoordinateIndex;
    using coordinate_conversions::elevationVelocitySphericalCoordinateIndex;
    using coordinate_conversions::convertCartesianToSphericalState;
    using std::sqrt;

    // Test 1: test conversion of ( 0.0, 0.0, 1.0, 5.0, 6.0, -9.0 ).
    {
        // Set Cartesian state (x, y , z, xdot, ydot, zdot).
        const basic_mathematics::Vector6d cartesianState
                = ( basic_mathematics::Vector6d( ) << 0.0, 0.0, 1.0, 5.0, 6.0, -9.0 ).finished( );

        // Set expected spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const basic_mathematics::Vector6d expectedSphericalState
                = ( basic_mathematics::Vector6d( ) << 1.0, 0.0, PI / 2.0, -9.0, 6.0, -5.0 ).finished( );

        // Convert Cartesian to spherical state.
        const basic_mathematics::Vector6d convertedSphericalState
                = convertCartesianToSphericalState( cartesianState );

        // Check that converted spherical state matches expected spherical state.
        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState( radiusSphericalCoordinateIndex ),
                                    convertedSphericalState( radiusSphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedSphericalState( azimuthSphericalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState( elevationSphericalCoordinateIndex ),
                                    convertedSphericalState( elevationSphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState(
                                        radialVelocitySphericalCoordinateIndex ),
                                    convertedSphericalState(
                                        radialVelocitySphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState(
                                        azimuthVelocitySphericalCoordinateIndex ),
                                    convertedSphericalState(
                                        azimuthVelocitySphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState(
                                        elevationVelocitySphericalCoordinateIndex ),
                                    convertedSphericalState(
                                        elevationVelocitySphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: test conversion of ( 2.0, 0.0, 0.0, -4.0, 6.0, -6.0 ).
    {
        // Set Cartesian state (x, y , z, xdot, ydot, zdot).
        const basic_mathematics::Vector6d cartesianState
                = ( basic_mathematics::Vector6d( ) << 2.0, 0.0, 0.0, -4.0, 6.0, -6.0 ).finished( );

        // Set expected spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const basic_mathematics::Vector6d expectedSphericalState
                = ( basic_mathematics::Vector6d( ) << 2.0, 0.0, 0.0, -4.0, 6.0, -6.0 ).finished( );

        // Convert Cartesian to spherical state ( r, theta, phi, Vr, Vtheta, Vphi).
        const basic_mathematics::Vector6d convertedSphericalState
                = convertCartesianToSphericalState( cartesianState );

        // Check that converted spherical state matches expected spherical state.
        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState( radiusSphericalCoordinateIndex ),
                                    convertedSphericalState( radiusSphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedSphericalState( azimuthSphericalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedSphericalState( elevationSphericalCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState(
                                        radialVelocitySphericalCoordinateIndex ),
                                    convertedSphericalState(
                                        radialVelocitySphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState(
                                        azimuthVelocitySphericalCoordinateIndex ),
                                    convertedSphericalState(
                                        azimuthVelocitySphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedSphericalState(
                                        elevationVelocitySphericalCoordinateIndex ),
                                    convertedSphericalState(
                                        elevationVelocitySphericalCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 3: test conversion of ( -1.0, -1.0, sqrt( 2.0 ), -1.0, 1.0, sqrt( 2.0 ) ).
    {
        // Set Cartesian state ( x, y, z, xdot, ydot, zdot).
        const basic_mathematics::Vector6d cartesianState
                = ( basic_mathematics::Vector6d( )
                    << -1.0, -1.0, sqrt( 2.0 ), -1.0, 1.0, sqrt( 2.0 ) ).finished( );

        // Set expected spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const basic_mathematics::Vector6d expectedSphericalState
                = ( basic_mathematics::Vector6d( )
                    << 2.0, -PI * 3.0 / 4.0, PI / 4.0, 1.0, -sqrt( 2.0 ), 1.0 ).finished( );

        // Convert Cartesian to spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const basic_mathematics::Vector6d convertedSphericalState
                = convertCartesianToSphericalState( cartesianState );

        // Check that converted spherical state matches the expected spherical state.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedSphericalState, convertedSphericalState,
                                           std::numeric_limits< double >::epsilon( ) );
    }
}

// Test conversion from spherical (radius, azimuth, elevation, radial velocity, azimuthal velocity,
// elevational velocity) to Cartesian state (x, y, z, xdot, ydot, zdot).
BOOST_AUTO_TEST_CASE( testSphericalToCartesianStateConversion )
{
    using mathematical_constants::PI;
    using coordinate_conversions::xCartesianCoordinateIndex;
    using coordinate_conversions::yCartesianCoordinateIndex;
    using coordinate_conversions::zCartesianCoordinateIndex;
    using coordinate_conversions::xDotCartesianCoordinateIndex;
    using coordinate_conversions::yDotCartesianCoordinateIndex;
    using coordinate_conversions::zDotCartesianCoordinateIndex;
    using coordinate_conversions::convertSphericalToCartesianState;
    using std::sqrt;

    // Test 1: test conversion of ( 1.0, 0.0, pi/2, -9.0, 6.0, -5.0 ).
    {
        // Set Spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const basic_mathematics::Vector6d sphericalState
                = ( basic_mathematics::Vector6d( ) << 1.0, 0.0, PI / 2.0, -9.0, 6.0, -5.0 ).finished( );

        // Set expected Cartesian state (x, y, z, xdot, ydot, zdot).
        const basic_mathematics::Vector6d expectedCartesianState
                = ( basic_mathematics::Vector6d( ) << 0.0, 0.0, 1.0, 5.0, 6.0, -9.0 ).finished( );

        // Convert spherical to Cartesian state.
        const basic_mathematics::Vector6d convertedCartesianState
                = convertSphericalToCartesianState( sphericalState );

        // Check that converted Cartesian state matches expected state.
        BOOST_CHECK_SMALL( convertedCartesianState( xCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCartesianState( yCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( zCartesianCoordinateIndex ),
                                    convertedCartesianState( zCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( xDotCartesianCoordinateIndex ),
                                    convertedCartesianState( xDotCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( yDotCartesianCoordinateIndex ),
                                    convertedCartesianState( yDotCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( zDotCartesianCoordinateIndex ),
                                    convertedCartesianState( zDotCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 2: test conversion of ( 2.0, 0.0, 0.0, -4.0, 6.0, -6.0 ).
    {
        // Set spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const basic_mathematics::Vector6d sphericalState
                = ( basic_mathematics::Vector6d( ) << 2.0, 0.0, 0.0, -4.0, 6.0, -6.0 ).finished( );

        // Set expected Cartesian state ( x, y, z, xdot, ydot, zdot).
        const basic_mathematics::Vector6d expectedCartesianState
                = ( basic_mathematics::Vector6d( ) << 2.0, 0.0, 0.0, -4.0, 6.0, -6.0 ).finished( );

        // Convert spherical to Cartesian state.
        const basic_mathematics::Vector6d convertedCartesianState
                = convertSphericalToCartesianState( sphericalState );

        // Check that converted Cartesian state matches expected state.
        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( xCartesianCoordinateIndex ),
                                    convertedCartesianState( xCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCartesianState( yCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_SMALL( convertedCartesianState( zCartesianCoordinateIndex ),
                           std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( xDotCartesianCoordinateIndex ),
                                    convertedCartesianState( xDotCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( yDotCartesianCoordinateIndex ),
                                    convertedCartesianState( yDotCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );

        BOOST_CHECK_CLOSE_FRACTION( expectedCartesianState( zDotCartesianCoordinateIndex ),
                                    convertedCartesianState( zDotCartesianCoordinateIndex ),
                                    std::numeric_limits< double >::epsilon( ) );
    }

    // Test 3: test conversion of ( 2.0, -3*pi/4, pi/4, 1.0, -sqrt( 2.0 ), 1.0 ).
    {
        // Set spherical state (r, theta, phi, Vr, Vtheta, Vphi).
        const basic_mathematics::Vector6d sphericalState
                = ( basic_mathematics::Vector6d( )
                    << 2.0, -PI * 3.0 / 4.0, PI / 4.0, 1.0, -sqrt( 2.0 ), 1.0 ).finished( );

        // Set expected Cartesian state (x, y, z, xdot, ydot, zdot).
        const basic_mathematics::Vector6d expectedCartesianState
                = ( basic_mathematics::Vector6d( )
                    << -1.0, -1.0, sqrt( 2.0 ), -1.0, 1.0, sqrt( 2.0 ) ).finished( );

        // Convert spherical to Cartesian state.
        const basic_mathematics::Vector6d convertedCartesianState
                = convertSphericalToCartesianState( sphericalState );

        // Check that converted Cartesian state matches the expected state.
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedCartesianState, convertedCartesianState,
                                           1.0e-15 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// -----------------------------------------------------------------------------
// Ouster OS1 ROS input node.
// -----------------------------------------------------------------------------

#include <pcl_conversions/pcl_conversions.h>

#include "ouster/os1.h"
#include "ouster/os1_packet.h"
#include "ouster_ros/os1_ros.h"

// Maths libraries
#include <Eigen/Eigen>
#include <Eigen/Geometry>

namespace ouster_ros {
namespace OS1 {

using namespace ouster::OS1;

// Note that ROS coordinate system is x=forwards, y=left, z=up
Eigen::Matrix3f imuRotateRoll, imuRotatePitch, imuRotateYaw, imuRotateCombined;
int rotate_using_imu = 0;
int decimate_mask = 0;
float yaw_angle = 0.0f;
ros::Time last_log_time(0.0);
constexpr int max_packets_per_revolution = ouster::OS1::max_columns_per_revolution / ouster::OS1::columns_per_buffer;
char rx_packets[max_packets_per_revolution+1]; // 128 data + terminator
int last_rx_packet = 0;

// -----------------------------------------------------------------------------
// Set the yaw of the drone (message received from IMU)
// Input is in degrees 0..360
void set_yaw(float h)
{
    yaw_angle = (h - 180.0f) * (M_PI / 180.0f);
    imuRotateYaw = Eigen::AngleAxis<float>(-yaw_angle, Eigen::Vector3f::UnitZ());
    imuRotateCombined = imuRotateYaw * imuRotatePitch * imuRotateRoll; // In reverse order of application
}

// -----------------------------------------------------------------------------
// Returns true if this lidar packet starts a new packet
bool is_start_lidar_packet(const PacketMsg& pm) {
    return col_h_encoder_count(pm.buf.data()) == 0;
}

// -----------------------------------------------------------------------------
ns timestamp_of_imu_packet(const PacketMsg& pm) {
    return ns(imu_gyro_ts(pm.buf.data()));
}

// -----------------------------------------------------------------------------
ns timestamp_of_lidar_packet(const PacketMsg& pm) {
    return ns(col_timestamp(nth_col(0, pm.buf.data())));
}

// -----------------------------------------------------------------------------
bool read_imu_packet(const client& cli, PacketMsg& m) {
    m.buf.resize(imu_packet_bytes + 1);
    return read_imu_packet(cli, m.buf.data());
}

// -----------------------------------------------------------------------------
bool read_lidar_packet(const client& cli, PacketMsg& m) {
    m.buf.resize(lidar_packet_bytes + 1);
    return read_lidar_packet(cli, m.buf.data());
}

// -----------------------------------------------------------------------------
// Convert a packet from the IMU in OS1 format into a packet in ROS format
sensor_msgs::Imu packet_to_imu_msg(const PacketMsg& p) {
    const double standard_g = 9.80665;
    sensor_msgs::Imu m;
    const uint8_t* buf = p.buf.data();

    m.header.stamp.fromNSec(imu_gyro_ts(buf));
    m.header.frame_id = "os1_imu";

    m.orientation.x = 0;
    m.orientation.y = 0;
    m.orientation.z = 0;
    m.orientation.w = 0;

    m.linear_acceleration.x = imu_la_x(buf) * standard_g;
    m.linear_acceleration.y = imu_la_y(buf) * standard_g;
    m.linear_acceleration.z = imu_la_z(buf) * standard_g;

    m.angular_velocity.x = imu_av_x(buf) * M_PI / 180.0;
    m.angular_velocity.y = imu_av_y(buf) * M_PI / 180.0;
    m.angular_velocity.z = imu_av_z(buf) * M_PI / 180.0;

    for (int i = 0; i < 9; i++) {
        m.orientation_covariance[i] = -1;
        m.angular_velocity_covariance[i] = 0;
        m.linear_acceleration_covariance[i] = 0;
    }
    for (int i = 0; i < 9; i += 4) {
        m.linear_acceleration_covariance[i] = 0.01;
        m.angular_velocity_covariance[i] = 6e-4;
    }

    if (rotate_using_imu)
    {
        auto pt1 = Eigen::Vector3f(m.linear_acceleration.x, m.linear_acceleration.y, m.linear_acceleration.z);
        // Convert the gravity vector into a one-dimensional rotation around the gimbal axis
        // which counteracts the motor rotation
        float roll_angle = atan2(-pt1.y(), -pt1.z());
        // If angle is - then we are rolling left, else rolling right
        imuRotateRoll = Eigen::AngleAxis<float>(roll_angle, Eigen::Vector3f::UnitX());
        auto pt2 = imuRotateRoll * pt1;
        float pitch_angle = atan2(-pt2.x(), -pt2.z());
        imuRotatePitch = Eigen::AngleAxis<float>(-pitch_angle, Eigen::Vector3f::UnitY());

        imuRotateCombined = imuRotateYaw * imuRotatePitch * imuRotateRoll; // In reverse order of application
#if 0
        if ((m.header.stamp - last_log_time).toSec() > 0.2f)
        {
            last_log_time = m.header.stamp;
            int roll_deg = roll_angle * (180.0f / M_PI);
            int yaw_deg = yaw_angle * (180.0f / M_PI);
            int pitch_deg = pitch_angle * (180.0f / M_PI);
            printf("Yaw=%3d Pitch=%4d Roll=%4d (%f,%f,%f)->(%f,%f,%f)\n",
                yaw_deg, pitch_deg, roll_deg,
                (float)pt1.x(), (float)pt1.y(), (float)pt1.z(),
                (float)pt2.x(), (float)pt2.y(), (float)pt2.z());
            fflush(stdout);
        }
#endif
    }
    return m;
}

// -----------------------------------------------------------------------------
sensor_msgs::PointCloud2 cloud_to_cloud_msg(const CloudOS1& cloud, ns timestamp,
                                            const std::string& frame) {
    sensor_msgs::PointCloud2 msg;
    pcl::toROSMsg(cloud, msg);
    msg.header.frame_id = frame;
    msg.header.stamp.fromNSec(timestamp.count());
    return msg;
}

// -----------------------------------------------------------------------------
// Get the (transformed) point from a LIDAR buffer in converted form
static PointOS1 nth_point(int ind, const uint8_t* col_buf) {
    float h_angle_0 = col_h_angle(col_buf);
    auto tte = trig_table[ind];
    const uint8_t* px_buf = nth_px(ind, col_buf);
    float r = px_range(px_buf) / 1000.0;
    float h_angle = tte.beam_azimuth_angles + h_angle_0;

    PointOS1 point;
    point.reflectivity = px_reflectivity(px_buf);
    point.intensity = px_signal_photons(px_buf);
    if (rotate_using_imu)
    {
        auto pt1 = Eigen::Vector3f(
            -r * tte.cos_beam_altitude_angles * cosf(h_angle),
            r * tte.cos_beam_altitude_angles * sinf(h_angle),
            r * tte.sin_beam_altitude_angles);
        auto pt2 = imuRotateCombined * pt1;

        point.x = pt2.x();
        point.y = pt2.y();
        point.z = pt2.z();
    }
    else
    {
        point.x = -r * tte.cos_beam_altitude_angles * cosf(h_angle);
        point.y = r * tte.cos_beam_altitude_angles * sinf(h_angle);
        point.z = r * tte.sin_beam_altitude_angles;
    }
    point.ring = ind;
    return point;
}

// -----------------------------------------------------------------------------
// Process a lidar packet and send the generated points to the ros cloud (up to 1024 of)
void add_packet_to_cloud(ns scan_start_ts, ns scan_duration,
                         const PacketMsg& pm, CloudOS1& cloud) {
    const uint8_t* buf = pm.buf.data();

	// Output information about how many packets were received in this revolution
	int iValid = col_valid(buf);
	int iEnc = col_h_encoder_count(buf);
	int iPkt1 = (iEnc * max_packets_per_revolution) / encoder_ticks_per_rev; // Currently equivalent to dividing by 44
	int iPkt2 = col_measurement_id(buf); // This increments but does not reset to 0 like the docs claim it should
	if (iValid != -1)
	{
		printf("Invalid lidar packet\n");
	}
	if (iPkt1 != iPkt2)
	{
//		printf("Mismatch between measurement id %d and encoder count %d\n", iPkt2, iEnc);
	}
	if (iPkt1 >= max_packets_per_revolution)
	{
		printf("Error with packet id %d\n", iPkt1);
		iPkt1 = 0;
	}
	if (iPkt1 < last_rx_packet)
	{
		printf("Rx: %s\n", &rx_packets[0]);
		memset(&rx_packets[0], '0', max_packets_per_revolution);
	}
	rx_packets[iPkt1]++; // Increment packet count
	last_rx_packet = iPkt1;

    static int idecimatetoggle = 0;
    idecimatetoggle++;
    int idecimate = (idecimatetoggle & 64) ? 64-1 : 0-1;
    for (int icol = 0; icol < columns_per_buffer; icol++) {
        const uint8_t* col_buf = nth_col(icol, buf);
        float ts = (col_timestamp(col_buf) - scan_start_ts.count()) /
                   (float)scan_duration.count();

        for (int ipx = 0; ipx < pixels_per_column; ipx++) {
            if ((++idecimate & ouster_ros::OS1::decimate_mask) == 0) {
                auto p = nth_point(ipx, col_buf); // Get the (transformed) point
                p.t = ts;
                cloud.push_back(p); // Add the transformed point to the ROS cloud
            }
        }
    }
}

// -----------------------------------------------------------------------------
// Main spin loop in non-replay mode
void spin(const client& cli,
          const std::function<void(const PacketMsg& pm)>& lidar_handler, // Callback for LIDAR packets
          const std::function<void(const PacketMsg& pm)>& imu_handler)   // Callback for IMU packets
{
    // Prepare (single buffer) of input packets
    PacketMsg lidar_packet, imu_packet;
    lidar_packet.buf.resize(lidar_packet_bytes + 1);
    imu_packet.buf.resize(imu_packet_bytes + 1);

    // This loop customises ros::spin()
    while (ros::ok()) {
        // Poll the UDP sockets for valid data. This code is in ouster_client rather than ouster_ros
        auto state = poll_client(cli);
        if (state & ERROR) {
            ROS_ERROR("spin: poll_client returned error");
            return;
        }
        // Read and process LIDAR data if valid
        if (state & LIDAR_DATA) {
            if (read_lidar_packet(cli, lidar_packet.buf.data()))
                lidar_handler(lidar_packet);
        }
        // Read and process IMU data
        if (state & IMU_DATA) {
            if (read_imu_packet(cli, imu_packet.buf.data()))
                imu_handler(imu_packet);
        }
        ros::spinOnce();
    }
}

// -----------------------------------------------------------------------------
static ns nearest_scan_dur(ns scan_dur, ns ts) {
    return ns((ts.count() / scan_dur.count()) * scan_dur.count());
};

// -----------------------------------------------------------------------------
// Create a functor (lambda) that is the lidar handler.
// It processes a lidar packet and adds many points to the cloud
std::function<void(const PacketMsg&)> batch_packets(
    ns scan_dur, const std::function<void(ns, const CloudOS1&)>& f) {
    auto cloud = std::make_shared<OS1::CloudOS1>();
    auto scan_ts = ns(-1L); // Flag the scan timestamp as being unknown

    return [=](const PacketMsg& pm) mutable {
        ns packet_ts = OS1::timestamp_of_lidar_packet(pm);
        bool bIsStart = is_start_lidar_packet(pm);
			if (bIsStart)
				scan_ts = packet_ts; // Synchronise revolution with clock
        if (scan_ts.count() == -1L) // If the scan timestamp is unknown
            scan_ts = nearest_scan_dur(scan_dur, packet_ts); // I think this assumes scan duration never changes over course of OS1 lifetime

        OS1::add_packet_to_cloud(scan_ts, scan_dur, pm, *cloud);

        auto batch_dur = packet_ts - scan_ts;
        if (batch_dur >= scan_dur || batch_dur < ns(0)) {
            f(scan_ts, *cloud);

            cloud->clear();
            scan_ts = ns(-1L);
        }
    };
}
}
}


/* boost random/binomial_distribution.hpp header file
 *
 * Copyright Jens Maurer 2002
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: binomial_distribution.hpp,v 1.7 2002/12/22 22:03:10 jmaurer Exp $
 *
 */

#ifndef BOOST_RANDOM_BINOMIAL_DISTRIBUTION_HPP
#define BOOST_RANDOM_BINOMIAL_DISTRIBUTION_HPP

#include <cmath>
#include <cassert>
#include <boost/random/uniform_01.hpp>

namespace boost {

// Knuth
template<class UniformRandomNumberGenerator, class IntType = int,
         class RealType = double,
         class Adaptor = bernoulli_distribution<UniformRandomNumberGenerator, RealType> >
class binomial_distribution
{
public:
  typedef Adaptor adaptor_type;
  typedef UniformRandomNumberGenerator base_type;
  typedef IntType result_type;

  explicit binomial_distribution(base_type & rng, IntType t = 1,
                                 const RealType& p = RealType(0.5))
    : _rng(rng, p), _t(t)
  {
    assert(t >= 0);
    assert(RealType(0) <= 0 && p <= RealType(1));
  }

  // compiler-generated copy ctor and assignment operator are fine

  adaptor_type& adaptor() { return _rng; }
  base_type& base() const { return _rng.base(); }
  IntType t() const { return _t; }
  RealType p() const { return _rng.p(); }
  void reset() { _rng.reset(); }

  result_type operator()()
  {
    // TODO: This is O(_t), but it should be O(log(_t)) for large _t
    result_type n = 0;
    for(IntType i = 0; i < _t; ++i)
      if(_rng())
        ++n;
    return n;
  }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const binomial_distribution& x,
                         const binomial_distribution& y)
  {
    return x._t == y._t && x._rng == y._rng;
  }

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const binomial_distribution& bd)
  {
    os << bd._rng << " " << bd._t;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, binomial_distribution& bd)
  {
    is >> std::ws >> bd._rng >> std::ws >> bd._t;
    return is;
  }
#else
  // Use a member function
  bool operator==(const binomial_distribution& rhs) const
  {
    return _t == rhs._t && _rng == rhs._rng;
  }
#endif

#endif
private:
  adaptor_type _rng;
  IntType _t;
};

} // namespace boost

#endif // BOOST_RANDOM_BINOMIAL_DISTRIBUTION_HPP


#include "crypto_square.h"
#include <boost/algorithm/string/case_conv.hpp>
#include <algorithm>
#include <cctype>
#include <iterator>

using namespace std;

namespace
{

string normalize(string const &text)
{
    string result;
    copy_if(text.begin(), text.end(), back_inserter(result),
        [](const char c) { return !isspace(c) && !ispunct(c); });
    return boost::to_lower_copy(result);
}

}

namespace crypto_square
{

cipher::cipher(string const& text)
    : text_(normalize(text))
{
}

string cipher::normalize_plain_text() const
{
    return text_;
}

size_t cipher::size() const
{
    size_t length = 0;
    while (length*length < text_.size()) {
        ++length;
    }
    return length;
}

vector<string> cipher::plain_text_segments() const
{
    vector<string> segments;
    const size_t segment_size{size()};
    for (size_t i = 0; i < text_.length(); i += segment_size) {
        segments.push_back(text_.substr(i, segment_size));
    }
    return segments;
}

string cipher::cipher_text() const
{
    string s{normalized_cipher_text()};
    if (!s.empty()) {
        s.erase(remove(s.begin(), s.end(), ' '), s.end());
    }
    return s;
}

string cipher::normalized_cipher_text() const
{
    const auto num_rows = size();
    const auto plain = plain_text_segments();
    string result;
    for (size_t i = 0; i < num_rows; ++i) {
        for (string const& s : plain) {
            if (s.length() > i) {
                result += s[i];
            }
            else {
                result += ' ';
            }
        }
        result += ' ';
    }
    if (!result.empty()) {
        result.pop_back();
    }
    return result;
}

}


/*=============================================================================
  Copyright (c) 2010-2016 Bolero MURAKAMI
  https://github.com/bolero-MURAKAMI/Sprig

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

#include <sprig/config/config.hpp>

#ifdef SPRIG_USING_PRAGMA_ONCE
#	pragma once
#endif	// #ifdef SPRIG_USING_PRAGMA_ONCE

#include <boost/rational.hpp>
#include <sprig/cast_converter.hpp>

namespace sprig {
	namespace rational_detail {
		//
		// rational_arrange_digit_impl
		//
		template<typename IntType>
		SPRIG_INLINE IntType rational_arrange_digit_impl(
			IntType const& denom,
			IntType const& n,
			IntType const& current,
			IntType const& current_digit
			)
		{
			return (current % denom != 0)
				? rational_arrange_digit_impl(denom, n, current * n, current_digit + 1)
				: current_digit
				;
		}
		//
		// rational_arrange_factor_impl
		//
		template<typename IntType>
		SPRIG_INLINE IntType rational_arrange_factor_impl(
			IntType const& denom,
			IntType const& n,
			IntType const& current
			)
		{
			return (current % denom != 0)
				? rational_arrange_factor_impl(denom, n, current * n)
				: current
				;
		}
	}	// namespace rational_detail
	//
	// rational_cast
	//
	template<typename To, typename Converter, typename IntType>
	SPRIG_INLINE To rational_cast(boost::rational<IntType> const& from, Converter conv) {
		return conv(from.numerator()) / conv(from.denominator());
	}
	//
	// rational_converter_cast
	//
	template<typename Tag, typename To, typename IntType>
	SPRIG_INLINE To rational_converter_cast(boost::rational<IntType> const& from) {
		return sprig::converter_cast<Tag, To>(from.numerator()) / sprig::converter_cast<Tag, To>(from.denominator());
	}
	//
	// rational_arrange_digit
	//
	template<typename IntType>
	SPRIG_INLINE IntType rational_arrange_digit(
		boost::rational<IntType> const& v,
		IntType const& n = 10
		)
	{
		return rational_detail::rational_arrange_digit_impl<IntType>(v.denominator(), n, 1, 0);
	}
	//
	// rational_arrange_factor
	//
	template<typename IntType>
	SPRIG_INLINE IntType rational_arrange_factor(
		boost::rational<IntType> const& v,
		IntType const& n = 10
		)
	{
		return rational_detail::rational_arrange_factor_impl<IntType>(v.denominator(), n, 1);
	}
}	// namespace sprig

#endif	// #ifndef SPRIG_UTILITY_RATIONAL_HPP


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

#include <amgcl/value_type/complex.hpp>
#include <amgcl/backend/builtin.hpp>

#include "test_solver.hpp"

BOOST_AUTO_TEST_SUITE( test_solvers )

BOOST_AUTO_TEST_CASE(test_builtin_complex_backend)
{
    test_backend< amgcl::backend::builtin< std::complex<double> > >();
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <fstream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/vf2_sub_graph_iso.hpp>
#include <boost/graph/graphviz.hpp>

using namespace std;
using namespace boost;

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

  typedef adjacency_list<vecS, vecS, undirectedS> graph_type;
  graph_type graph1(0);
  graph_type graph2(0);
  ifstream inputFile;
  inputFile.open(argv[1]);
  dynamic_properties dp(ignore_other_properties);
  read_graphviz(inputFile, graph1, dp);
//   read_graphviz(inputFile,graph2, dp);
  graph2 = graph1;
  inputFile.close();

  // Create callback to print mappings
  vf2_print_callback<graph_type, graph_type> callback(graph1, graph2);
  vf2_subgraph_iso(graph1, graph2, callback);

}


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

#include <test/numeric.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/core/operators.hpp>
#include <boost/hana/tuple.hpp>
#include <boost/hana/type.hpp>

#include <test/auto/base.hpp>
#include <test/cnumeric.hpp>
#include <test/injection.hpp>
#include <vector>

// tested instances
#include <test/auto/comparable.hpp>
#include <test/auto/enumerable.hpp>
#include <test/auto/group.hpp>
#include <test/auto/integral_domain.hpp>
#include <test/auto/logical.hpp>
#include <test/auto/monoid.hpp>
#include <test/auto/orderable.hpp>
using namespace boost::hana;

namespace boost { namespace hana { namespace test {
    template <>
    auto instances<Numeric> = make<Tuple>(
          type<Enumerable>
        , type<Monoid>
        , type<Group>
        , type<Ring>
        , type<IntegralDomain>
        , type<Comparable>
        , type<Orderable>
        , type<Logical>
    );

    template <>
    auto objects<Numeric> = make<Tuple>(
          numeric(0)
        , numeric(1)
        , numeric(2)
        , numeric(3)
    );
}}}

namespace boost { namespace hana {
    template <>
    struct operators::of<test::Numeric>
        : operators::of<
            Monoid, Group, Ring, IntegralDomain,
            Comparable, Orderable, Logical
        >
    { };
}}

struct invalid {
    template <typename T>
    operator T const() { throw; }
};

int main() {
    test::check_datatype<test::Numeric>();

    // Comparable
    {
        auto f = test::injection([]{});
        auto x = test::numeric(1);
        auto y = test::numeric(2);

        // equal
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(x, x));
            BOOST_HANA_CONSTEXPR_CHECK(not_(equal(x, y)));
        }

        // not_equal
        {
            BOOST_HANA_CONSTEXPR_CHECK(not_equal(x, y));
            BOOST_HANA_CONSTEXPR_CHECK(not_(not_equal(x, x)));
        }

        // comparing
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                comparing(f)(x, x),
                equal(f(x), f(x))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                comparing(f)(x, y),
                equal(f(x), f(y))
            ));
        }

        // operators
        {
            using namespace boost::hana::operators;
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                x == y,
                equal(x, y)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                x != y,
                not_equal(x, y)
            ));
        }
    }

    // Orderable
    {
        auto ord = test::numeric;

        // test::injection is also monotonic
        auto f = test::injection([]{});

        // less
        {
            BOOST_HANA_CONSTEXPR_CHECK(less(ord(0), ord(1)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(less(ord(0), ord(0))));
            BOOST_HANA_CONSTEXPR_CHECK(not_(less(ord(1), ord(0))));
        }

        // less_equal
        {
            BOOST_HANA_CONSTEXPR_CHECK(less_equal(ord(0), ord(1)));
            BOOST_HANA_CONSTEXPR_CHECK(less_equal(ord(0), ord(0)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(less_equal(ord(1), ord(0))));
        }

        // greater_equal
        {
            BOOST_HANA_CONSTEXPR_CHECK(greater_equal(ord(1), ord(0)));
            BOOST_HANA_CONSTEXPR_CHECK(greater_equal(ord(0), ord(0)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(greater_equal(ord(0), ord(1))));
        }

        // greater
        {
            BOOST_HANA_CONSTEXPR_CHECK(greater(ord(1), ord(0)));
            BOOST_HANA_CONSTEXPR_CHECK(not_(greater(ord(0), ord(0))));
            BOOST_HANA_CONSTEXPR_CHECK(not_(greater(ord(0), ord(1))));
        }

        // max
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                max(ord(0), ord(0)), ord(0)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                max(ord(1), ord(0)), ord(1)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                max(ord(0), ord(1)), ord(1)
            ));
        }

        // min
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                min(ord(0), ord(0)),
                ord(0)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                min(ord(1), ord(0)),
                ord(0)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                min(ord(0), ord(1)),
                ord(0)
            ));
        }

        // ordering
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                ordering(f)(ord(1), ord(0)),
                less(f(ord(1)), f(ord(0)))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                ordering(f)(ord(0), ord(1)),
                less(f(ord(0)), f(ord(1)))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                ordering(f)(ord(0), ord(0)),
                less(f(ord(0)), f(ord(0)))
            ));
        }

        // operators
        {
            using namespace boost::hana::operators;
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                ord(0) < ord(1),
                less(ord(0), ord(1))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                ord(0) <= ord(1),
                less_equal(ord(0), ord(1))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                ord(0) > ord(1),
                greater(ord(0), ord(1))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                ord(0) >= ord(1),
                greater_equal(ord(0), ord(1))
            ));
        }
    }

    // Enumerable
    {
        // succ
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                succ(test::numeric(0)),
                test::numeric(1)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                succ(test::numeric(1)),
                test::numeric(2)
            ));
        }

        // pred
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                pred(test::numeric(1)),
                test::numeric(0)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                pred(test::numeric(2)),
                test::numeric(1)
            ));
        }
    }

    // Monoid
    {
        constexpr int x = 2, y = 3;

        // zero
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                zero<test::Numeric>(), test::numeric(0)
            ));
        }

        // plus
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                plus(test::numeric(x), test::numeric(y)),
                test::numeric(x + y)
            ));
        }

        // operators
        {
            using namespace boost::hana::operators;
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                test::numeric(x) + test::numeric(y),
                plus(test::numeric(x), test::numeric(y))
            ));
        }
    }

    // Group
    {
        constexpr int x = 2, y = 3;

        // minus
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                minus(test::numeric(x), test::numeric(y)),
                test::numeric(x - y)
            ));
        }

        // negate
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                negate(test::numeric(x)),
                test::numeric(-x)
            ));
        }

        // operators
        {
            using namespace boost::hana::operators;
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                test::numeric(x) - test::numeric(y),
                minus(test::numeric(x), test::numeric(y))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                -test::numeric(x),
                negate(test::numeric(x))
            ));
        }
    }

    // Ring
    {
        constexpr int x = 2, y = 3;

        // one
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                one<test::Numeric>(),
                test::numeric(1)
            ));
        }

        // mult
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                mult(test::numeric(x), test::numeric(y)),
                test::numeric(x * y)
            ));
        }

        // power
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                power(test::numeric(x), zero<test::CNumeric<int>>()),
                one<test::Numeric>()
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                power(test::numeric(x), one<test::CNumeric<int>>()),
                test::numeric(x)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                power(test::numeric(x), test::cnumeric<int, 2>),
                mult(test::numeric(x), test::numeric(x))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                power(test::numeric(x), test::cnumeric<int, 3>),
                mult(mult(test::numeric(x), test::numeric(x)), test::numeric(x))
            ));
        }

        // operators
        {
            using namespace boost::hana::operators;
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                test::numeric(x) * test::numeric(y),
                mult(test::numeric(x), test::numeric(y))
            ));
        }
    }

    // IntegralDomain
    {
        constexpr int x = 6, y = 3, z = 4;

        // quot
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                quot(test::numeric(x), test::numeric(y)),
                test::numeric(x / y)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                quot(test::numeric(x), test::numeric(z)),
                 test::numeric(x/ z)
            ));
        }

        // rem
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                rem(test::numeric(x), test::numeric(y)),
                test::numeric(x % y)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                rem(test::numeric(x), test::numeric(z)),
                test::numeric(x % z)
            ));
        }

        // operators
        {
            using namespace boost::hana::operators;
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                test::numeric(x) % test::numeric(y),
                rem(test::numeric(x), test::numeric(y))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                test::numeric(x) / test::numeric(y),
                quot(test::numeric(x), test::numeric(y))
            ));
        }
    }

    // Logical
    {
        auto logical = test::numeric;
        auto comparable = test::numeric;

        // not_
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                not_(logical(true)),
                logical(false)
            ));
        }

        // and_
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(true)),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(false)),
                logical(false)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(true), logical(true)),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(true), logical(false)),
                logical(false)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(false), invalid{}),
                logical(false)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(true), logical(true), logical(true)),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(true), logical(true), logical(false)),
                logical(false)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(true), logical(false), invalid{}),
                logical(false)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                and_(logical(false), invalid{}, invalid{}),
                logical(false)
            ));
        }

        // or_
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(true)),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(false)),
                logical(false)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(false), logical(false)),
                logical(false)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(false), logical(true)),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(true), invalid{}),
                logical(true)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(false), logical(false), logical(false)),
                logical(false)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(false), logical(false), logical(true)),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(false), logical(true), invalid{}),
                logical(true)
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                or_(logical(true), invalid{}, invalid{}),
                logical(true)
            ));
        }

        // if_
        {
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                if_(logical(true), comparable(0), comparable(1)),
                comparable(0)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                if_(logical(false), comparable(0), comparable(1)),
                comparable(1)
            ));
        }

        // eval_if
        {
            auto t = [=](auto) { return comparable(0); };
            auto e = [=](auto) { return comparable(1); };

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                eval_if(logical(true), t, e),
                comparable(0)
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                eval_if(logical(false), t, e),
                comparable(1)
            ));
        }

        // while_
        {
            //! @todo Implement this
        }

        // until
        {
            auto has_length = [](auto n) {
                return [n](auto v) { return v.size() == n; };
            };
            auto f = [](auto v) {
                v.push_back(v.size());
                return v;
            };

            BOOST_HANA_RUNTIME_CHECK(equal(
                until(has_length(0u), std::vector<int>{}, f),
                std::vector<int>{}
            ));

            BOOST_HANA_RUNTIME_CHECK(equal(
                until(has_length(1u), std::vector<int>{}, f),
                std::vector<int>{0}
            ));

            BOOST_HANA_RUNTIME_CHECK(equal(
                until(has_length(2u), std::vector<int>{}, f),
                std::vector<int>{0, 1}
            ));

            BOOST_HANA_RUNTIME_CHECK(equal(
                until(has_length(3u), std::vector<int>{}, f),
                std::vector<int>{0, 1, 2}
            ));

            BOOST_HANA_RUNTIME_CHECK(equal(
                until(has_length(4u), std::vector<int>{}, f),
                std::vector<int>{0, 1, 2, 3}
            ));

            // Make sure it can be called with an lvalue state:
            std::vector<int> v{};
            BOOST_HANA_RUNTIME_CHECK(equal(
                until(has_length(4u), v, f),
                std::vector<int>{0, 1, 2, 3}
            ));
        }

        // operators
        {
            using namespace boost::hana::operators;
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                !logical(true),
                not_(logical(true))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                logical(true) && logical(true),
                and_(logical(true), logical(true))
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                logical(false) || logical(false),
                or_(logical(false), logical(false))
            ));
        }
    }
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

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

#include <boost/compute/command_queue.hpp>
#include <boost/compute/lambda.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/find.hpp>
#include <boost/compute/algorithm/find_if.hpp>
#include <boost/compute/algorithm/find_if_not.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/constant_buffer_iterator.hpp>

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

namespace bc = boost::compute;
namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(find_int)
{
    int data[] = { 9, 15, 1, 4, 9, 9, 4, 15, 12, 1 };
    bc::vector<int> vector(data, data + 10);

    bc::vector<int>::iterator iter =
        bc::find(vector.begin(), vector.end(), 4);
    BOOST_CHECK(iter == vector.begin() + 3);
    BOOST_CHECK_EQUAL(*iter, 4);

    iter = bc::find(vector.begin(), vector.end(), 12);
    BOOST_CHECK(iter == vector.begin() + 8);
    BOOST_CHECK_EQUAL(*iter, 12);

    iter = bc::find(vector.begin(), vector.end(), 1);
    BOOST_CHECK(iter == vector.begin() + 2);
    BOOST_CHECK_EQUAL(*iter, 1);

    iter = bc::find(vector.begin(), vector.end(), 9);
    BOOST_CHECK(iter == vector.begin());
    BOOST_CHECK_EQUAL(*iter, 9);

    iter = bc::find(vector.begin(), vector.end(), 100);
    BOOST_CHECK(iter == vector.end());
}

BOOST_AUTO_TEST_CASE(find_int2)
{
    using bc::int2_;

    int data[] = { 1, 2, 4, 5, 7, 8 };
    bc::vector<int2_> vector(
        reinterpret_cast<int2_ *>(data),
        reinterpret_cast<int2_ *>(data) + 3
    );
    CHECK_RANGE_EQUAL(int2_, 3, vector, (int2_(1, 2), int2_(4, 5), int2_(7, 8)));

    bc::vector<int2_>::iterator iter =
        bc::find(vector.begin(), vector.end(), int2_(4, 5));
    BOOST_CHECK(iter == vector.begin() + 1);
    BOOST_CHECK_EQUAL(*iter, int2_(4, 5));

    iter = bc::find(vector.begin(), vector.end(), int2_(10, 11));
    BOOST_CHECK(iter == vector.end());
}

BOOST_AUTO_TEST_CASE(find_if_not_int)
{
    int data[] = { 2, 4, 6, 8, 1, 3, 5, 7, 9 };
    bc::vector<int> vector(data, data + 9);

    bc::vector<int>::iterator iter =
        bc::find_if_not(vector.begin(), vector.end(), bc::_1 == 2);
    BOOST_CHECK(iter == vector.begin() + 1);
    BOOST_CHECK_EQUAL(*iter, 4);
}

BOOST_AUTO_TEST_CASE(find_point_by_distance)
{
    using boost::compute::float2_;
    using boost::compute::lambda::_1;
    using boost::compute::lambda::distance;

    float2_ points[] = {
        float2_(0, 0), float2_(2, 2), float2_(4, 4), float2_(8, 8)
    };
    compute::vector<float2_> vec(points, points + 4, queue);

    compute::vector<float2_>::iterator iter =
        compute::find_if(vec.begin(), vec.end(), distance(_1, float2_(5, 5)) < 1.5f, queue);
    BOOST_CHECK(iter == vec.begin() + 2);

    float2_ value;
    compute::copy_n(iter, 1, &value, queue);
    BOOST_CHECK_EQUAL(value, float2_(4, 4));
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/test/unit_test.hpp>

#include <boost/format.hpp>

#include <set>
#include <iostream>

using std::ostream;
using boost::format;
using namespace crave;

enum color_enum { RED, GREEN, BLUE };
CRAVE_ENUM(color_enum, (RED)(GREEN)(BLUE));

enum football_enum {
  GK,   // Goalkeeper
  SW,   // Sweeper
  LWB,  // Left-Wing-Back
  LB,   // Left-Back
  LCB,  // Left-Centre-Back
  RCB,  // Right-Centre-Back
  RB,   // Right-Back
  RWB,  // Right-Wing-Back
  DM,   // Defensive Midfielder
  LM,   // Left Wide Midfielder
  CM,   // Centre Midfielder
  RM,   // Right Wide Midfielder
  AM,   // Attacking Midfielder
  LW,   // Left Winger (Striker)
  SS,   // Secondary Striker
  RW,   // Right Winger
  CF    // Centre Striker
};
CRAVE_ENUM(football_enum, (GK)(SW)(LWB)(LB)(LCB)(RCB)(RB)(RWB)(DM)(LM)(CM)(RM)(AM)(LW)(SS)(RW)(CF));

BOOST_FIXTURE_TEST_SUITE(Random_Object_t, Context_Fixture)

class my_rand_obj : public rand_obj {
 public:
  randv<color_enum> color;
  randv<int> x;

  my_rand_obj(rand_obj* parent = 0) : rand_obj(parent), color(this), x(this) { constraint(color() == x()); }
};

BOOST_AUTO_TEST_CASE(t_rand_enum) {
  my_rand_obj obj(0);
  for (int i = 0; i < 20; i++) {
    BOOST_REQUIRE(obj.next());
    std::cout << obj.color << " " << obj.x << std::endl;
    BOOST_REQUIRE(obj.color == RED || obj.color == GREEN || obj.color == BLUE);
    BOOST_REQUIRE(obj.color == obj.x);
  }
}

BOOST_AUTO_TEST_CASE(t_rand_enum_standalone) {
  BOOST_REQUIRE_THROW(new randv<color_enum>(0), std::runtime_error);
}

class tall_rand_enum_obj : public rand_obj {
 public:
  randv<football_enum> player;

  tall_rand_enum_obj(rand_obj* parent = 0) : rand_obj(parent), player(this) {
    constraint(player() == GK && player() != CF);
  }
};

class tall_rand_enum_obj_gt : public rand_obj {
 public:
  randv<football_enum> player;

  tall_rand_enum_obj_gt(rand_obj* parent = 0) : rand_obj(parent), player(this) { constraint(player() > AM); }
};

BOOST_AUTO_TEST_CASE(enum_no_overflow) {
  tall_rand_enum_obj obj(0);
  BOOST_REQUIRE(obj.next());

  BOOST_REQUIRE_EQUAL(obj.player, GK);
}

BOOST_AUTO_TEST_CASE(enum_gt) {
  tall_rand_enum_obj_gt obj(0);

  for (int i = 0; i < 100; ++i) {
    BOOST_REQUIRE(obj.next());
    BOOST_REQUIRE_GT(obj.player, AM);
    BOOST_REQUIRE(obj.player == LW || obj.player == SS || obj.player == RW || obj.player == CF);
  }
}

class item : public rand_obj {
 public:
  item(rand_obj* parent) : rand_obj(parent), a(this), b(this), c(this) { constraint(a() + b() == c()); }

 public:
  randv<int> a;
  randv<int> b;
  randv<int> c;
};

class item1 : public item {
 public:
  item1(rand_obj* parent) : item(parent) {
    constraint(10 <= a() && a() <= 20);
    constraint(a() + b() + c() <= 200);
  }
};

class item2 : public item1 {
 public:
  item2(rand_obj* parent) : item1(parent), d(this) { constraint(a() + b() + c() == 100); }
  randv<int> d;
};

BOOST_AUTO_TEST_CASE(t2) {
  item it(0);
  BOOST_REQUIRE(it.next());
  std::cout << it.a << " " << it.b << " " << it.c << std::endl;
  BOOST_REQUIRE(it.a + it.b == it.c);
}

BOOST_AUTO_TEST_CASE(t3) {
  item1 it(0);
  BOOST_REQUIRE(it.next());
  std::cout << it.a << " " << it.b << " " << it.c << std::endl;
  BOOST_REQUIRE(it.a + it.b == it.c);
  BOOST_REQUIRE(10 <= it.a && it.a <= 20);
  BOOST_REQUIRE(it.a + it.b + it.c <= 200);
}

BOOST_AUTO_TEST_CASE(t4) {
  item2 it(0);
  BOOST_REQUIRE(it.next());
  std::cout << it.a << " " << it.b << " " << it.c << std::endl;
  BOOST_REQUIRE(it.a + it.b == it.c);
  BOOST_REQUIRE(10 <= it.a && it.a <= 20);
  BOOST_REQUIRE(it.a + it.b + it.c == 100);
}

class obj : public rand_obj {
 public:
  obj(rand_obj* parent) : rand_obj(parent), a(this), b(this), c(this), d(this), e(this), f(this) {
    constraint(dist(a(), distribution<int>::simple_range(-20, -10)));
    constraint(dist(b(), distribution<unsigned int>::simple_range(10, 20)));
    constraint(dist(c(), distribution<short>::simple_range(-20, -10)));
    constraint(dist(d(), distribution<unsigned short>::simple_range(10, 20)));
    constraint("e", dist(e(), distribution<char>::simple_range('a', 'z')));
    constraint("f", dist(f(), distribution<unsigned char>::simple_range('A', 'Z')));
  }
  randv<int> a;
  randv<unsigned int> b;
  randv<short> c;
  randv<unsigned short> d;
  randv<char> e;
  randv<unsigned char> f;

  friend ostream& operator<<(ostream& os, const obj& o) {
    os << "(" << o.a << " " << o.b << " " << o.c << " " << o.d << " " << o.e << " " << o.f << ")";
    return os;
  }
};

class obj1 : public obj {
 public:
  obj1(rand_obj* parent) : obj(parent) {
    disable_constraint("e");
    disable_constraint("f");
    constraint(dist(e(), distribution<char>::simple_range('A', 'Z')));
    constraint(dist(f(), distribution<unsigned char>::simple_range('a', 'z')));
  }
};

class obj2 : public rand_obj {
 public:
  obj2(rand_obj* parent) : rand_obj(parent), g(this), h(this), i(this), j(this), k(this), l(this) {
    constraint(dist(g(), distribution<long>::simple_range(-20, -10)));
    constraint(dist(h(), distribution<unsigned long>::simple_range(10, 20)));
    constraint(dist(i(), distribution<long long>::simple_range(-20, -10)));
    constraint(dist(j(), distribution<unsigned long long>::simple_range(10, 20)));
  }
  randv<long> g;
  randv<unsigned long> h;
  randv<long long> i;
  randv<unsigned long long> j;
  randv<bool> k;
  obj1 l;

  friend ostream& operator<<(ostream& os, const obj2& o1) {
    os << o1.l << " " << o1.g << " " << o1.h << " " << o1.i << " " << o1.j << " " << o1.k;
    return os;
  }
};

BOOST_AUTO_TEST_CASE(t5) {
  obj it(0);
  for (int i = 0; i < 20; i++) {
    BOOST_REQUIRE(it.next());
    BOOST_REQUIRE(-20 <= it.a && it.a <= -10);
    BOOST_REQUIRE(10 <= it.b && it.b <= 20);
    BOOST_REQUIRE(-20 <= it.c && it.c <= -10);
    BOOST_REQUIRE(10 <= it.d && it.d <= 20);
    BOOST_REQUIRE('a' <= it.e && it.e <= 'z');
    BOOST_REQUIRE('A' <= it.f && it.f <= 'Z');
  }

  obj1 it1(0);
  for (int i = 0; i < 20; i++) {
    BOOST_REQUIRE(it1.next());
    BOOST_REQUIRE(-20 <= it1.a && it1.a <= -10);
    BOOST_REQUIRE(10 <= it1.b && it1.b <= 20);
    BOOST_REQUIRE(-20 <= it1.c && it1.c <= -10);
    BOOST_REQUIRE(10 <= it1.d && it1.d <= 20);
    BOOST_REQUIRE('a' <= it1.f && it1.f <= 'z');
    BOOST_REQUIRE('A' <= it1.e && it1.e <= 'Z');
  }

  obj2 it2(0);
  for (int i = 0; i < 20; i++) {
    BOOST_REQUIRE(it2.next());
    BOOST_REQUIRE(-20 <= it2.g && it2.g <= -10);
    BOOST_REQUIRE(10 <= it2.h && it2.h <= 20);
    BOOST_REQUIRE(-20 <= it2.i && it2.i <= -10);
    BOOST_REQUIRE(10 <= it2.j && it2.j <= 20);
    BOOST_REQUIRE(-20 <= it2.l.a && it2.l.a <= -10);
    BOOST_REQUIRE(10 <= it2.l.b && it2.l.b <= 20);
    BOOST_REQUIRE(-20 <= it2.l.c && it2.l.c <= -10);
    BOOST_REQUIRE(10 <= it2.l.d && it2.l.d <= 20);
    BOOST_REQUIRE('a' <= it2.l.f && it2.l.f <= 'z');
    BOOST_REQUIRE('A' <= it2.l.e && it2.l.e <= 'Z');
  }
}

struct Item1 : public rand_obj {
  Item1() : x(this), pivot(0) {
    constraint("c1", x() * x() >= 24);
    constraint("c2", x() <= reference(pivot));
  }

  bool next() {
    int lower = 0;
    int upper = 100;
    while (lower < upper) {
      std::cout << lower << " " << upper << std::endl;
      pivot = (upper + lower) / 2;
      if (constraint.next())
        upper = x;
      else
        lower = pivot + 1;
    }
    x = upper;
    return true;
  }

  randv<unsigned> x;
  int pivot;
};

BOOST_AUTO_TEST_CASE(binary_search_test) {
  VariableDefaultSolver::bypass_constraint_analysis = true;

  Item1 it;
  BOOST_REQUIRE(it.next());
  BOOST_REQUIRE_EQUAL(it.x, 5);

  VariableDefaultSolver::bypass_constraint_analysis = false;
}

struct Item2 : public rand_obj {
  Item2() : i(), address(this), data(this) {
    constraint(address() % 4 == 0);
    constraint(address() <= 1000u);
    constraint(data().size() == 4);
    constraint(foreach (data(), -50 <= data()[i] && data()[i] <= 50));
    constraint(foreach (data(), data()[i - 1] <= data()[i]));
  }

  placeholder i;
  randv<unsigned> address;
  rand_vec<int> data;
};

BOOST_AUTO_TEST_CASE(item_with_vector) {
  Item2 it;
  for (int i = 0; i < 20; i++) {
    BOOST_REQUIRE(it.next());
    BOOST_REQUIRE_EQUAL(it.data.size(), 4);
    std::cout << "@" << it.address << ": " << it.data[0] << " " << it.data[1] << " " << it.data[2] << " " << it.data[3]
              << std::endl;
    BOOST_REQUIRE_EQUAL(it.address % 4, 0);
    BOOST_REQUIRE_LE(it.address, 1000);
    for (unsigned i = 0; i < it.data.size(); i++) {
      BOOST_REQUIRE_LE(-50, it.data[i]);
      BOOST_REQUIRE_LE(it.data[i], 50);
      if (i > 0) BOOST_REQUIRE_LE(it.data[i - 1], it.data[i]);
    }
  }
}

class Constraint_base : public Generator {
 public:
  Constraint_base() : Generator(), constraint(*this) {}

 protected:
  Generator& constraint;
};

class Constraint1 : public Constraint_base {
 public:
  Variable<unsigned> x;

  Constraint1() : Constraint_base() { constraint(x < 10); }
};

class Constraint2 : public Constraint1 {
 public:
  Constraint2() : Constraint1() { constraint(x > 6); }
};

BOOST_AUTO_TEST_CASE(t1) {
  Constraint2 c2;

  c2();
  unsigned r = c2[c2.x];
  BOOST_REQUIRE_LT(r, 10);
  BOOST_REQUIRE_GT(r, 6);

  c2(c2.x != r)();
  r = c2[c2.x];
  BOOST_REQUIRE_LT(r, 10);
  BOOST_REQUIRE_GT(r, 6);

  c2(c2.x != r)();
  r = c2[c2.x];
  BOOST_REQUIRE_LT(r, 10);
  BOOST_REQUIRE_GT(r, 6);

  c2(c2.x != r);
  BOOST_REQUIRE(!c2.next());
}

BOOST_AUTO_TEST_SUITE_END()  // Context

//  vim: ft=cpp:ts=2:sw=2:expandtab


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or generate_n at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/algorithm/generate_n.hpp>
#include <boost/simd/function/enumerate.hpp>
#include <boost/simd/pack.hpp>
#include <numeric>
#include <vector>
#include <simd_test.hpp>

using namespace boost::simd;
using namespace boost::alignment;

struct g
{
  g(float seed = 0.0f, float step = 1.0f)
    : i_(seed),  step_(step)  {}
  template < typename T> T operator()(bs::as_<T>)
  {
    auto z = bs::enumerate<T>(i_, step_);
    i_+= bs::cardinal_of<T>::value*step_;
    return z;
  }
  float i_;
  float step_;
};

struct gstd
{
  gstd(float seed = 0.0f, float step = 1.0f)
    : i_(seed),  step_(step)  {}
  float operator()()
  {
    float z = i_;
    i_+= step_;
    return z;
  }
  float i_;
  float step_;
};

STF_CASE_TPL( "Check unary simd::generate_n", STF_NUMERIC_TYPES )
{
  static const int N = pack<T>::static_size;

  std::vector<T> values(2*N+1), ref(2*N+1);
  std::generate(ref.begin(), ref.begin()+2*N+1, gstd());
  boost::simd::generate_n(values.data(), 2*N+1, g());
  STF_EQUAL( values, ref );
  std::generate(ref.begin(), ref.begin()+2*N+1, gstd(2, 3));
  boost::simd::generate_n(values.data(),2*N+1, g(2, 3));
  STF_EQUAL( values, ref );

}


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

/*
 Copyright (C) 2015 Klaus Spanderen

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

#include <ql/methods/finitedifferences/operators/numericaldifferentiation.hpp>

#ifndef QL_EXTRA_SAFETY_CHECKS
#define BOOST_DISABLE_ASSERTS 1
#endif
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-local-typedefs"
#endif
#include <boost/multi_array.hpp>
#include <utility>
#if defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)) || (__GNUC__ > 4))
#    pragma GCC diagnostic pop
#endif

namespace QuantLib {

    namespace {
        Array calcOffsets(
            Real h, Size n, NumericalDifferentiation::Scheme scheme) {
            QL_REQUIRE(n > 1, "number of steps must be greater than one");

            Array retVal(n);
            switch (scheme) {
              case NumericalDifferentiation::Central:
                QL_REQUIRE(n > 2 && (n % 2),
                    "number of steps must be an odd number greater than two");
                for (Integer i=0; i < Integer(n); ++i)
                    retVal[i] = (i-Integer(n/2))*h;
                break;
              case NumericalDifferentiation::Backward:
                for (Size i=0; i < n; ++i)
                    retVal[i]=-(i*h);
                break;
              case NumericalDifferentiation::Forward:
                for (Size i=0; i < n; ++i)
                    retVal[i]=i*h;
                break;
              default:
                QL_FAIL("unknown numerical differentiation scheme");
            }

            return retVal;
        }

        // This is a C++ implementation of the algorithm/pseudo code in
        // B. Fornberg, 1998. Calculation of Weights
        //                    in Finite Difference Formulas
        // https://amath.colorado.edu/faculty/fornberg/Docs/sirev_cl.pdf
        Array calcWeights(const Array& x, Size M) {
            const Size N = x.size();
            QL_REQUIRE(N > M, "number of points must be greater "
                               "than the order of the derivative");

            boost::multi_array<Real, 3>  d(boost::extents[M+1][N][N]);
            d[0][0][0] = 1.0;
            Real c1 = 1.0;

            for (Size n=1; n < N; ++n) {
                Real c2 = 1.0;
                for (Size nu=0; nu < n; ++nu) {
                    const Real c3 = x[n] - x[nu];
                    c2*=c3;

                    for (Size m=0; m <= std::min(n, M); ++m) {
                        d[m][n][nu] = (x[n]*d[m][n-1][nu]
                             - ((m > 0)? m*d[m-1][n-1][nu] : 0.0))/c3;
                    }
                }

                for (Size m=0; m <= M; ++m) {
                    d[m][n][n] = c1/c2*( ((m > 0)? m*d[m-1][n-1][n-1] : 0.0) -
                        x[n-1]*d[m][n-1][n-1] );
                }
                c1=c2;
            }

            Array retVal(N);
            for (Size i=0; i < N; ++i) {
                retVal[i] = d[M][N-1][i];
            }
            return retVal;
        }
    }

    NumericalDifferentiation::NumericalDifferentiation(ext::function<Real(Real)> f,
                                                       Size orderOfDerivative,
                                                       Array x_offsets)
    : offsets_(std::move(x_offsets)), w_(calcWeights(offsets_, orderOfDerivative)),
      f_(std::move(f)) {}

    NumericalDifferentiation::NumericalDifferentiation(ext::function<Real(Real)> f,
                                                       Size orderOfDerivative,
                                                       Real stepSize,
                                                       Size steps,
                                                       Scheme scheme)
    : offsets_(calcOffsets(stepSize, steps, scheme)), w_(calcWeights(offsets_, orderOfDerivative)),
      f_(std::move(f)) {}
}