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

***************************************************************************/

#define TEST_DOUBLE 0
#define TEST_DEVICE_VECTOR 1
#define TEST_CPU_DEVICE 0
#define TEST_MULTICORE_TBB_SORT 1
#define TEST_AMP 1
#define GOOGLE_TEST 1
#if (GOOGLE_TEST == 1)
#include <gtest/gtest.h>
#include "common/stdafx.h"
#if (TEST_AMP == 0)
#include "common/myocl.h"
#endif
#include "common/test_common.h"
#include <amp.h>
#if (TEST_AMP == 1)
#include <bolt/amp/sort.h>
#include <bolt/miniDump.h>
#include <bolt/unicode.h>
#else
#include <bolt/cl/sort.h>
#include <bolt/miniDump.h>
#include <bolt/unicode.h>
#endif

#include <boost/shared_array.hpp>
#include <array>
#include <algorithm>
/////////////////////////////////////////////////////////////////////////////////////////////////////////
//  Below are helper routines to compare the results of two arrays for googletest
//  They return an assertion object that googletest knows how to track
//This is a compare routine for naked pointers.

// UDD which contains four doubles
//BOLT_FUNCTOR(uddtD4,
struct uddtD4
{
    double a;
    double b;
    double c;
    double d;

    bool operator==(const uddtD4& rhs) const restrict (amp,cpu)
    {
        bool equal = true;
        double th = 0.0000000001;
        if (rhs.a < th && rhs.a > -th)
            equal = ( (1.0*a - rhs.a) < th && (1.0*a - rhs.a) > -th) ? equal : false;
        else
            equal = ( (1.0*a - rhs.a)/rhs.a < th && (1.0*a - rhs.a)/rhs.a > -th) ? equal : false;
        if (rhs.b < th && rhs.b > -th)
            equal = ( (1.0*b - rhs.b) < th && (1.0*b - rhs.b) > -th) ? equal : false;
        else
            equal = ( (1.0*b - rhs.b)/rhs.b < th && (1.0*b - rhs.b)/rhs.b > -th) ? equal : false;
        if (rhs.c < th && rhs.c > -th)
            equal = ( (1.0*c - rhs.c) < th && (1.0*c - rhs.c) > -th) ? equal : false;
        else
            equal = ( (1.0*c - rhs.c)/rhs.c < th && (1.0*c - rhs.c)/rhs.c > -th) ? equal : false;
        if (rhs.d < th && rhs.d > -th)
            equal = ( (1.0*d - rhs.d) < th && (1.0*d - rhs.d) > -th) ? equal : false;
        else
            equal = ( (1.0*d - rhs.d)/rhs.d < th && (1.0*d - rhs.d)/rhs.d > -th) ? equal : false;
        return equal;
    }

};
//);

// Functor for UDD. Adds all four double elements and returns true if lhs_sum > rhs_sum
//BOLT_FUNCTOR(AddD4,
struct AddD4
{
    bool operator()(const uddtD4 &lhs, const uddtD4 &rhs) const restrict (amp,cpu)
    {

        if( ( lhs.a + lhs.b + lhs.c + lhs.d ) > ( rhs.a + rhs.b + rhs.c + rhs.d) )
            return true;
        return false;
    };
};
//);
uddtD4 identityAddD4 = { 1.0, 1.0, 1.0, 1.0 };
uddtD4 initialAddD4  = { 1.00001, 1.000003, 1.0000005, 1.00000007 };

#if 0
TEST(SortUDD, AddDouble4)
{
    //setup containers
    size_t length = (1<<8);
    bolt::amp::device_vector< uddtD4 > input(  length, initialAddD4,  true );
    std::vector< uddtD4 > refInput( length, initialAddD4 );

    // call sort
    AddD4 ad4gt;
    bolt::amp::sort(input.begin(),    input.end(), ad4gt);
    std::sort( refInput.begin(), refInput.end(), ad4gt );

    // compare results
    cmpArrays(refInput, input);
}
#endif
#if (TEST_MULTICORE_TBB_SORT == 1)
TEST(SortUDD, MultiCoreAddDouble4)
{
    //setup containers
    size_t length = (1<<16);
    bolt::amp::control ctl = bolt::amp::control::getDefault( );
    ctl.setForceRunMode(bolt::amp::control::MultiCoreCpu); // tested with serial path also
    bolt::amp::device_vector< uddtD4 > input(  length, initialAddD4);
    std::vector< uddtD4 > refInput( length, initialAddD4 );

    // call sort
    AddD4 ad4gt;
    bolt::amp::sort(ctl, input.begin(), input.end(), ad4gt);
    std::sort( refInput.begin(), refInput.end(), ad4gt );

    // compare results
    cmpArrays(refInput, input);
}

TEST( MultiCoreCPU, MultiCoreFloat )
{
    size_t length = 1<<16;
    bolt::amp::device_vector< float > boltInput(  length, 3.f);
    std::vector< float > stdInput( length, 3.f );

    bolt::amp::control ctl = bolt::amp::control::getDefault( );
    ctl.setForceRunMode(bolt::amp::control::MultiCoreCpu); // tested with serial path also
    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ));
    bolt::amp::sort( ctl, boltInput.begin( ), boltInput.end( ) );

    //  Loop through the array and compare all the values with each other
    cmpArrays( stdInput, boltInput );

}
TEST( MultiCoreCPU, MultiCoreNormal )
{
    size_t length = 1<<16;
    std::vector< float > boltInput( length, 2.f );
    std::vector< float > stdInput( length, 2.f );

    bolt::amp::control ctl = bolt::amp::control::getDefault( );
    ctl.setForceRunMode(bolt::amp::control::MultiCoreCpu); // tested with serial path also
    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ));
    bolt::amp::sort( ctl, boltInput.begin( ), boltInput.end( ) );

    //  Loop through the array and compare all the values with each other
    cmpArrays( stdInput, boltInput );

}
#endif

/////////////////////////////////////////////////////////////////////////////////////////////////////////
//  Fixture classes are now defined to enable googletest to process type parameterized tests

//  This class creates a C++ 'TYPE' out of a size_t value
template< size_t N >
class TypeValue
{
public:
    static const size_t value = N;
};

template <typename T>
T random_gen()
{
    return (T)rand();
}

template <>
int random_gen<int>()
{

    int result = rand();
    static bool toggle = true;
    toggle = toggle?false:true;
    if (toggle)
            return result;
    else
            return -result;
}

template <>
unsigned int random_gen<unsigned int>()
{
    return rand();
}

//  Test fixture class, used for the Type-parameterized tests
//  Namely, the tests that use std::array and TYPED_TEST_P macros
template< typename ArrayTuple >
class SortArrayTest: public ::testing::Test
{
public:
    SortArrayTest( ): m_Errors( 0 )
    {}

    virtual void SetUp( )
    {
        std::generate(stdInput.begin(), stdInput.end(), random_gen<ArrayType>);
        boltInput = stdInput;
    };

    virtual void TearDown( )
    {};

    virtual ~SortArrayTest( )
    {}

protected:
    typedef typename std::tuple_element< 0, ArrayTuple >::type ArrayType;
    static const size_t ArraySize = typename std::tuple_element< 1, ArrayTuple >::type::value;
    typename std::array< ArrayType, ArraySize > stdInput, boltInput;
    int m_Errors;

};

TYPED_TEST_CASE_P( SortArrayTest );

TYPED_TEST_P( SortArrayTest, Normal )
{
    typedef std::array< ArrayType, ArraySize > ArrayCont;

    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ));
    bolt::amp::sort( boltInput.begin( ), boltInput.end( ) );

    ArrayCont::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end() );
    ArrayCont::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end() );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpStdArray< ArrayType, ArraySize >::cmpArrays( stdInput, boltInput );
}

TYPED_TEST_P( SortArrayTest, GPU_DeviceNormal )
{
    //  Create a new command queue for a different device, but use the same context as was provided
    //  by the default control device
    Concurrency::accelerator accel(Concurrency::accelerator::default_accelerator);
    bolt::amp::control c_gpu( accel );  // construct control structure from the queue.

    typedef std::array< ArrayType, ArraySize > ArrayCont;

    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ));
    bolt::amp::sort( c_gpu, boltInput.begin( ), boltInput.end( ) );

    ArrayCont::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end() );
    ArrayCont::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end() );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpStdArray< ArrayType, ArraySize >::cmpArrays( stdInput, boltInput );
    // FIXME - releaseOcl(ocl);
}

#if (TEST_CPU_DEVICE == 1)
TYPED_TEST_P( SortArrayTest, CPU_DeviceNormal )
{
    //  Create a new command queue for a different device, but use the same context as was provided
    //  by the default control device
    Concurrency::accelerator accel(Concurrency::accelerator::cpu_accelerator);
    bolt::amp::control c_cpu( accel );  // construct control structure from the queue.

    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ));
    bolt::cl::sort( c_cpu, boltInput.begin( ), boltInput.end( ) );

    ArrayCont::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end() );
    ArrayCont::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end() );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpStdArray< ArrayType, ArraySize >::cmpArrays( stdInput, boltInput );
    // FIXME - releaseOcl(ocl);
}
#endif

TYPED_TEST_P( SortArrayTest, GreaterFunction )
{
    typedef std::array< ArrayType, ArraySize > ArrayCont;

    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ), std::greater< ArrayType >() );

    bolt::amp::sort( boltInput.begin( ), boltInput.end( ), bolt::amp::greater< ArrayType >( ) );

    ArrayCont::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end() );
    ArrayCont::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end() );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpStdArray< ArrayType, ArraySize >::cmpArrays( stdInput, boltInput );

}

TYPED_TEST_P( SortArrayTest, GPU_DeviceGreaterFunction )
{
    //  Create a new command queue for a different device, but use the same context as was provided
    //  by the default control device
    Concurrency::accelerator accel(Concurrency::accelerator::default_accelerator);
    bolt::amp::control c_gpu( accel );  // construct control structure from the queue.

    typedef std::array< ArrayType, ArraySize > ArrayCont;

    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ), std::greater< ArrayType >( ));
    bolt::amp::sort( c_gpu, boltInput.begin( ), boltInput.end( ), bolt::amp::greater< ArrayType >( ) );

    ArrayCont::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end() );
    ArrayCont::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end() );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpStdArray< ArrayType, ArraySize >::cmpArrays( stdInput, boltInput );
    // FIXME - releaseOcl(ocl);
}
#if (TEST_CPU_DEVICE == 1)
TYPED_TEST_P( SortArrayTest, CPU_DeviceGreaterFunction )
{
    typedef std::array< ArrayType, ArraySize > ArrayCont;
    Concurrency::accelerator accel(Concurrency::accelerator::default_accelerator);
    bolt::amp::control c_gpu( accel );  // construct control structure from the queue.

    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ), std::greater< ArrayType >( ));
    bolt::amp::sort( c_cpu, boltInput.begin( ), boltInput.end( ), bolt::amp::greater< ArrayType >( ) );

    ArrayCont::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end() );
    ArrayCont::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end() );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpStdArray< ArrayType, ArraySize >::cmpArrays( stdInput, boltInput );
    // FIXME - releaseOcl(ocl);
}
#endif
TYPED_TEST_P( SortArrayTest, LessFunction )
{
    typedef std::array< ArrayType, ArraySize > ArrayCont;

    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ), std::less<ArrayType>());
    bolt::amp::sort( boltInput.begin( ), boltInput.end( ), bolt::amp::less<ArrayType>() );

    ArrayCont::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end() );
    ArrayCont::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end() );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpStdArray< ArrayType, ArraySize >::cmpArrays( stdInput, boltInput );
}

TYPED_TEST_P( SortArrayTest, GPU_DeviceLessFunction )
{
    typedef std::array< ArrayType, ArraySize > ArrayCont;
    //  Create a new command queue for a different device, but use the same context as was provided
    //  by the default control device
    Concurrency::accelerator accel(Concurrency::accelerator::default_accelerator);
    bolt::amp::control c_gpu( accel );  // construct control structure from the queue.

    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ), std::less< ArrayType >( ));
    bolt::amp::sort( c_gpu, boltInput.begin( ), boltInput.end( ), bolt::amp::less< ArrayType >( ) );

    ArrayCont::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end() );
    ArrayCont::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end() );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpStdArray< ArrayType, ArraySize >::cmpArrays( stdInput, boltInput );
    // FIXME - releaseOcl(ocl);
}

#if (TEST_CPU_DEVICE == 1)
TYPED_TEST_P( SortArrayTest, CPU_DeviceLessFunction )
{
    typedef std::array< ArrayType, ArraySize > ArrayCont;

    Concurrency::accelerator accel(Concurrency::accelerator::default_accelerator);
    bolt::amp::control c_gpu( accel );  // construct control structure from the queue.

    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ), std::less< ArrayType >( ));
    bolt::amp::sort( c_cpu, boltInput.begin( ), boltInput.end( ), bolt::amp::less< ArrayType >( ) );

    ArrayCont::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end() );
    ArrayCont::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end() );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpStdArray< ArrayType, ArraySize >::cmpArrays( stdInput, boltInput );
    // FIXME - releaseOcl(ocl);
}
#endif

#if (TEST_CPU_DEVICE == 1)
REGISTER_TYPED_TEST_CASE_P( SortArrayTest, Normal, GPU_DeviceNormal,
                                           GreaterFunction, GPU_DeviceGreaterFunction,
                                           LessFunction, GPU_DeviceLessFunction, CPU_DeviceNormal, CPU_DeviceGreaterFunction, CPU_DeviceLessFunction);
#else
REGISTER_TYPED_TEST_CASE_P( SortArrayTest, Normal, GPU_DeviceNormal,
                                           GreaterFunction, GPU_DeviceGreaterFunction,
                                           LessFunction, GPU_DeviceLessFunction );
#endif

#if (TEST_MULTICORE_TBB_SORT == 1)
//REGISTER_TYPED_TEST_CASE_P( SortArrayTest, MultiCoreNormal );
#endif

typedef ::testing::Types<
    std::tuple< int, TypeValue< 1 > >,
    std::tuple< int, TypeValue< 31 > >,
    std::tuple< int, TypeValue< 32 > >,
    std::tuple< int, TypeValue< 63 > >,
    std::tuple< int, TypeValue< 64 > >,
    std::tuple< int, TypeValue< 127 > >,
    std::tuple< int, TypeValue< 128 > >,
    std::tuple< int, TypeValue< 129 > >,
    std::tuple< int, TypeValue< 1000 > >,
    std::tuple< int, TypeValue< 1053 > >,
    std::tuple< int, TypeValue< 4096 > >,
    std::tuple< int, TypeValue< 4097 > >,
    std::tuple< int, TypeValue< 65535 > >,
    std::tuple< int, TypeValue< 65536 > >
> IntegerTests;

typedef ::testing::Types<
    std::tuple< unsigned int, TypeValue< 1 > >,
    std::tuple< unsigned int, TypeValue< 31 > >,
    std::tuple< unsigned int, TypeValue< 32 > >,
    std::tuple< unsigned int, TypeValue< 63 > >,
    std::tuple< unsigned int, TypeValue< 64 > >,
    std::tuple< unsigned int, TypeValue< 127 > >,
    std::tuple< unsigned int, TypeValue< 128 > >,
    std::tuple< unsigned int, TypeValue< 129 > >,
    std::tuple< unsigned int, TypeValue< 1000 > >,
    std::tuple< unsigned int, TypeValue< 1053 > >,
    std::tuple< unsigned int, TypeValue< 4096 > >,
    std::tuple< unsigned int, TypeValue< 4097 > >,
    std::tuple< unsigned int, TypeValue< 65535 > >,
    std::tuple< unsigned int, TypeValue< 65536 > >
> UnsignedIntegerTests;

typedef ::testing::Types<
    std::tuple< float, TypeValue< 1 > >,
    //std::tuple< float, TypeValue< 31 > >,
    std::tuple< float, TypeValue< 32 > >,
    //std::tuple< float, TypeValue< 63 > >,
    std::tuple< float, TypeValue< 64 > >,
    //std::tuple< float, TypeValue< 127 > >,
    std::tuple< float, TypeValue< 128 > >,
    //std::tuple< float, TypeValue< 129 > >,
    //std::tuple< float, TypeValue< 1000 > >,
    //std::tuple< float, TypeValue< 1053 > >,
    std::tuple< float, TypeValue< 4096 > >,
    //std::tuple< float, TypeValue< 4097 > >,
    //std::tuple< float, TypeValue< 65535 > >,
    std::tuple< float, TypeValue< 65536 > >
> FloatTests;

#if (TEST_DOUBLE == 1)
typedef ::testing::Types<
    std::tuple< double, TypeValue< 1 > >,
    std::tuple< double, TypeValue< 31 > >,
    std::tuple< double, TypeValue< 32 > >,
    std::tuple< double, TypeValue< 63 > >,
    std::tuple< double, TypeValue< 64 > >,
    std::tuple< double, TypeValue< 127 > >,
    std::tuple< double, TypeValue< 128 > >,
    std::tuple< double, TypeValue< 129 > >,
    std::tuple< double, TypeValue< 1000 > >,
    std::tuple< double, TypeValue< 1053 > >,
    std::tuple< double, TypeValue< 4096 > >,
    std::tuple< double, TypeValue< 4097 > >,
    std::tuple< double, TypeValue< 65535 > >,
    std::tuple< double, TypeValue< 65536 > >
> DoubleTests;
#endif

INSTANTIATE_TYPED_TEST_CASE_P( Integer, SortArrayTest, IntegerTests );
INSTANTIATE_TYPED_TEST_CASE_P( UnsignedInteger, SortArrayTest, UnsignedIntegerTests );
INSTANTIATE_TYPED_TEST_CASE_P( Float, SortArrayTest, FloatTests );
#if (TEST_DOUBLE == 1)
INSTANTIATE_TYPED_TEST_CASE_P( Double, SortArrayTest, DoubleTests );
#endif

#if 1
/////////////////////////////////////////////////////////////////////////////////////////////////////////
//  Fixture classes are now defined to enable googletest to process value parameterized tests
//  ::testing::TestWithParam< int > means that GetParam( ) returns int values, which i use for array size

class SortIntegerVector: public ::testing::TestWithParam< int >
{
public:

    SortIntegerVector( ): stdInput( GetParam( ) ), boltInput( GetParam( ) )
    {
        std::generate(stdInput.begin(), stdInput.end(), rand);
        boltInput = stdInput;
    }

protected:
    std::vector< int > stdInput, boltInput;
};

//  ::testing::TestWithParam< int > means that GetParam( ) returns int values, which i use for array size
class SortFloatVector: public ::testing::TestWithParam< int >
{
public:
    SortFloatVector( ): stdInput( GetParam( ) ), boltInput( GetParam( ) )
    {
        std::generate(stdInput.begin(), stdInput.end(), rand);
        boltInput = stdInput;
    }

protected:
    std::vector< float > stdInput, boltInput;
};

#if (TEST_DOUBLE == 1)
//  ::testing::TestWithParam< int > means that GetParam( ) returns int values, which i use for array size
class SortDoubleVector: public ::testing::TestWithParam< int >
{
public:
    SortDoubleVector( ): stdInput( GetParam( ) ), boltInput( GetParam( ) )
    {
        std::generate(stdInput.begin(), stdInput.end(), rand);
        boltInput = stdInput;
    }

protected:
    std::vector< double > stdInput, boltInput;
};
#endif

//  ::testing::TestWithParam< int > means that GetParam( ) returns int values, which i use for array size
class SortIntegerDeviceVector: public ::testing::TestWithParam< int >
{
public:
    // Create an std and a bolt vector of requested size, and initialize all the elements to 1
    SortIntegerDeviceVector( ): stdInput( GetParam( ) ), boltInput( static_cast<size_t>( GetParam( ) ) )
    {
        std::generate(stdInput.begin(), stdInput.end(), rand);
        //boltInput = stdInput;
        //FIXME - The above should work but the below loop is used.
        for (int i=0; i< GetParam( ); i++)
        {
            boltInput[i] = stdInput[i];
        }
    }

protected:
    std::vector< int > stdInput;
    bolt::amp::device_vector< int > boltInput;
};

//  ::testing::TestWithParam< int > means that GetParam( ) returns int values, which i use for array size
class SortFloatDeviceVector: public ::testing::TestWithParam< int >
{
public:
    // Create an std and a bolt vector of requested size, and initialize all the elements to 1
    SortFloatDeviceVector( ): stdInput( GetParam( ) ), boltInput( static_cast<size_t>( GetParam( ) ) )
    {
        std::generate(stdInput.begin(), stdInput.end(), random_gen<int>);
        //boltInput = stdInput;
        //FIXME - The above should work but the below loop is used.
        for (int i=0; i< GetParam( ); i++)
        {
            boltInput[i] = stdInput[i];
        }

    }

protected:
    std::vector< float > stdInput;
    bolt::amp::device_vector< float > boltInput;
};

#if (TEST_DOUBLE == 1)
//  ::testing::TestWithParam< int > means that GetParam( ) returns int values, which i use for array size
class SortDoubleDeviceVector: public ::testing::TestWithParam< int >
{
public:
    // Create an std and a bolt vector of requested size, and initialize all the elements to 1
    SortDoubleDeviceVector( ): stdInput( GetParam( ) ), boltInput( static_cast<size_t>( GetParam( ) ) )
    {
        std::generate(stdInput.begin(), stdInput.end(), rand);
        //boltInput = stdInput;
        //FIXME - The above should work but the below loop is used.
        for (int i=0; i< GetParam( ); i++)
        {
            boltInput[i] = stdInput[i];
        }
    }

protected:
    std::vector< double > stdInput;
    bolt::amp::device_vector< double > boltInput;
};
#endif

//  ::testing::TestWithParam< int > means that GetParam( ) returns int values, which i use for array size
class SortIntegerNakedPointer: public ::testing::TestWithParam< int >
{
public:
    //  Create an std and a bolt vector of requested size, and initialize all the elements to 1
    SortIntegerNakedPointer( ): stdInput( new int[ GetParam( ) ] ), boltInput( new int[ GetParam( ) ] )
    {}

    virtual void SetUp( )
    {
        size_t size = GetParam( );

        std::generate(stdInput, stdInput + size, rand);
        for (int i = 0; i<size; i++)
        {
            boltInput[i] = stdInput[i];
        }
    };

    virtual void TearDown( )
    {
        delete [] stdInput;
        delete [] boltInput;
    };

protected:
     int* stdInput;
     int* boltInput;
};

//  ::testing::TestWithParam< int > means that GetParam( ) returns int values, which i use for array size
class SortFloatNakedPointer: public ::testing::TestWithParam< int >
{
public:
    //  Create an std and a bolt vector of requested size, and initialize all the elements to 1
    SortFloatNakedPointer( ): stdInput( new float[ GetParam( ) ] ), boltInput( new float[ GetParam( ) ] )
    {}

    virtual void SetUp( )
    {
        size_t size = GetParam( );

        std::generate(stdInput, stdInput + size, rand);
        for (int i = 0; i<size; i++)
        {
            boltInput[i] = stdInput[i];
        }
    };

    virtual void TearDown( )
    {
        delete [] stdInput;
        delete [] boltInput;
    };

protected:
     float* stdInput;
     float* boltInput;
};

#if (TEST_DOUBLE ==1 )
//  ::testing::TestWithParam< int > means that GetParam( ) returns int values, which i use for array size
class SortDoubleNakedPointer: public ::testing::TestWithParam< int >
{
public:
    //  Create an std and a bolt vector of requested size, and initialize all the elements to 1
    SortDoubleNakedPointer( ): stdInput( new double[ GetParam( ) ] ), boltInput( new double[ GetParam( ) ] )
    {}

    virtual void SetUp( )
    {
        size_t size = GetParam( );

        std::generate(stdInput, stdInput + size, rand);

        for( int i=0; i < size; i++ )
        {
            boltInput[ i ] = stdInput[ i ];
        }
    };

    virtual void TearDown( )
    {
        delete [] stdInput;
        delete [] boltInput;
    };

protected:
     double* stdInput;
     double* boltInput;
};
#endif

TEST_P( SortIntegerVector, Normal )
{
    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ) );
    bolt::amp::sort( boltInput.begin( ), boltInput.end( ) );

    std::vector< int >::iterator::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end( ) );
    std::vector< int >::iterator::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end( ) );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpArrays( stdInput, boltInput );
}

// Come Back here
TEST_P( SortFloatVector, Normal )
{
    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ) );
    bolt::amp::sort( boltInput.begin( ), boltInput.end( ) );

    std::vector< float >::iterator::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end( ) );
    std::vector< float >::iterator::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end( ) );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpArrays( stdInput, boltInput );
}
#if (TEST_DOUBLE == 1)
TEST_P( SortDoubleVector, Inplace )
{
    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ) );
    bolt::amp::sort( boltInput.begin( ), boltInput.end( ) );

    std::vector< double >::iterator::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end( ) );
    std::vector< double >::iterator::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end( ) );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpArrays( stdInput, boltInput );
}
#endif
#if (TEST_DEVICE_VECTOR == 1)
TEST_P( SortIntegerDeviceVector, Inplace )
{
    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ) );
    bolt::amp::sort( boltInput.begin( ), boltInput.end( ) );

    std::vector< int >::iterator::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end( ) );
    std::vector< int >::iterator::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end( ) );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpArrays( stdInput, boltInput );
}
TEST_P( SortFloatDeviceVector, Inplace )
{
    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ) );
    bolt::amp::sort( boltInput.begin( ), boltInput.end( ) );

    std::vector< float >::iterator::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end( ) );
    std::vector< float >::iterator::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end( ) );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpArrays( stdInput, boltInput );
}
#if (TEST_DOUBLE == 1)
TEST_P( SortDoubleDeviceVector, Inplace )
{
    //  Calling the actual functions under test
    std::sort( stdInput.begin( ), stdInput.end( ) );
    bolt::amp::sort( boltInput.begin( ), boltInput.end( ) );

    std::vector< double >::iterator::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end( ) );
    std::vector< double >::iterator::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end( ) );

    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );

    //  Loop through the array and compare all the values with each other
    cmpArrays( stdInput, boltInput );
}
#endif
#endif

TEST_P( SortIntegerNakedPointer, Inplace )
{
    size_t endIndex = GetParam( );

    //  Calling the actual functions under test
    stdext::checked_array_iterator< int* > wrapStdInput( stdInput, endIndex );
    std::sort( wrapStdInput, wrapStdInput + endIndex );

    stdext::checked_array_iterator< int* > wrapBoltInput( boltInput, endIndex );
    bolt::amp::sort( wrapBoltInput, wrapBoltInput + endIndex );

    //  Loop through the array and compare all the values with each other
    cmpArrays( stdInput, boltInput, endIndex );
}

TEST_P( SortFloatNakedPointer, Inplace )
{
    size_t endIndex = GetParam( );

    //  Calling the actual functions under test
    stdext::checked_array_iterator< float* > wrapStdInput( stdInput, endIndex );
    std::sort( wrapStdInput, wrapStdInput + endIndex );
    //std::sort( stdInput, stdInput + endIndex );

    stdext::checked_array_iterator< float* > wrapBoltInput( boltInput, endIndex );
    bolt::amp::sort( wrapBoltInput, wrapBoltInput + endIndex );

    //  Loop through the array and compare all the values with each other
    cmpArrays( stdInput, boltInput, endIndex );
}

#if (TEST_DOUBLE == 1)
TEST_P( SortDoubleNakedPointer, Inplace )
{
    size_t endIndex = GetParam( );

    //  Calling the actual functions under test
    stdext::checked_array_iterator< double* > wrapStdInput( stdInput, endIndex );
    std::sort( wrapStdInput, wrapStdInput + endIndex );
    //std::sort( stdInput, stdInput + endIndex );

    stdext::checked_array_iterator< double* > wrapBoltInput( boltInput, endIndex );
    bolt::amp::sort( wrapBoltInput, wrapBoltInput + endIndex );

    //  Loop through the array and compare all the values with each other
    cmpArrays( stdInput, boltInput, endIndex );
}
#endif
std::array<int, 15> TestValues = {2,4,8,16,32,64,128,256,512,1024,2048,4096,8192,16384,32768};
//Test lots of consecutive numbers, but small range, suitable for integers because they overflow easier
//INSTANTIATE_TEST_CASE_P( SortRange, SortIntegerVector, ::testing::Range( 0, 1024, 7 ) );
INSTANTIATE_TEST_CASE_P( SortValues, SortIntegerVector, ::testing::ValuesIn( TestValues.begin(), TestValues.end() ) );
//INSTANTIATE_TEST_CASE_P( SortRange, SortFloatVector, ::testing::Range( 0, 1024, 3 ) );
INSTANTIATE_TEST_CASE_P( SortValues, SortFloatVector, ::testing::ValuesIn( TestValues.begin(), TestValues.end() ) );
#if (TEST_DOUBLE == 1)
//INSTANTIATE_TEST_CASE_P( SortRange, SortDoubleVector, ::testing::Range( 0, 1024, 21 ) );
INSTANTIATE_TEST_CASE_P( SortValues, SortDoubleVector, ::testing::ValuesIn( TestValues.begin(), TestValues.end() ) );
#endif
//INSTANTIATE_TEST_CASE_P( SortRange, SortIntegerDeviceVector, ::testing::Range( 0, 1024, 53 ) );
INSTANTIATE_TEST_CASE_P( SortValues, SortIntegerDeviceVector, ::testing::ValuesIn( TestValues.begin(), TestValues.end() ) );
//INSTANTIATE_TEST_CASE_P( SortRange, SortFloatDeviceVector, ::testing::Range( 0, 1024, 53 ) );
INSTANTIATE_TEST_CASE_P( SortValues, SortFloatDeviceVector, ::testing::ValuesIn( TestValues.begin(), TestValues.end() ) );
#if (TEST_DOUBLE == 1)
//INSTANTIATE_TEST_CASE_P( SortRange, SortDoubleDeviceVector, ::testing::Range( 0, 1024, 53 ) );
INSTANTIATE_TEST_CASE_P( SortValues, SortDoubleDeviceVector, ::testing::ValuesIn( TestValues.begin(), TestValues.end() ) );
#endif
//INSTANTIATE_TEST_CASE_P( SortRange, SortIntegerNakedPointer, ::testing::Range( 0, 1024, 13) );
INSTANTIATE_TEST_CASE_P( SortValues, SortIntegerNakedPointer, ::testing::ValuesIn( TestValues.begin(), TestValues.end() ) );
//INSTANTIATE_TEST_CASE_P( SortRange, SortFloatNakedPointer, ::testing::Range( 0, 1024, 13) );
INSTANTIATE_TEST_CASE_P( SortValues, SortFloatNakedPointer, ::testing::ValuesIn( TestValues.begin(), TestValues.end() ) );
#if (TEST_DOUBLE == 1)
//INSTANTIATE_TEST_CASE_P( SortRange, SortDoubleNakedPointer, ::testing::Range( 0, 1024, 13) );
INSTANTIATE_TEST_CASE_P( Sort, SortDoubleNakedPointer, ::testing::ValuesIn( TestValues.begin(), TestValues.end() ) );
#endif
#endif

/*******UDD testing **********/
#if 0
//BOLT_FUNCTOR(UDD,
struct UDD {
    int a;
    int b;

    bool operator() (const UDD& lhs, const UDD& rhs) const restrict (amp, cpu){
        return ((lhs.a+lhs.b) > (rhs.a+rhs.b));
    }
    bool operator < (const UDD& other) const restrict (amp, cpu) {
        return ((a+b) < (other.a+other.b));
    }
    bool operator > (const UDD& other) const restrict (amp, cpu) {
        return ((a+b) > (other.a+other.b));
    }
    bool operator == (const UDD& other) const restrict (amp, cpu) {
        return ((a+b) == (other.a+other.b));
    }
    UDD() restrict (amp, cpu)
        : a(0),b(0) { }

    UDD(int _in) restrict (amp, cpu)
        : a(_in), b(_in +1)  { }

    static UDD random()
    {
        UDD udd;
        udd.a = random_gen<int>();
        udd.b = random_gen<int>();
        return udd;
    }
};
//);

//BOLT_FUNCTOR(sortBy_UDD_a,
    struct sortBy_UDD_a {
        bool operator() (const UDD& a, const UDD& b) const restrict(amp, cpu)
        {
            return (a.a>b.a);
        };
    };
//);

//BOLT_FUNCTOR(sortBy_UDD_b,
    struct sortBy_UDD_b {
        bool operator() (UDD& a, UDD& b) const restrict(amp, cpu)
        {
            return (a.b>b.b);
        };
    };
//);

//BOLT_CREATE_TYPENAME(bolt::cl::less<UDD>);
//BOLT_CREATE_TYPENAME(bolt::cl::greater<UDD>);

//BOLT_CREATE_TYPENAME( bolt::cl::device_vector< UDD >::iterator );
//BOLT_CREATE_CLCODE( bolt::cl::device_vector< UDD >::iterator, bolt::cl::deviceVectorIteratorTemplate );

template< typename ArrayTuple >
class SortUDDArrayTest: public ::testing::Test
{
public:
    SortUDDArrayTest( ): m_Errors( 0 )
    {}

    virtual void SetUp( )
    {
        std::generate(stdInput.begin(), stdInput.end(), UDD::random);
        boltInput = stdInput;
    };

    virtual void TearDown( )
    {};

    virtual ~SortUDDArrayTest( )
    {}

protected:
    typedef typename std::tuple_element< 0, ArrayTuple >::type ArrayType;
    static const size_t ArraySize = typename std::tuple_element< 1, ArrayTuple >::type::value;
    typename std::array< ArrayType, ArraySize > stdInput, boltInput;
    int m_Errors;
};

TYPED_TEST_CASE_P( SortUDDArrayTest );

TYPED_TEST_P( SortUDDArrayTest, Normal )
{
    typedef std::array< ArrayType, ArraySize > ArrayCont;
    //  Calling the actual functions under test

    std::sort( stdInput.begin( ), stdInput.end( ), UDD() );
    bolt::amp::sort( boltInput.begin( ), boltInput.end( ), UDD() );

    ArrayCont::difference_type stdNumElements = std::distance( stdInput.begin( ), stdInput.end() );
    ArrayCont::difference_type boltNumElements = std::distance( boltInput.begin( ), boltInput.end() );
    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );
    //  Loop through the array and compare all the values with each other
    cmpStdArray< ArrayType, ArraySize >::cmpArrays( stdInput, boltInput );

    std::sort( stdInput.begin( ), stdInput.end( ), sortBy_UDD_a() );
    bolt::amp::sort( boltInput.begin( ), boltInput.end( ), sortBy_UDD_a() );

    stdNumElements = std::distance( stdInput.begin( ), stdInput.end() );
    boltNumElements = std::distance( boltInput.begin( ), boltInput.end() );
    //  Both collections should have the same number of elements
    EXPECT_EQ( stdNumElements, boltNumElements );
    //  Loop through the array and compare all the values with each other
    cmpStdArray< ArrayType, ArraySize >::cmpArrays( stdInput, boltInput );

}

typedef ::testing::Types<
    std::tuple< UDD, TypeValue< 1 > >,
    //std::tuple< UDD, TypeValue< 31 > >,
    std::tuple< UDD, TypeValue< 32 > >,
    //std::tuple< UDD, TypeValue< 63 > >,
    std::tuple< UDD, TypeValue< 64 > >,
    //std::tuple< UDD, TypeValue< 127 > >,
    std::tuple< UDD, TypeValue< 128 > >,
    //std::tuple< UDD, TypeValue< 129 > >,
    //std::tuple< UDD, TypeValue< 1000 > >,
    //std::tuple< UDD, TypeValue< 1053 > >,
    std::tuple< UDD, TypeValue< 4096 > >,
    //std::tuple< UDD, TypeValue< 4097 > >
    //std::tuple< UDD, TypeValue< 65535 > >,
    std::tuple< UDD, TypeValue< 65536 > >
> UDDTests;

REGISTER_TYPED_TEST_CASE_P( SortUDDArrayTest,  Normal);
INSTANTIATE_TYPED_TEST_CASE_P( UDDTest, SortUDDArrayTest, UDDTests );
#endif
int main(int argc, char* argv[])
{
    ::testing::InitGoogleTest( &argc, &argv[ 0 ] );

    //  Register our minidump generating logic
    bolt::miniDumpSingleton::enableMiniDumps( );

    int retVal = RUN_ALL_TESTS( );

    //  Reflection code to inspect how many tests failed in gTest
    ::testing::UnitTest& unitTest = *::testing::UnitTest::GetInstance( );

    unsigned int failedTests = 0;
    for( int i = 0; i < unitTest.total_test_case_count( ); ++i )
    {
        const ::testing::TestCase& testCase = *unitTest.GetTestCase( i );
        for( int j = 0; j < testCase.total_test_count( ); ++j )
        {
            const ::testing::TestInfo& testInfo = *testCase.GetTestInfo( j );
            if( testInfo.result( )->Failed( ) )
                ++failedTests;
        }
    }

    //  Print helpful message at termination if we detect errors, to help users figure out what to do next
    if( failedTests )
    {
        bolt::tout << _T( "\nFailed tests detected in test pass; please run test again with:" ) << std::endl;
        bolt::tout << _T( "\t--gtest_filter=<XXX> to select a specific failing test of interest" ) << std::endl;
        bolt::tout << _T( "\t--gtest_catch_exceptions=0 to generate minidump of failing test, or" ) << std::endl;
        bolt::tout << _T( "\t--gtest_break_on_failure to debug interactively with debugger" ) << std::endl;
        bolt::tout << _T( "\t    (only on googletest assertion failures, not SEH exceptions)" ) << std::endl;
    }
    std::cout << "Test Completed. Press Enter to exit.\n .... ";
    //getchar();
    return retVal;
}
#else
//BOLT Header files
#include "common/myocl.h"
#include <bolt/cl/clcode.h>
#include <bolt/cl/device_vector.h>
#include <bolt/cl/sort.h>
#include <bolt/cl/functional.h>
#include <bolt/cl/control.h>

//STD Header files
#include <iostream>
#include <algorithm>  // for testing against STL functions.
#include <vector>

// A Data structure defining a less than operator

template <typename T>
struct MyType {
    T a;

    bool operator() (const MyType& lhs, const MyType& rhs) {
        return (lhs.a > rhs.a);
    }
    bool operator < (const MyType& other) const {
        return (a < other.a);
    }
    bool operator > (const MyType& other) const {
        return (a > other.a);
    }
    MyType(const MyType &other)
        : a(other.a) { }
    MyType()
        : a(0) { }
    MyType(T& _in)
        : a(_in) { }
};

BOLT_CREATE_TYPENAME(MyType<int>);
BOLT_CREATE_CLCODE(MyType<int>, "template <typename T> struct MyType { T a; \n\nbool operator() (const MyType& lhs, const MyType& rhs) { return (lhs.a > rhs.a); } \n\nbool operator < (const MyType& other) const { return (a < other.a); }\n\n bool operator > (const MyType& other) const { return (a > other.a);} \n\n };");
BOLT_CREATE_TYPENAME(MyType<float>);
BOLT_CREATE_CLCODE(MyType<float>, "template <typename T> struct MyType { T a; \n\nbool operator() (const MyType& lhs, const MyType& rhs) { return (lhs.a > rhs.a); } \n\nbool operator < (const MyType& other) const { return (a < other.a); } \n\n bool operator > (const MyType& other) const { return (a > other.a);} \n\n };");
BOLT_CREATE_TYPENAME(MyType<double>);
BOLT_CREATE_CLCODE(MyType<double>, "template <typename T> struct MyType { T a; \n\nbool operator() (const MyType& lhs, const MyType& rhs) { return (lhs.a > rhs.a); } \n\nbool operator < (const MyType& other) const { return (a < other.a); }\n\n bool operator > (const MyType& other) const { return (a > other.a);} \n\n };");

BOLT_CREATE_TYPENAME(bolt::cl::less< MyType<int> >);
// A Data structure defining a Functor
template <typename T>
struct MyFunctor{
    T a;
    T b;

    bool operator() (const MyFunctor& lhs, const MyFunctor& rhs) {
        return (lhs.a > rhs.a);
    }
    bool operator < (const MyFunctor& other) const {
        return (a < other.a);
    }
    bool operator > (const MyFunctor& other) const {
        return (a > other.a);
    }
    MyFunctor(const MyFunctor &other)
        : a(other.a), b(other.b) { }
    MyFunctor()
        : a(0), b(0) { }
    MyFunctor(T& _in)
        : a(_in), b(_in) { }
};
BOLT_CREATE_TYPENAME(MyFunctor<int>);
BOLT_CREATE_CLCODE(MyFunctor<int>, "template<typename T> struct MyFunctor { T a; T b; \n\nbool operator() (const MyFunctor& lhs, const MyFunctor& rhs) { return (lhs.a > rhs.a); }   \n\nbool operator < (const MyFunctor& other) const { return (a < other.a); }   \n\nbool operator > (const MyFunctor& other) const { return (a > other.a);}  \n\n}; \n\n");
BOLT_CREATE_TYPENAME(MyFunctor<float>);
BOLT_CREATE_CLCODE(MyFunctor<float>, "template<typename T> struct MyFunctor { T a; T b; \n\nbool operator() (const MyFunctor& lhs, const MyFunctor& rhs) { return (lhs.a > rhs.a); }   \n\nbool operator < (const MyFunctor& other) const { return (a < other.a); }   \n\nbool operator > (const MyFunctor& other) const { return (a > other.a);}  \n\n}; \n\n");
BOLT_CREATE_TYPENAME(MyFunctor<double>);
BOLT_CREATE_CLCODE(MyFunctor<double>, "template<typename T> struct MyFunctor { T a; T b; \n\nbool operator() (const MyFunctor& lhs, const MyFunctor& rhs) { return (lhs.a > rhs.a); }   \n\nbool operator < (const MyFunctor& other) const { return (a < other.a); }   \n\nbool operator > (const MyFunctor& other) const { return (a > other.a);}  \n\n}; \n\n");

template <typename T>
bool FUNCTION (T &i,T &j) { return (i<j); }

template <typename stdType>
void UserDefinedLambdaSortTestOfLength(size_t length)
{
    std::vector<stdType> stdInput(length);
    std::vector<stdType> boltInput(length);
    std::vector<stdType>::iterator it;
    auto func = [](const stdType & a, const stdType & b) {  return a < b;  };

    size_t i;
    for (i=0;i<length;i++)
    {
        boltInput[i]= (stdType)(length - i +2);
        stdInput[i]= (stdType)(length - i +2);
    }

    bolt::cl::sort(boltInput.begin(), boltInput.end(), func," [](const stdType & a, const stdType & b) {  return a < b;  };");
    std::sort(stdInput.begin(), stdInput.end(),func);
    for (i=0; i<length; i++)
    {
        if(stdInput[i] == boltInput[i])
            continue;
        else
            break;
    }
    if (i==length)
        std::cout << "Test Passed" <<std::endl;
    else
        std::cout << "Test Failed i = " << i <<std::endl;
}

// This is a test case for handling function pointers
// OpenCL will not handle this so commented it out completely
#if 0
template <typename stdType>
void UserDefinedFunctionSortTestOfLength(size_t length)
{
    std::vector<stdType> stdInput(length);
    std::vector<stdType> boltInput(length);

    typedef bool (*MyFunction)(stdType &i,stdType &j);
    size_t i;
    for (i=0;i<length;i++)
    {
        boltInput[i] = (stdType)(length - i +2);
        stdInput[i]  = (stdType)(length - i +2);
    }
    MyFunction function = FUNCTION<stdType>;
    std::string functionString("bool FUNCTION(" + std::string(typeid(stdType).name()) + " in1, " + std::string(typeid(stdType).name()) + " in2) { return (in1 < in2); }");
    bolt::cl::sort(boltInput.begin(), boltInput.end(), functionString);
    std::sort(stdInput.begin(), stdInput.end(), function);
    for (i=0; i<length; i++)
    {
        if(stdInput[i] == boltInput[i])
            continue;
        else
            break;
    }
    if (i==length)
        std::cout << "Test Passed" <<std::endl;
    else
        std::cout << "Test Failed i = " << i <<std::endl;
}
#endif

BOLT_CREATE_TYPENAME(bolt::cl::greater< MyType<int> >);
BOLT_CREATE_TYPENAME(bolt::cl::greater< MyType<float> >);
BOLT_CREATE_TYPENAME(bolt::cl::greater< MyType<double> >);
BOLT_CREATE_TYPENAME(bolt::cl::greater< MyFunctor<int> >);
BOLT_CREATE_TYPENAME(bolt::cl::greater< MyFunctor<float> >);
BOLT_CREATE_TYPENAME(bolt::cl::greater< MyFunctor<double> >);
BOLT_CREATE_TYPENAME(bolt::cl::device_vector< bolt::cl::greater<MyFunctor<int>> >::iterator );
BOLT_CREATE_TYPENAME(bolt::cl::device_vector< bolt::cl::greater<MyFunctor<float>> >::iterator );
BOLT_CREATE_TYPENAME(bolt::cl::device_vector< bolt::cl::greater<MyFunctor<double>> >::iterator );
template <typename stdType>
void UserDefinedBoltFunctorSortTestOfLength(size_t length)
{
    typedef MyFunctor<stdType> myfunctor;

    std::vector<myfunctor> stdInput(length);
    std::vector<myfunctor> boltInput(length);

    size_t i;
    for (i=0;i<length;i++)
    {
        boltInput[i].a = (stdType)(i +2);
        stdInput[i].a  = (stdType)(i +2);
    }

    bolt::cl::sort(boltInput.begin(), boltInput.end(),bolt::cl::greater<myfunctor>());
    std::sort(stdInput.begin(), stdInput.end(),bolt::cl::greater<myfunctor>());

    for (i=0; i<length; i++)
    {
        if (stdInput[i].a == boltInput[i].a)
            continue;
        else
            break;
    }
    if (i==length)
        std::cout << "Test Passed" <<std::endl;
    else
        std::cout << "Test Failed i = " << i <<std::endl;
}

template <typename stdType>
void UserDefinedFunctorSortTestOfLength(size_t length)
{
    typedef MyFunctor<stdType> myfunctor;

    std::vector<myfunctor> stdInput(length);
    std::vector<myfunctor> boltInput(length);

    size_t i;
    for (i=0;i<length;i++)
    {
        boltInput[i].a = (stdType)(i +2);
        stdInput[i].a  = (stdType)(i +2);
    }

    bolt::cl::sort(boltInput.begin(), boltInput.end(),myfunctor());
    std::sort(stdInput.begin(), stdInput.end(),myfunctor());

    for (i=0; i<length; i++)
    {
        if (stdInput[i].a == boltInput[i].a)
            continue;
        else
            break;
    }
    if (i==length)
        std::cout << "Test Passed" <<std::endl;
    else
        std::cout << "Test Failed i = " << i <<std::endl;
}

template <typename stdType>
void UserDefinedObjectSortTestOfLength(size_t length)
{
    typedef MyType<stdType> mytype;

    std::vector<mytype> stdInput(length);
    std::vector<mytype> boltInput(length);

    size_t i;
    for (i=0;i<length;i++)
    {
        boltInput[i].a = (stdType)(i +2);
        stdInput[i].a  = (stdType)(i +2);
    }

    bolt::cl::sort(boltInput.begin(), boltInput.end(),bolt::cl::greater<mytype>());
    std::sort(stdInput.begin(), stdInput.end(),bolt::cl::greater<mytype>());
    for (i=0; i<length; i++)
    {
        if(stdInput[i].a == boltInput[i].a)
            continue;
        else
            break;
    }
    if (i==length)
        std::cout << "Test Passed" <<std::endl;
    else
        std::cout << "Test Failed i = " << i <<std::endl;
}

template <typename T>
void BasicSortTestOfLength(size_t length)
{

    std::vector<T> stdInput(length);
    std::vector<T> boltInput(length);
    std::vector<T> stdBackup(length);
    std::generate (stdInput.begin(), stdInput.end(),rand);

    //Ascending Sort
    size_t i;
#if 0
    for (i=0;i<length;i++)
    {
        boltInput[i]= ((T)(stdInput[i]) * 0xAB789F) & ((1<<31) - 1);
        if(i%2)
            boltInput[i] = - boltInput[i];
        stdInput[i] = boltInput[i];
        //printf ("\n%d",stdInput[i]);
    }
    stdBackup = stdInput;
    //printf("\n");

    bolt::cl::sort(boltInput.begin(), boltInput.end()/*, bolt::cl::greater<T>()*/);
    std::sort(stdInput.begin(), stdInput.end()/*, bolt::cl::greater<T>()*/);
    /*for (i=0; i<length; i++)
    {
        std::cout << i << " : " << stdInput[i] << " , " << boltInput[i] << std::endl;
    }*/
    for (i=0; i<length; i++)
    {
        if(stdInput[i] == boltInput[i])
            continue;
        else
            break;
    }
    if (i==length)
    {
        std::cout << "\nTest Passed - Ascending" <<std::endl;
    }
    else
    {
        std::cout << "\nTest Failed  - Ascending i = " << i <<std::endl;
        for (int j=0;j<256;j++)
        {
            if((i+j)<0 || (i+j)>=length)
                std::cout << "Out of Index\n";
            else
                printf("%5x -- %8x -- %8x\n",(i+j),stdInput[i+j],boltInput[i+j]);
        }
    }
#endif

#if 1
    //Descending Sort
    stdInput = stdBackup;
    for (i=0;i<length;i++)
    {
        boltInput[i]= (T)(stdInput[i]);
    }
    //printf("\n");

    bolt::cl::sort(boltInput.begin(), boltInput.end(), bolt::cl::greater<T>());
    std::sort(stdInput.begin(), stdInput.end(), bolt::cl::greater<T>());
    /*for (i=0; i<length; i++)
    {
        std::cout << i << " : " << stdInput[i] << " , " << boltInput[i] << std::endl;
    }*/
    for (i=0; i<length; i++)
    {
        if(stdInput[i] == boltInput[i])
            continue;
        else
            break;
    }
    if (i==length)
    {
        std::cout << "\nTest Passed - Descending" <<std::endl;
    }
    else
    {
        std::cout << "\nTest Failed - Descending i = " << i <<std::endl;
        for (int j=0;j<256;j++)
        {
            if((i+j)<0 || (i+j)>=length)
                std::cout << "Out of Index\n";
            else
                printf("%5x -- %8x -- %8x\n",(i+j),stdInput[i+j],boltInput[i+j]);
        }
    }
#endif

}

template <typename T>
void BasicSortTestOfLengthWithDeviceVector(size_t length)
{
    std::vector<T> stdInput(length);
    bolt::cl::device_vector<T> boltInput(length);

    size_t i;
    for (i=0;i<length;i++)
    {
        boltInput[i]= (T)(length - i +2);
        stdInput[i]= (T)(length - i +2);
    }

    bolt::cl::sort(boltInput.begin(), boltInput.end());
    std::sort(stdInput.begin(), stdInput.end());
    for (i=0; i<length; i++)
    {
        if(stdInput[i] == boltInput[i])
            continue;
        else
            break;
    }
    if (i==length)
        std::cout << "Test Passed" <<std::endl;
    else
        std::cout << "Test Failed i = " << i <<std::endl;
}

template <typename T>
void BasicSortTestWithBoltFunctorOfLengthWithDeviceVector(size_t length)
{
    std::vector<T> stdInput(length);
    bolt::cl::device_vector<T> boltInput(length);

    size_t i;
    for (i=0;i<length;i++)
    {
        boltInput[i]= (T)(i +2);
        stdInput[i]= (T)(i +2);
    }

    bolt::cl::sort(boltInput.begin(), boltInput.end(), bolt::cl::greater<T>());
    std::sort(stdInput.begin(), stdInput.end(), bolt::cl::greater<T>());
    for (i=0; i<length; i++)
    {
        if(stdInput[i] == boltInput[i])
            continue;
        else
            break;
    }
    if (i==length)
        std::cout << "Test Passed" <<std::endl;
    else
        std::cout << "Test Failed i = " << i <<std::endl;
}

template <typename stdType>
void UDDSortTestOfLengthWithDeviceVector(size_t length)
{
    typedef MyType<stdType> mytype;
    std::vector<mytype> stdInput(length);
    bolt::cl::device_vector<mytype> boltInput(length);

    size_t i;
    for (i=0;i<length;i++)
    {
        //FIX ME - This should work
        //boltInput[i].a = (stdType)(length - i +2);
        stdInput[i].a  = (stdType)(length - i +2);
    }

    bolt::cl::sort(boltInput.begin(), boltInput.end());
    std::sort(stdInput.begin(), stdInput.end());
    for (i=0; i<length; i++)
    {
         //FIX ME - stdInput should be changed to boltInput
        if(stdInput[i].a == stdInput[i].a)
            continue;
        else
            break;
    }
    if (i==length)
        std::cout << "Test Passed" <<std::endl;
    else
        std::cout << "Test Failed i = " << i <<std::endl;
}

template <typename stdType>
void UDDSortTestWithBoltFunctorOfLengthWithDeviceVector(size_t length)
{
    typedef MyType<stdType> mytype;
    std::vector<mytype> stdInput(length);
    bolt::cl::device_vector<mytype> boltInput(length);

    size_t i;
    for (i=0;i<length;i++)
    {
        //FIX ME - This should work
        //boltInput[i].a = (stdType)(length - i +2);
        stdInput[i].a  = (stdType)(length - i +2);
    }

    bolt::cl::sort(boltInput.begin(), boltInput.end(),bolt::cl::less<mytype>());
    std::sort(stdInput.begin(), stdInput.end(),bolt::cl::less<mytype>());
    for (i=0; i<length; i++)
    {
         //FIX ME - stdInput should be changed to boltInput
        if(stdInput[i].a == stdInput[i].a)
            continue;
        else
            break;
    }
    if (i==length)
        std::cout << "Test Passed" <<std::endl;
    else
        std::cout << "Test Failed i = " << i <<std::endl;
}
/*
void TestWithBoltControl(int length)
{

    MyOclContext ocl = initOcl(CL_DEVICE_TYPE_CPU, 0);
    bolt::cl::control c(ocl._queue);  // construct control structure from the queue.
    //c.debug(bolt::cl::control::debug::Compile + bolt::cl::control::debug::SaveCompilerTemps);
    typedef MyFunctor<int> myfunctor;
    typedef MyType<int> mytype;

    std::vector<int> boltInput(length);
    std::vector<int> stdInput(length);
    std::vector<myfunctor> myFunctorBoltInput(length);
    std::vector<mytype> myTypeBoltInput(length);
    //bolt::cl::device_vector<int> dvInput(c,length);
    size_t i;

    for (i=0;i<length;i++)
    {
        boltInput[i]= (int)(length - i +2);
        stdInput[i]= (int)(length - i +2);
    }
    for (i=0;i<length;i++)
    {
        myFunctorBoltInput[i].a= (int)(length - i +2);
        myTypeBoltInput[i].a= (int)(length - i +2);
    }

    //BasicSortTestOfLengthWithDeviceVector
    bolt::cl::device_vector<int> dvInput( boltInput.begin(), boltInput.end(), CL_MEM_USE_HOST_PTR | CL_MEM_READ_WRITE, c);
    bolt::cl::sort(c, dvInput.begin(), dvInput.end());

    std::sort(stdInput.begin(),stdInput.end());
    for (i=0;i<length;i++)
    {
        if(dvInput[i] == stdInput[i])
        {
            continue;
        }
        else
            break;
    }
    if(i==length)
        std::cout << "Test Passed\n";
    else
        std::cout << "Test Failed. i = "<< i << std::endl;
    //Device Vector with greater functor
    bolt::cl::sort(c, dvInput.begin(), dvInput.end(),bolt::cl::greater<int>());
    //UserDefinedBoltFunctorSortTestOfLength
    bolt::cl::sort(c, myFunctorBoltInput.begin(), myFunctorBoltInput.end(),bolt::cl::greater<myfunctor>());
    //UserDefinedBoltFunctorSortTestOfLength
    bolt::cl::sort(c, myTypeBoltInput.begin(), myTypeBoltInput.end(),bolt::cl::greater<mytype>());
    return;
}
*/
int main(int argc, char* argv[])
{
#if 1
    //UDDSortTestOfLengthWithDeviceVector<int>(256);
    BasicSortTestOfLength<int>(256/*2097152/*131072/*16777216/*33554432/*atoi(argv[1])*/);
    BasicSortTestOfLength<int>(4096);
    BasicSortTestOfLength<int>(2097152);
    BasicSortTestOfLength<int>(131072);
    BasicSortTestOfLength<int>(512);
    BasicSortTestOfLength<int>(1024);
    BasicSortTestOfLength<int>(2048);
    BasicSortTestOfLength<int>(2560);
#endif
#if 1
    BasicSortTestOfLength<unsigned int>(256/*2097152/*131072/*16777216/*33554432/*atoi(argv[1])*/);
    BasicSortTestOfLength<unsigned int>(4096);
    BasicSortTestOfLength<unsigned int>(2097152);
    BasicSortTestOfLength<unsigned int>(131072);
    BasicSortTestOfLength<unsigned int>(16777472); // 2^24 + 256
    BasicSortTestOfLength<unsigned int>(2048);
    BasicSortTestOfLength<unsigned int>(2560);
#endif
#if 0
    std::vector<int> input(1024);
    std::generate(input.begin(), input.end(), rand);
    bolt::cl::sort( input.begin(), input.end(), bolt::cl::greater<int>());

    int a[10] = {2, 9, 3, 7, 5, 6, 3, 8, 3, 4};
    bolt::cl::sort( a, a+10, bolt::cl::greater<int>());
    //Test the non Power Of 2 Buffer size
    //The following two commented codes does not run. It will throw and cl::Error exception
    //BasicSortTestOfLengthWithDeviceVector<int>(254);
    //BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<int>(254);
    UserDefinedFunctorSortTestOfLength<int>(254);
    UserDefinedBoltFunctorSortTestOfLength<int>(254);
    UserDefinedObjectSortTestOfLength<int>(254);
    BasicSortTestOfLength<int>(254);

#endif
    //The following two are not working because device_vector support is not there.
    //UDDSortTestOfLengthWithDeviceVector<int>(256);
    //UDDSortTestWithBoltFunctorOfLengthWithDeviceVector<int>(256);

#define TEST_ALL 1
//#define TEST_DOUBLE 1

#if (TEST_ALL == 1)
    std::cout << "Testing BasicSortTestWithBoltFunctorOfLengthWithDeviceVector\n";
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<int>(8);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<int>(16);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<int>(32);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<int>(64);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<int>(128);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<int>(256);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<int>(512);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<int>(1024);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<int>(2048);
    //BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<int>(1048576);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<float>(8);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<float>(16);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<float>(32);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<float>(64);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<float>(128);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<float>(256);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<float>(512);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<float>(1024);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<float>(2048);
    //BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<float>(1048576);
#if (TEST_DOUBLE == 1)
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<double>(8);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<double>(16);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<double>(32);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<double>(64);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<double>(128);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<double>(256);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<double>(512);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<double>(1024);
    BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<double>(2048);
    //BasicSortTestWithBoltFunctorOfLengthWithDeviceVector<double>(1048576);
#endif
#endif

#if (TEST_ALL == 1)
//#if 0
    std::cout << "Testing BasicSortTestOfLengthWithDeviceVector\n";
    BasicSortTestOfLengthWithDeviceVector<int>(8);
    BasicSortTestOfLengthWithDeviceVector<int>(16);
    BasicSortTestOfLengthWithDeviceVector<int>(32);
    BasicSortTestOfLengthWithDeviceVector<int>(64);
    BasicSortTestOfLengthWithDeviceVector<int>(128);
    BasicSortTestOfLengthWithDeviceVector<int>(256);
    BasicSortTestOfLengthWithDeviceVector<int>(512);
    BasicSortTestOfLengthWithDeviceVector<int>(1024);
    BasicSortTestOfLengthWithDeviceVector<int>(2048);
    //BasicSortTestOfLengthWithDeviceVector<int>(1048576);
    BasicSortTestOfLengthWithDeviceVector<float>(8);
    BasicSortTestOfLengthWithDeviceVector<float>(16);
    BasicSortTestOfLengthWithDeviceVector<float>(32);
    BasicSortTestOfLengthWithDeviceVector<float>(64);
    BasicSortTestOfLengthWithDeviceVector<float>(128);
    BasicSortTestOfLengthWithDeviceVector<float>(256);
    BasicSortTestOfLengthWithDeviceVector<float>(512);
    BasicSortTestOfLengthWithDeviceVector<float>(1024);
    BasicSortTestOfLengthWithDeviceVector<float>(2048);
    //BasicSortTestOfLengthWithDeviceVector<float>(1048576);
#if (TEST_DOUBLE == 1)
    BasicSortTestOfLengthWithDeviceVector<double>(8);
    BasicSortTestOfLengthWithDeviceVector<double>(16);
    BasicSortTestOfLengthWithDeviceVector<double>(32);
    BasicSortTestOfLengthWithDeviceVector<double>(64);
    BasicSortTestOfLengthWithDeviceVector<double>(128);
    BasicSortTestOfLengthWithDeviceVector<double>(256);
    BasicSortTestOfLengthWithDeviceVector<double>(512);
    BasicSortTestOfLengthWithDeviceVector<double>(1024);
    BasicSortTestOfLengthWithDeviceVector<double>(2048);
    //BasicSortTestOfLengthWithDeviceVector<double>(1048576);
#endif
#endif

#if (TEST_ALL == 1)
    std::cout << "Testing UserDefinedBoltFunctorSortTestOfLength\n";
    UserDefinedBoltFunctorSortTestOfLength<int>(8);
    UserDefinedBoltFunctorSortTestOfLength<int>(16);
    UserDefinedBoltFunctorSortTestOfLength<int>(32);
    UserDefinedBoltFunctorSortTestOfLength<int>(64);
    UserDefinedBoltFunctorSortTestOfLength<int>(128);
    UserDefinedBoltFunctorSortTestOfLength<int>(256);
    UserDefinedBoltFunctorSortTestOfLength<int>(512);
    UserDefinedBoltFunctorSortTestOfLength<int>(1024);
    UserDefinedBoltFunctorSortTestOfLength<int>(2048);
    UserDefinedBoltFunctorSortTestOfLength<int>(1048576);
    UserDefinedBoltFunctorSortTestOfLength<float>(8);
    UserDefinedBoltFunctorSortTestOfLength<float>(16);
    UserDefinedBoltFunctorSortTestOfLength<float>(32);
    UserDefinedBoltFunctorSortTestOfLength<float>(64);
    UserDefinedBoltFunctorSortTestOfLength<float>(128);
    UserDefinedBoltFunctorSortTestOfLength<float>(256);
    UserDefinedBoltFunctorSortTestOfLength<float>(512);
    UserDefinedBoltFunctorSortTestOfLength<float>(1024);
    UserDefinedBoltFunctorSortTestOfLength<float>(2048);
    UserDefinedBoltFunctorSortTestOfLength<float>(1048576);
#if (TEST_DOUBLE == 1)
    UserDefinedBoltFunctorSortTestOfLength<double>(8);
    UserDefinedBoltFunctorSortTestOfLength<double>(16);
    UserDefinedBoltFunctorSortTestOfLength<double>(32);
    UserDefinedBoltFunctorSortTestOfLength<double>(64);
    UserDefinedBoltFunctorSortTestOfLength<double>(128);
    UserDefinedBoltFunctorSortTestOfLength<double>(256);
    UserDefinedBoltFunctorSortTestOfLength<double>(512);
    UserDefinedBoltFunctorSortTestOfLength<double>(1024);
    UserDefinedBoltFunctorSortTestOfLength<double>(2048);
    UserDefinedBoltFunctorSortTestOfLength<double>(1048576);
#endif
#endif

#if (TEST_ALL == 1)
    std::cout << "Testing UserDefinedFunctorSortTestOfLength\n";
    UserDefinedFunctorSortTestOfLength<int>(8);
    UserDefinedFunctorSortTestOfLength<int>(16);
    UserDefinedFunctorSortTestOfLength<int>(32);
    UserDefinedFunctorSortTestOfLength<int>(64);
    UserDefinedFunctorSortTestOfLength<int>(128);
    UserDefinedFunctorSortTestOfLength<int>(256);
    UserDefinedFunctorSortTestOfLength<int>(512);
    UserDefinedFunctorSortTestOfLength<int>(1024);
    UserDefinedFunctorSortTestOfLength<int>(2048);
    UserDefinedFunctorSortTestOfLength<int>(1048576);
    UserDefinedFunctorSortTestOfLength<float>(8);
    UserDefinedFunctorSortTestOfLength<float>(16);
    UserDefinedFunctorSortTestOfLength<float>(32);
    UserDefinedFunctorSortTestOfLength<float>(64);
    UserDefinedFunctorSortTestOfLength<float>(128);
    UserDefinedFunctorSortTestOfLength<float>(256);
    UserDefinedFunctorSortTestOfLength<float>(512);
    UserDefinedFunctorSortTestOfLength<float>(1024);
    UserDefinedFunctorSortTestOfLength<float>(2048);
    UserDefinedFunctorSortTestOfLength<float>(1048576);

#if (TEST_DOUBLE == 1)
    UserDefinedFunctorSortTestOfLength<double>(8);
    UserDefinedFunctorSortTestOfLength<double>(16);
    UserDefinedFunctorSortTestOfLength<double>(32);
    UserDefinedFunctorSortTestOfLength<double>(64);
    UserDefinedFunctorSortTestOfLength<double>(128);
    UserDefinedFunctorSortTestOfLength<double>(256);
    UserDefinedFunctorSortTestOfLength<double>(512);
    UserDefinedFunctorSortTestOfLength<double>(1024);
    UserDefinedFunctorSortTestOfLength<double>(2048);
    UserDefinedFunctorSortTestOfLength<double>(1048576);
#endif
#endif

// This is a test case for handling function pointers
// OpenCL will not handle this so commented it out completely
#if 0
    UserDefinedFunctionSortTestOfLength<int>(256);
    UserDefinedFunctionSortTestOfLength<int>(512);
    UserDefinedFunctionSortTestOfLength<int>(1024);
    UserDefinedFunctionSortTestOfLength<int>(2048);
    UserDefinedFunctionSortTestOfLength<int>(1048576);
    UserDefinedFunctionSortTestOfLength<float>(256);
    UserDefinedFunctionSortTestOfLength<float>(512);
    UserDefinedFunctionSortTestOfLength<float>(1024);
    UserDefinedFunctionSortTestOfLength<float>(2048);
    UserDefinedFunctionSortTestOfLength<float>(1048576);
#if (TEST_DOUBLE == 1)
    UserDefinedFunctionSortTestOfLength<double>(256);
    UserDefinedFunctionSortTestOfLength<double>(512);
    UserDefinedFunctionSortTestOfLength<double>(1024);
    UserDefinedFunctionSortTestOfLength<double>(2048);
    UserDefinedFunctionSortTestOfLength<double>(1048576);
#endif
#endif

#if (TEST_ALL == 1)
    std::cout << "Testing UserDefinedObjectSortTestOfLength\n";
    UserDefinedObjectSortTestOfLength<int>(8);
    UserDefinedObjectSortTestOfLength<int>(16);
    UserDefinedObjectSortTestOfLength<int>(32);
    UserDefinedObjectSortTestOfLength<int>(64);
    UserDefinedObjectSortTestOfLength<int>(128);
    UserDefinedObjectSortTestOfLength<int>(256);
    UserDefinedObjectSortTestOfLength<int>(512);
    UserDefinedObjectSortTestOfLength<int>(1024);
    UserDefinedObjectSortTestOfLength<int>(2048);
    UserDefinedObjectSortTestOfLength<int>(1048576);
    UserDefinedObjectSortTestOfLength<float>(8);
    UserDefinedObjectSortTestOfLength<float>(16);
    UserDefinedObjectSortTestOfLength<float>(32);
    UserDefinedObjectSortTestOfLength<float>(64);
    UserDefinedObjectSortTestOfLength<float>(128);
    UserDefinedObjectSortTestOfLength<float>(256);
    UserDefinedObjectSortTestOfLength<float>(512);
    UserDefinedObjectSortTestOfLength<float>(1024);
    UserDefinedObjectSortTestOfLength<float>(2048);
    UserDefinedObjectSortTestOfLength<float>(1048576);
#if (TEST_DOUBLE == 1)
    UserDefinedObjectSortTestOfLength<double>(8);
    UserDefinedObjectSortTestOfLength<double>(16);
    UserDefinedObjectSortTestOfLength<double>(32);
    UserDefinedObjectSortTestOfLength<double>(64);
    UserDefinedObjectSortTestOfLength<double>(128);
    UserDefinedObjectSortTestOfLength<double>(256);
    UserDefinedObjectSortTestOfLength<double>(512);
    UserDefinedObjectSortTestOfLength<double>(1024);
    UserDefinedObjectSortTestOfLength<double>(2048);
    UserDefinedObjectSortTestOfLength<double>(1048576);
#endif
#endif

#if (TEST_ALL == 1)
    std::cout << "Testing BasicSortTestOfLength\n";
    BasicSortTestOfLength<int>(256);
    BasicSortTestOfLength<int>(512);
    BasicSortTestOfLength<int>(1024);
    BasicSortTestOfLength<int>(2048);
    BasicSortTestOfLength<int>(1048576);
    //BasicSortTestOfLength<float>(256);
    //BasicSortTestOfLength<float>(512);
    //BasicSortTestOfLength<float>(1024);
    //BasicSortTestOfLength<float>(2048);
    //BasicSortTestOfLength<float>(1048576);

#if (TEST_DOUBLE == 1)
    BasicSortTestOfLength<double>(256);
    BasicSortTestOfLength<double>(512);
    BasicSortTestOfLength<double>(1024);
    BasicSortTestOfLength<double>(2048);
    BasicSortTestOfLength<double>(1048576);
#endif
#endif

    std::cout << "Test Completed" << std::endl;
    return 0;
    getchar();
    return 0;
}
#endif


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

#include "ssmkit/map/linear_gaussian.hpp"
#include "ssmkit/distribution/gaussian.hpp"
#include "ssmkit/distribution/conditional.hpp"
#include "ssmkit/process/markov.hpp"
#include "ssmkit/process/memoryless.hpp"

#include "ssmkit/process/hierarchical.hpp"

using namespace PROJECT_NAME;

BOOST_AUTO_TEST_SUITE(markov_test);

BOOST_AUTO_TEST_CASE(test1) {
  arma::arma_rng::set_seed_random();

  map::LinearGaussian f{{{1, 1}, {0, 1}}, {{0.0001, 0}, {0, 0.01}}};
  distribution::Gaussian nu(2);

  map::LinearGaussian h{{1, 0}, {0.0001}};
  distribution::Gaussian omega(1);

  distribution::Conditional<decltype(nu), decltype(f)> dyn(nu, f);
  distribution::Conditional<decltype(omega), decltype(h)> obs(omega, h);

  process::Markov<decltype(nu), decltype(f), decltype(nu)> markov(dyn, nu);
  process::Memoryless<decltype(omega), decltype(h)> memless(obs);

  auto r = markov.initialize();
  auto r2 = memless.random(r);
  // double l = markov.likelihood(r);

  process::Hierarchical<decltype(markov), decltype(memless)> tt(markov, memless);

  //std::cout << process::ProcessTraits<decltype(markov)>::TArity::value
  //          << std::endl;

  tt.initialize();
  auto r3 = tt.random(1, 'c');
  auto ll = tt.likelihood(r3);

  //std::cout << std::get<0>(r3) << std::get<1>(r3) << std::endl;

   // std::vector<decltype(markov.random())> v;

    auto v = markov.random_n(10);

   // std::for_each(v.begin(), v.end(), [](decltype(markov.random()) &p) {
   //   std::cout << p << std::endl;
   // });
}

BOOST_AUTO_TEST_SUITE_END();


#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <gridmap/operations/rasterize.h>
#include <navigation_interface/params.h>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <pluginlib/class_list_macros.h>
#include <pure_pursuit_controller/plugin.h>
#include <visualization_msgs/MarkerArray.h>

PLUGINLIB_EXPORT_CLASS(pure_pursuit_controller::PurePursuitController, navigation_interface::Controller)

namespace pure_pursuit_controller
{

namespace
{

typedef boost::geometry::model::d2::point_xy<double> Point;  // Point type for boost polygon library

typedef boost::geometry::model::polygon<Point,           // Point type
                                        true,            // Orientation is clockwise
                                        false,           // Closure is implied (last point != first point)
                                        std::vector,     // Store for points (efficient if all points known in advance)
                                        std::vector,     // Store for rings (efficient if all holes known in advance)
                                        std::allocator,  // Point allocator
                                        std::allocator   // Ring allocator
                                        >
    Polygon;

struct CollisionCheck
{
    bool in_collision;
    double min_distance_to_collision;
    visualization_msgs::Marker marker;
};

CollisionCheck robotInCollision(const gridmap::OccupancyGrid& grid, const Eigen::Isometry2d& robot_pose,
                                const Eigen::Isometry2d& future_pose, const std::vector<Eigen::Vector2d>& footprint,
                                const float alpha)
{
    // Want to interpolate footprint between current robot pose and future robot pose
    const double linear_step = 0.01;
    const double angular_step = 0.04;
    const size_t max_steps = 20;
    const size_t min_steps = 2;

    // First create interpolated poses
    std::vector<Eigen::Isometry2d> interpolated;
    interpolated.push_back(robot_pose);

    const Eigen::Vector2d linear_dir = future_pose.translation() - robot_pose.translation();
    const double linear_dist = linear_dir.norm();
    const Eigen::Rotation2Dd rot(robot_pose.linear());
    const Eigen::Rotation2Dd future_rot(future_pose.linear());
    const Eigen::Rotation2Dd rot_dir(future_rot.inverse() * rot);
    const double rot_dist = std::abs(rot_dir.smallestAngle());

    const std::size_t steps =
        std::min(max_steps, std::max(min_steps, static_cast<std::size_t>(
                                                    std::max((linear_dist / linear_step), (rot_dist / angular_step)))));

    for (std::size_t j = 1; j < steps; j++)
    {
        const double fraction = static_cast<double>(j + 1) / static_cast<double>(steps);
        const Eigen::Isometry2d pose =
            Eigen::Translation2d(robot_pose.translation() + fraction * linear_dir) * rot.slerp(fraction, future_rot);
        interpolated.push_back(pose);
    }

    // Create vector of polygons. Polygon at each pose starting from robot pose
    std::vector<Polygon> polygons;
    for (const Eigen::Isometry2d& pose : interpolated)
    {
        Polygon polygon;
        for (const Eigen::Vector2d& p : footprint)
        {
            const Eigen::Vector2d world_point = pose.translation() + pose.rotation() * p;
            Point point(world_point[0], world_point[1]);
            boost::geometry::append(polygon.outer(), point);
        }
        polygons.push_back(polygon);
    }

    // Union the footprints
    Polygon union_output = polygons.front();
    for (std::size_t i = 1; i < polygons.size(); i++)
    {
        std::vector<Polygon> temp;
        boost::geometry::union_(union_output, polygons[i], temp);
        ROS_ASSERT_MSG(temp.size() == 1, "Footprint union failed. More than 1 resulting polygons");
        union_output = temp[0];
    }

    int min_x = std::numeric_limits<int>::max();
    int max_x = 0;

    int min_y = std::numeric_limits<int>::max();
    int max_y = 0;

    std::vector<Eigen::Array2i> map_footprint;
    for (const Point& p : union_output.outer())
    {
        const auto map_point = grid.dimensions().getCellIndex(Eigen::Vector2d(p.x(), p.y()));
        map_footprint.push_back(map_point);
        min_x = std::min(map_point.x(), min_x);
        max_x = std::max(map_point.x(), max_x);
        min_y = std::min(map_point.y(), min_y);
        max_y = std::max(map_point.y(), max_y);
    }
    map_footprint.push_back(map_footprint.front());

    const auto connected_poly = gridmap::connectPolygon(map_footprint);

    double min_distance_to_collision = std::numeric_limits<double>::max();
    {
        const cv::Mat cv_im = cv::Mat(grid.dimensions().size().y(), grid.dimensions().size().x(), CV_8U,
                                      reinterpret_cast<void*>(const_cast<uint8_t*>(grid.cells().data())));

        // Invert the costmap data such that all objects are considered zeros
        cv::Mat inv_cv_im;
        cv::bitwise_not(cv_im, inv_cv_im);

        // Calculate the distance transform to all objects (zero pixels)
        cv::Mat distance;
        cv::distanceTransform(inv_cv_im, distance, cv::DIST_L2, cv::DIST_MASK_PRECISE, CV_32F);

        for (const auto& p : connected_poly)
        {
            const double f = static_cast<double>(distance.at<float>(p.y(), p.x()));
            min_distance_to_collision = std::min(min_distance_to_collision, f);
        }
        min_distance_to_collision *= grid.dimensions().resolution();
    }

    visualization_msgs::Marker marker;
    marker.ns = "points";
    marker.id = 0;
    marker.type = visualization_msgs::Marker::POINTS;
    marker.header.stamp = ros::Time::now();
    marker.header.frame_id = "map";
    marker.frame_locked = true;
    marker.scale.x = 0.02;
    marker.scale.y = 0.02;
    marker.scale.z = 0.00;
    marker.color.a = 1.f;
    marker.pose.orientation.w = 1.0;

    bool in_collision = false;
    auto append_raster = [&grid, &marker, &in_collision, alpha](const int x, const int y) {
        const Eigen::Array2i p{x, y};

        const Eigen::Vector2d w = grid.dimensions().getCellCenter(p);
        geometry_msgs::Point mp;
        mp.x = w.x();
        mp.y = w.y();
        mp.z = 0.0;
        marker.points.push_back(mp);
        std_msgs::ColorRGBA c;
        c.a = alpha;
        if (grid.dimensions().contains(p) && grid.occupied(p))
        {
            in_collision = true;
            c.r = 1.0;
        }
        else
        {
            c.g = 1.0;
        }
        marker.colors.push_back(c);
    };

    gridmap::rasterPolygonFill(append_raster, connected_poly, min_x, max_x, min_y, max_y);

    // rasterPolygonFill is not properly including all edges
    for (const auto& p : connected_poly)
        append_raster(p.x(), p.y());

    return {in_collision, min_distance_to_collision, marker};
}

visualization_msgs::Marker buildMarker(const navigation_interface::KinodynamicState& robot_state,
                                       const navigation_interface::KinodynamicState& target_state)
{
    visualization_msgs::Marker marker;
    marker.ns = "target";
    marker.id = 0;
    marker.type = visualization_msgs::Marker::ARROW;
    marker.header.stamp = ros::Time::now();
    marker.header.frame_id = "odom";
    marker.frame_locked = true;
    marker.scale.x = 0.02;
    marker.scale.y = 0.04;
    marker.scale.z = 0.04;
    marker.color.a = 1.f;
    marker.pose.orientation.w = 1.0;

    {
        geometry_msgs::Point mp;
        mp.x = robot_state.pose.translation().x();
        mp.y = robot_state.pose.translation().y();
        mp.z = 0.0;
        marker.points.push_back(mp);
    }

    {
        geometry_msgs::Point mp;
        mp.x = target_state.pose.translation().x();
        mp.y = target_state.pose.translation().y();
        mp.z = 0.0;
        marker.points.push_back(mp);
    }

    return marker;
}

// We can treat rotation like a Z axis, so we can calculate a 3D norm between any two poses
double dist(const Eigen::Isometry2d& pose_1, const Eigen::Isometry2d& pose_2, const double a_weight)
{
    const double diff_x = pose_2.translation().x() - pose_1.translation().x();
    const double diff_y = pose_2.translation().y() - pose_1.translation().y();
    const double diff_a = Eigen::Rotation2Dd(pose_1.linear().inverse() * pose_2.linear()).smallestAngle();

    return (diff_x * diff_x) + (diff_y * diff_y) + a_weight * (diff_a * diff_a);
}

// Find the node on the path that is closest to the robot.
// If the robot is between two nodes, it will always pick the one in front to prevent back-tracking
// Because the nodes aren't evenly spaced, this is actually not a trivial problem
std::size_t findClosest(const std::vector<navigation_interface::KinodynamicState>& nodes, const Eigen::Isometry2d& pose)
{
    if (nodes.size() <= 2)
        return nodes.size() - 1;

    std::vector<double> distances;
    std::transform(
        nodes.begin(), nodes.end(), std::back_inserter(distances),
        [&pose](const navigation_interface::KinodynamicState& state) { return dist(state.pose, pose, 0.1); });
    const auto closest_it = std::min_element(distances.begin(), distances.end());
    const std::size_t closest_i = std::distance(distances.begin(), closest_it);

    if (closest_i == 0 || closest_i == nodes.size() - 1)
    {
        return static_cast<std::size_t>(closest_i);
    }

    // Resolve ambiguity by checking points slightly before and after the closest node
    // Find midpoints  -----A------M1---closest_i---M2-------B------->
    Eigen::Isometry2d m_1;
    Eigen::Isometry2d m_2;
    {
        // Calculate pose of M1
        const Eigen::Vector2d translation_vec =
            nodes[closest_i].pose.translation() - nodes[closest_i - 1].pose.translation();
        const Eigen::Rotation2Dd rot(nodes[closest_i - 1].pose.linear());
        const Eigen::Rotation2Dd target_rot(nodes[closest_i].pose.linear());

        m_1 = Eigen::Translation2d(nodes[closest_i - 1].pose.translation() + 0.99 * translation_vec) *
              rot.slerp(0.99, target_rot);
    }

    {
        // Calculate pose of M2
        const Eigen::Vector2d translation_vec =
            nodes[closest_i + 1].pose.translation() - nodes[closest_i].pose.translation();
        const Eigen::Rotation2Dd rot(nodes[closest_i].pose.linear());
        const Eigen::Rotation2Dd target_rot(nodes[closest_i + 1].pose.linear());

        m_2 = Eigen::Translation2d(nodes[closest_i].pose.translation() + 0.01 * translation_vec) *
              rot.slerp(0.01, target_rot);
    }

    if (dist(m_1, pose, 0.1) < dist(m_2, pose, 0.1))
    {
        return static_cast<std::size_t>(closest_i);
    }
    else
    {
        return static_cast<std::size_t>(closest_i + 1);
    }
}

// Performs a lookahead to give the robot a "carrot" to chase
navigation_interface::KinodynamicState lookAhead(const std::vector<navigation_interface::KinodynamicState>& nodes,
                                                 const Eigen::Isometry2d& robot_pose, const Eigen::Vector3d& velocity,
                                                 const std::size_t path_index, const double look_ahead_time,
                                                 const unsigned int interpolation_steps)
{
    Eigen::Isometry2d future_pose = robot_pose;
    future_pose.pretranslate(look_ahead_time * Eigen::Vector2d(velocity.topRows(2)));
    future_pose.rotate(Eigen::Rotation2Dd(look_ahead_time * velocity[2]));

    // To prevent the robot from getting stuck with 0 lookahead, set a minimum of 5cm
    // Note: these are squared distances!
    const double required_distance = std::max(0.0025, dist(robot_pose, future_pose, 0.1));

    for (std::size_t i = path_index; i < nodes.size(); ++i)
    {
        const double dist_to_node = dist(robot_pose, nodes[i].pose, 0.1);

        // Found first node that is far enough. Now interpolate between previous node and this node.
        if (dist_to_node > required_distance)
        {
            const Eigen::Isometry2d start_pose = (i == 0) ? robot_pose : nodes[i - 1].pose;
            const Eigen::Vector2d translation_vec = nodes[i].pose.translation() - start_pose.translation();
            const Eigen::Rotation2Dd rot(start_pose.linear());
            const Eigen::Rotation2Dd target_rot(nodes[i].pose.linear());

            Eigen::Isometry2d prev_interpolated_pose = nodes[i].pose;

            for (int j = interpolation_steps; j >= 0; j--)
            {
                // Step from front to back to avoid finding a point behind the robot.
                const double fraction = static_cast<double>(j) / static_cast<double>(interpolation_steps);
                const Eigen::Isometry2d interpolated_pose =
                    Eigen::Translation2d(start_pose.translation() + fraction * translation_vec) *
                    rot.slerp(fraction, target_rot);

                // Found pose just within range. Return the previous one (just outside)
                if (dist(interpolated_pose, robot_pose, 0.1) < required_distance)
                {
                    navigation_interface::KinodynamicState target_state = nodes[i];
                    target_state.pose = prev_interpolated_pose;
                    return target_state;
                }

                prev_interpolated_pose = interpolated_pose;
            }

            // None of the interpolated poses are within required_distance. Just use the node itself.
            return nodes[i];
        }
    }
    return nodes.back();
}

}  // namespace

PurePursuitController::PurePursuitController()
{
}

PurePursuitController::~PurePursuitController()
{
}

// cppcheck-suppress unusedFunction
bool PurePursuitController::setTrajectory(const navigation_interface::Trajectory& trajectory)
{
    if (trajectory.states.empty())
        return false;

    path_index_ = 1;  // first pose is under robot
    trajectory_.reset(new navigation_interface::Trajectory(trajectory));
    control_integral_ = Eigen::Vector3d::Zero();
    control_error_ = Eigen::Vector3d::Zero();
    last_update_ = ros::SteadyTime::now();

    return true;
}

// cppcheck-suppress unusedFunction
void PurePursuitController::clearTrajectory()
{
    trajectory_.reset();
    path_index_ = 0;
    control_integral_ = Eigen::Vector3d::Zero();
    control_error_ = Eigen::Vector3d::Zero();
    last_update_ = ros::SteadyTime::now();
}

// cppcheck-suppress unusedFunction
boost::optional<std::string> PurePursuitController::trajectoryId() const
{
    if (trajectory_)
        return boost::optional<std::string>(trajectory_->id);
    else
        return {};
}

boost::optional<navigation_interface::Trajectory> PurePursuitController::trajectory() const
{
    if (trajectory_)
        return boost::optional<navigation_interface::Trajectory>(*trajectory_);
    else
        return {};
}

navigation_interface::Controller::Result
    // cppcheck-suppress unusedFunction
    PurePursuitController::control(const ros::SteadyTime& time, const gridmap::AABB& local_region,
                                   const navigation_interface::KinodynamicState& robot_state,
                                   const Eigen::Isometry2d& map_to_odom)
{
    navigation_interface::Controller::Result result;
    result.command = Eigen::Vector3d::Zero();

    if (!trajectory_)
    {
        result.outcome = navigation_interface::Controller::Outcome::FAILED;
        return result;
    }

    double dt = time.toSec() - last_update_.toSec();
    ROS_ASSERT_MSG(dt > 0.0, "Pure Pursuit - Negative time step!");
    last_update_ = time;

    const double dist_to_goal = (trajectory_->states.back().pose.translation() - robot_state.pose.translation()).norm();
    const double angle_to_goal =
        std::abs(Eigen::Rotation2Dd(robot_state.pose.linear().inverse() * trajectory_->states.back().pose.linear())
                     .smallestAngle());

    //
    // Goal condition
    //
    if (angle_to_goal < yaw_goal_tolerance_ && dist_to_goal < xy_goal_tolerance_)
    {
        ROS_INFO_STREAM("Control Complete! angle_to_goal: " << angle_to_goal << " dist_to_goal: " << dist_to_goal);
        result.outcome = navigation_interface::Controller::Outcome::COMPLETE;
        last_update_ = ros::SteadyTime::now();
        return result;
    }

    //
    // Find target
    //
    // path_index is the index closest to where the robot is currently
    // target_state is the pose the robot is aiming for
    navigation_interface::KinodynamicState target_state;
    path_index_ = findClosest(trajectory_->states, robot_state.pose);

    const double dist_to_closest = dist(robot_state.pose, trajectory_->states[path_index_].pose, 0.1);
    if (dist_to_closest > tracking_error_)
    {
        ROS_WARN_STREAM("Tracking error. Distance to trajectory: " << dist_to_closest << " > " << tracking_error_);
        result.outcome = navigation_interface::Controller::Outcome::FAILED;
        last_update_ = ros::SteadyTime::now();
        return result;
    }

    target_state = lookAhead(trajectory_->states, robot_state.pose, robot_state.velocity, path_index_, look_ahead_time_,
                             interpolation_steps_);

    //
    // Check immediate collisions
    //
    double min_distance_to_collision;
    {
        // TODO need a more intelligent way to lock read access without waiting for path planning
        // auto lock = map_data_->grid.getLock();
        gridmap::OccupancyGrid local_grid(map_data_->grid, local_region);
        // lock.unlock();

        const Eigen::Isometry2d map_robot_pose = map_to_odom * robot_state.pose;
        const Eigen::Isometry2d map_goal_pose = map_to_odom * target_state.pose;

        const CollisionCheck cc = robotInCollision(local_grid, map_robot_pose, map_goal_pose, robot_footprint_, 1.f);
        min_distance_to_collision = cc.min_distance_to_collision;
        if (debug_viz_)
            footprint_pub_.publish(cc.marker);

        if (cc.in_collision)
        {
            ROS_WARN_STREAM("Robot is in collision!");
            result.outcome = navigation_interface::Controller::Outcome::FAILED;
            last_update_ = ros::SteadyTime::now();
            return result;
        }
    }

    if (debug_viz_)
        target_state_pub_.publish(buildMarker(robot_state, target_state));

    ROS_ASSERT(robot_state.pose.linear().allFinite());
    ROS_ASSERT(robot_state.pose.translation().allFinite());

    //
    // PD control (PID when close to goal)
    //
    const Eigen::Vector2d target_vec_wrt_robot =
        robot_state.pose.linear().inverse() * (target_state.pose.translation() - robot_state.pose.translation());
    const Eigen::Vector3d control_error = {
        target_vec_wrt_robot[0], target_vec_wrt_robot[1],
        Eigen::Rotation2Dd(robot_state.pose.linear().inverse() * target_state.pose.linear()).smallestAngle()};
    ROS_ASSERT(control_error.allFinite());

    Eigen::Vector3d target_velocity;

    const Eigen::Vector3d control_dot_ = (control_error - control_error_) / dt;
    control_error_ = control_error;
    ROS_ASSERT(control_error_.allFinite());

    const bool final_pid_control = path_index_ == trajectory_->states.size() - 1;
    if (final_pid_control)
    {
        control_integral_ += control_error;

        // Integrator wind-up prevention clamp
        for (unsigned int i = 0; i < 3; ++i)
        {
            control_integral_[i] =
                std::min(std::max(-control_integral_max_[i], control_integral_[i]), control_integral_max_[i]);
        }

        target_velocity = p_gain_.cwiseProduct(control_error) + i_gain_.cwiseProduct(control_integral_) +
                          d_gain_.cwiseProduct(control_dot_);
    }
    else
    {
        target_velocity = p_gain_.cwiseProduct(control_error) + d_gain_.cwiseProduct(control_dot_);
    }

    ROS_ASSERT_MSG(target_velocity.allFinite(), "%f %f %f", target_velocity[0], target_velocity[1], target_velocity[2]);

    //
    // Max acceleration check
    //
    Eigen::Vector3d vel_command = target_velocity;

    const Eigen::Vector2d translation_dv(target_velocity[0] - prev_cmd_vel[0], target_velocity[1] - prev_cmd_vel[1]);
    const double rotation_dv = target_velocity[2] - prev_cmd_vel[2];

    // We limit the X and Y accelerations together to preserve the direction
    const double translation_accel = translation_dv.norm() / dt;
    if (translation_accel > max_translation_accel_)
    {
        // ROS_INFO_STREAM("Excessive translation acceleration: " << translation_accel << " > " <<
        // max_translation_accel_);
        vel_command[0] = prev_cmd_vel[0] + translation_dv[0] * (max_translation_accel_ / translation_accel);
        vel_command[1] = prev_cmd_vel[1] + translation_dv[1] * (max_translation_accel_ / translation_accel);
    }

    const double rotation_accel = std::abs(rotation_dv) / dt;
    if (rotation_accel > max_rotation_accel_)
    {
        // ROS_INFO_STREAM("Excessive rotation acceleration: " << rotation_accel << " > " << max_rotation_accel_);
        vel_command[2] = prev_cmd_vel[2] + rotation_dv * (max_rotation_accel_ / rotation_accel);
    }

    ROS_ASSERT(vel_command.allFinite());

    prev_cmd_vel = vel_command;

    //
    // Max velocity check
    //

    // Limit max velocity based on distance to nearest obstacle
    const double d = std::max(min_avoid_distance_, std::min(max_avoid_distance_, min_distance_to_collision));
    const double velocity_scale = d / max_avoid_distance_;

    Eigen::Vector3d augmented_max_vel = {std::min(max_velocity_[0], max_velocity_[0] * velocity_scale),
                                         std::min(max_velocity_[1], max_velocity_[1] * velocity_scale),
                                         std::min(max_velocity_[2], max_velocity_[2] * velocity_scale)};

    {
        double vel_factor = 1.0;
        for (long i = 0; i < 3; ++i)
        {
            if (std::abs(vel_command[i]) > augmented_max_vel[i])
            {
                vel_factor = std::max(vel_factor, std::abs(vel_command[i] / augmented_max_vel[i]));
            }
        }

        vel_command /= vel_factor;
        ROS_ASSERT(vel_command.allFinite());
    }

    result.command = vel_command;
    result.outcome = navigation_interface::Controller::Outcome::SUCCESSFUL;

    return result;
}

// cppcheck-suppress unusedFunction
void PurePursuitController::onInitialize(const YAML::Node& parameters)
{
    look_ahead_time_ = parameters["look_ahead_time"].as<double>(look_ahead_time_);

    max_velocity_[0] = parameters["max_velocity_x"].as<double>(max_velocity_[0]);
    max_velocity_[1] = parameters["max_velocity_y"].as<double>(max_velocity_[1]);
    max_velocity_[2] = parameters["max_velocity_w"].as<double>(max_velocity_[2]);

    max_translation_accel_ = parameters["max_translation_accel"].as<double>(max_translation_accel_);
    max_rotation_accel_ = parameters["max_rotation_accel"].as<double>(max_rotation_accel_);

    xy_goal_tolerance_ = parameters["xy_goal_tolerance"].as<double>(xy_goal_tolerance_);
    yaw_goal_tolerance_ = parameters["yaw_goal_tolerance"].as<double>(yaw_goal_tolerance_);

    p_gain_[0] = parameters["p_gain_x"].as<double>(p_gain_[0]);
    p_gain_[1] = parameters["p_gain_y"].as<double>(p_gain_[1]);
    p_gain_[2] = parameters["p_gain_w"].as<double>(p_gain_[2]);

    i_gain_[0] = parameters["i_gain_x"].as<double>(i_gain_[0]);
    i_gain_[1] = parameters["i_gain_y"].as<double>(i_gain_[1]);
    i_gain_[2] = parameters["i_gain_w"].as<double>(i_gain_[2]);

    d_gain_[0] = parameters["d_gain_x"].as<double>(d_gain_[0]);
    d_gain_[1] = parameters["d_gain_y"].as<double>(d_gain_[1]);
    d_gain_[2] = parameters["d_gain_w"].as<double>(d_gain_[2]);

    control_integral_max_[0] = parameters["control_integral_max_x"].as<double>(control_integral_max_[0]);
    control_integral_max_[1] = parameters["control_integral_max_y"].as<double>(control_integral_max_[1]);
    control_integral_max_[2] = parameters["control_integral_max_w"].as<double>(control_integral_max_[2]);

    max_avoid_distance_ = parameters["max_avoid_distance"].as<double>(max_avoid_distance_);
    min_avoid_distance_ = parameters["min_avoid_distance"].as<double>(min_avoid_distance_);

    robot_footprint_ = navigation_interface::get_point_list(
        parameters, "footprint",
        {{0.480, 0.000}, {0.380, -0.395}, {-0.380, -0.395}, {-0.480, 0.000}, {-0.380, 0.395}, {0.380, 0.395}});

    debug_viz_ = parameters["debug_viz"].as<bool>(debug_viz_);
    if (debug_viz_)
    {
        ros::NodeHandle nh("~");
        target_state_pub_ = nh.advertise<visualization_msgs::Marker>("target_state", 100);
        footprint_pub_ = nh.advertise<visualization_msgs::Marker>("footprint", 100);
    }
}

// cppcheck-suppress unusedFunction
void PurePursuitController::onMapDataChanged()
{
}
}  // namespace pure_pursuit_controller


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

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

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

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

/*! \file orea/aggregation/dimcalculator.hpp
    \brief Dynamic Initial Margin calculator base class
    \ingroup analytics
*/

#pragma once

#include <orea/aggregation/collatexposurehelper.hpp>
#include <orea/cube/cubeinterpretation.hpp>
#include <orea/cube/inmemorycube.hpp>
#include <orea/scenario/aggregationscenariodata.hpp>

#include <ored/portfolio/nettingsetmanager.hpp>
#include <ored/portfolio/portfolio.hpp>
#include <ored/report/report.hpp>

#include <ql/time/date.hpp>

#include <boost/shared_ptr.hpp>

namespace ore {
namespace analytics {
using namespace QuantLib;
using namespace QuantExt;
using namespace data;
using namespace std;

//! CVA Spread Sensitivity Calculator
/*!
  Compute hazard rate and CDS spread sensivities for a given exposure profile
  on an externally provided sensitivity grid.
*/
class CVASpreadSensitivityCalculator {
public:
    CVASpreadSensitivityCalculator(//! For logging purposes to distinguish sensi runs, e.g. for different netting sets
				   const std::string& key,
				   //! Asof date
				   const Date& asof,
				   //! Netting set EPE profile
				   const vector<Real>& epe,
				   //! Date grid
				   const vector<Date>& dates,
				   //! Default term structure for the netting set
				   const Handle<DefaultProbabilityTermStructure>& dts,
				   //! Market recovery rate
				   const Real& recovery,
				   //! CDS Discount curve
				   const Handle<YieldTermStructure>& yts,
				   //! Shift grid
				   const vector<Period>& shiftTenors,
				   //! Shift size
				   Real shiftSize = 0.0001);

    //! Inspectors
    // @{
    const string key() { return key_; }
    Date asof() { return asof_; }
    const vector<Real>& exposureProfile() { return epe_; }
    const vector<Date>& exposureDateGrid() { return dates_; }
    const Handle<DefaultProbabilityTermStructure>& defaultTermStructure() { return dts_; }
    Real recoveryRate() { return recovery_; }
    const Handle<YieldTermStructure>& discountCurve() { return yts_; }
    const vector<Period> shiftTenors() { return shiftTenors_; }
    // @}

    //! Results
    // @{
    const vector<Real> shiftTimes() { return shiftTimes_; }
    Real shiftSize() { return shiftSize_; }
    const vector<Real> hazardRateSensitivities() { return hazardRateSensitivities_; }
    const vector<Real> cdsSpreadSensitivities() { return cdsSpreadSensitivities_; }
    const Matrix& jacobi() { return jacobi_; }
    // @}

private:
    // survival probability with shifted hazard rates in the specified bucket
    Real survivalProbability(Date d, bool shift, Size index);
    Real survivalProbability(Time t, bool shift, Size index);
    // CVA calculation with and without shifted hazard rates
    Real cva(bool shift = false, Size index = 0);
    //! Fair CDS Sprewad calculation with and without shifted hazard rates
    Real fairCdsSpread(Size term, bool shift = false, Size index = 0);

    string key_;
    Date asof_;
    vector<Real> epe_;
    vector<Date> dates_;
    Handle<DefaultProbabilityTermStructure> dts_;
    Real recovery_;
    Handle<YieldTermStructure> yts_;
    vector<Period> shiftTenors_;

    vector<Real> shiftTimes_;
    Real shiftSize_;
    vector<Real> hazardRateSensitivities_;
    vector<Real> cdsSpreadSensitivities_;
    Matrix jacobi_;
};

} // namespace analytics
} // namespace ore