#ifndef STAN_ANALYZE_MCMC_COMPUTE_POTENTIAL_SCALE_REDUCTION_HPP
#define STAN_ANALYZE_MCMC_COMPUTE_POTENTIAL_SCALE_REDUCTION_HPP

#include <stan/math/prim.hpp>
#include <stan/analyze/mcmc/autocovariance.hpp>
#include <stan/analyze/mcmc/split_chains.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <algorithm>
#include <cmath>
#include <vector>
#include <limits>

namespace stan {
namespace analyze {

/**
 * Computes the potential scale reduction (Rhat) for the specified
 * parameter across all kept samples.
 *
 * See more details in Stan reference manual section "Potential
 * Scale Reduction". http://mc-stan.org/users/documentation
 *
 * Current implementation assumes draws are stored in contiguous
 * blocks of memory.  Chains are trimmed from the back to match the
 * length of the shortest chain.
 *
 * @param draws stores pointers to arrays of chains
 * @param sizes stores sizes of chains
 * @return potential scale reduction for the specified parameter
 */
inline double compute_potential_scale_reduction(
    std::vector<const double*> draws, std::vector<size_t> sizes) {
  int num_chains = sizes.size();
  size_t num_draws = sizes[0];
  for (int chain = 1; chain < num_chains; ++chain) {
    num_draws = std::min(num_draws, sizes[chain]);
  }

  // check if chains are constant; all equal to first draw's value
  bool are_all_const = false;
  Eigen::VectorXd init_draw = Eigen::VectorXd::Zero(num_chains);

  for (int chain = 0; chain < num_chains; chain++) {
    Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, 1>> draw(
        draws[chain], sizes[chain]);

    for (int n = 0; n < num_draws; n++) {
      if (!boost::math::isfinite(draw(n))) {
        return std::numeric_limits<double>::quiet_NaN();
      }
    }

    init_draw(chain) = draw(0);

    if (draw.isApproxToConstant(draw(0))) {
      are_all_const |= true;
    }
  }

  if (are_all_const) {
    // If all chains are constant then return NaN
    // if they all equal the same constant value
    if (init_draw.isApproxToConstant(init_draw(0))) {
      return std::numeric_limits<double>::quiet_NaN();
    }
  }

  using boost::accumulators::accumulator_set;
  using boost::accumulators::stats;
  using boost::accumulators::tag::mean;
  using boost::accumulators::tag::variance;

  Eigen::VectorXd chain_mean(num_chains);
  accumulator_set<double, stats<variance>> acc_chain_mean;
  Eigen::VectorXd chain_var(num_chains);
  double unbiased_var_scale = num_draws / (num_draws - 1.0);

  for (int chain = 0; chain < num_chains; ++chain) {
    accumulator_set<double, stats<mean, variance>> acc_draw;
    for (int n = 0; n < num_draws; ++n) {
      acc_draw(draws[chain][n]);
    }

    chain_mean(chain) = boost::accumulators::mean(acc_draw);
    acc_chain_mean(chain_mean(chain));
    chain_var(chain)
        = boost::accumulators::variance(acc_draw) * unbiased_var_scale;
  }

  double var_between = num_draws * boost::accumulators::variance(acc_chain_mean)
                       * num_chains / (num_chains - 1);
  double var_within = chain_var.mean();

  // rewrote [(n-1)*W/n + B/n]/W as (n-1+ B/W)/n
  return sqrt((var_between / var_within + num_draws - 1) / num_draws);
}

/**
 * Computes the potential scale reduction (Rhat) for the specified
 * parameter across all kept samples.
 *
 * See more details in Stan reference manual section "Potential
 * Scale Reduction". http://mc-stan.org/users/documentation
 *
 * Current implementation assumes draws are stored in contiguous
 * blocks of memory.  Chains are trimmed from the back to match the
 * length of the shortest chain.  Argument size will be broadcast to
 * same length as draws.
 *
 * @param draws stores pointers to arrays of chains
 * @param sizes stores sizes of chains
 * @return potential scale reduction for the specified parameter
 */
inline double compute_potential_scale_reduction(
    std::vector<const double*> draws, size_t size) {
  int num_chains = draws.size();
  std::vector<size_t> sizes(num_chains, size);
  return compute_potential_scale_reduction(draws, sizes);
}

/**
 * Computes the split potential scale reduction (Rhat) for the
 * specified parameter across all kept samples.  When the number of
 * total draws N is odd, the (N+1)/2th draw is ignored.
 *
 * See more details in Stan reference manual section "Potential
 * Scale Reduction". http://mc-stan.org/users/documentation
 *
 * Current implementation assumes draws are stored in contiguous
 * blocks of memory.  Chains are trimmed from the back to match the
 * length of the shortest chain.
 *
 * @param draws stores pointers to arrays of chains
 * @param sizes stores sizes of chains
 * @return potential scale reduction for the specified parameter
 */
inline double compute_split_potential_scale_reduction(
    std::vector<const double*> draws, std::vector<size_t> sizes) {
  int num_chains = sizes.size();
  size_t num_draws = sizes[0];
  for (int chain = 1; chain < num_chains; ++chain) {
    num_draws = std::min(num_draws, sizes[chain]);
  }

  std::vector<const double*> split_draws = split_chains(draws, sizes);

  double half = num_draws / 2.0;
  std::vector<size_t> half_sizes(2 * num_chains, std::floor(half));

  return compute_potential_scale_reduction(split_draws, half_sizes);
}

/**
 * Computes the split potential scale reduction (Rhat) for the
 * specified parameter across all kept samples.  When the number of
 * total draws N is odd, the (N+1)/2th draw is ignored.
 *
 * See more details in Stan reference manual section "Potential
 * Scale Reduction". http://mc-stan.org/users/documentation
 *
 * Current implementation assumes draws are stored in contiguous
 * blocks of memory.  Chains are trimmed from the back to match the
 * length of the shortest chain.  Argument size will be broadcast to
 * same length as draws.
 *
 * @param draws stores pointers to arrays of chains
 * @param sizes stores sizes of chains
 * @return potential scale reduction for the specified parameter
 */
inline double compute_split_potential_scale_reduction(
    std::vector<const double*> draws, size_t size) {
  int num_chains = draws.size();
  std::vector<size_t> sizes(num_chains, size);
  return compute_split_potential_scale_reduction(draws, sizes);
}

}  // namespace analyze
}  // namespace stan

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 polynom toolbox - degree/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of polynom components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 06/03/2011
///
#include <nt2/include/functions/valuation.hpp>
#include <nt2/include/functions/isequal.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/memory/buffer.hpp>
#include <nt2/include/constants/real.hpp>
#include <nt2/table.hpp>

#include <boost/array.hpp>

NT2_TEST_CASE_TPL ( valuation_real__1_0,  NT2_REAL_TYPES)
{

  using nt2::valuation;
  using nt2::tag::valuation_;
  nt2::table<T> a =  nt2::_(T(1), T(4));
  nt2::table<T> b =  nt2::_(T(0), T(0), T(3));
  nt2::table<T> c(nt2::of_size(1, 0));
  NT2_DISPLAY(a);
  NT2_DISPLAY(valuation(a));
  NT2_DISPLAY(b);
  NT2_DISPLAY(valuation(b));
  NT2_TEST(nt2::isequal(0, valuation(a)));
  NT2_TEST(nt2::isequal(-1, valuation(b)));
  b(2) = b(1) = T(1);
  NT2_TEST(nt2::isequal(1, valuation(b)));

} // end of test for floating_


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

/*

    ADOBE CONFIDENTIAL
    ___________________

    Copyright 2001 - 2004 Adobe Systems Incorporated
    All Rights Reserved.

    NOTICE: All information contained herein is, and remains the property of
    Adobe Systems Incorporated and its suppliers, if any. The intellectual and
    technical concepts contained herein are proprietary to Adobe Systems Incorporated
    and its suppliers and may be covered by U.S. and Foreign Patents, patents
    in process, and are protected by trade secret or copyright law. Dissemination
    of this information or reproduction of this material is strictly forbidden
    unless prior written permission is obtained from Adobe Systems Incorporated.

*/

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

// start_of_example

#include <boost/detail/lightweight_test.hpp>
#include <adobe/enum_ops.hpp>

enum Foo
    {
    foo_4 = 1 << 2,
    foo_8 = 1 << 3
    };

ADOBE_DEFINE_BITSET_OPS(Foo)

int main()
    {
    Foo     a(foo_4);
    Foo     b(foo_8);
    Foo     c(a | b);

    BOOST_TEST(a == 4L);
    BOOST_TEST(b == 8L);
    BOOST_TEST(c == 12L);

    return boost::report_errors();
    }

// end_of_example


/*
 *  A test program for boost/rational.hpp.
 *  Change the typedef at the beginning of run_tests() to try out different
 *  integer types.  (These tests are designed only for signed integer
 *  types.  They should work for short, int and long.)
 *
 *  (C) Copyright Stephen Silver, 2001. Permission to copy, use, modify, sell
 *  and distribute this software is granted provided this copyright notice
 *  appears in all copies. This software is provided "as is" without express or
 *  implied warranty, and with no claim as to its suitability for any purpose.
 *
 *  Incorporated into the boost rational number library, and modified and
 *  extended, by Paul Moore, with permission.
 */

// boostinspect:nolicense (don't complain about the lack of a Boost license)
// (Stephen Silver hasn't been contacted yet for permission to change the
// license.  If Paul Moore's permission is also needed, then that's a problem
// since he hasn't been in contact for years.)

// Revision History
// 30 Aug 13  Add bug-test of assignments holding the basic and/or strong
//            guarantees (Daryle Walker)
// 27 Aug 13  Add test for cross-version constructor template (Daryle Walker)
// 23 Aug 13  Add bug-test of narrowing conversions during order comparison;
//            spell logical-negation in it as "!" because MSVC won't accept
//            "not" (Daryle Walker)
// 05 Nov 06  Add testing of zero-valued denominators & divisors; casting with
//            types that are not implicitly convertible (Daryle Walker)
// 04 Nov 06  Resolve GCD issue with depreciation (Daryle Walker)
// 02 Nov 06  Add testing for operator<(int_type) w/ unsigneds (Daryle Walker)
// 31 Oct 06  Add testing for operator<(rational) overflow (Daryle Walker)
// 18 Oct 06  Various fixes for old compilers (Joaquín M López Muñoz)
// 27 Dec 05  Add testing for Boolean conversion operator (Daryle Walker)
// 24 Dec 05  Change code to use Boost.Test (Daryle Walker)
// 04 Mar 01  Patches for Intel C++ and GCC (David Abrahams)

#define BOOST_TEST_MAIN  "Boost::Rational unit tests"

#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/mpl/list.hpp>
#include <boost/operators.hpp>
#include <boost/preprocessor/stringize.hpp>
#include <boost/integer/common_factor_rt.hpp>

#include <boost/rational.hpp>

#include <boost/test/unit_test.hpp>

#include <climits>
#include <iomanip>
#include <ios>
#include <iostream>
#include <istream>
#include <ostream>
#include <sstream>
#include <stdexcept>
#include <string>

// We can override this on the compile, as -DINT_TYPE=short or whatever.
// The default test is against rational<long>.
#ifndef INT_TYPE
#define INT_TYPE long
#endif

namespace {

class MyOverflowingUnsigned;

// This is a trivial user-defined wrapper around the built in int type.
// It can be used as a test type for rational<>
class MyInt : boost::operators<MyInt>
{
    friend class MyOverflowingUnsigned;
    int val;
public:
    MyInt(int n = 0) : val(n) {}
    friend MyInt operator+ (const MyInt&);
    friend MyInt operator- (const MyInt&);
    MyInt& operator+= (const MyInt& rhs) { val += rhs.val; return *this; }
    MyInt& operator-= (const MyInt& rhs) { val -= rhs.val; return *this; }
    MyInt& operator*= (const MyInt& rhs) { val *= rhs.val; return *this; }
    MyInt& operator/= (const MyInt& rhs) { val /= rhs.val; return *this; }
    MyInt& operator%= (const MyInt& rhs) { val %= rhs.val; return *this; }
    MyInt& operator|= (const MyInt& rhs) { val |= rhs.val; return *this; }
    MyInt& operator&= (const MyInt& rhs) { val &= rhs.val; return *this; }
    MyInt& operator^= (const MyInt& rhs) { val ^= rhs.val; return *this; }
    const MyInt& operator++() { ++val; return *this; }
    const MyInt& operator--() { --val; return *this; }
    bool operator< (const MyInt& rhs) const { return val < rhs.val; }
    bool operator== (const MyInt& rhs) const { return val == rhs.val; }
    bool operator! () const { return !val; }
    friend std::istream& operator>>(std::istream&, MyInt&);
    friend std::ostream& operator<<(std::ostream&, const MyInt&);
};

inline MyInt operator+(const MyInt& rhs) { return rhs; }
inline MyInt operator-(const MyInt& rhs) { return MyInt(-rhs.val); }
inline std::istream& operator>>(std::istream& is, MyInt& i) { is >> i.val; return is; }
inline std::ostream& operator<<(std::ostream& os, const MyInt& i) { os << i.val; return os; }
inline MyInt abs(MyInt rhs) { if (rhs < MyInt()) rhs = -rhs; return rhs; }

// This is an "unsigned" wrapper, that throws on overflow.  It can be used to
// test rational<> when an operation goes out of bounds.
class MyOverflowingUnsigned
    : private boost::unit_steppable<MyOverflowingUnsigned>
    , private boost::ordered_euclidian_ring_operators1<MyOverflowingUnsigned>
{
    // Helper type-aliases
    typedef MyOverflowingUnsigned  self_type;
    typedef unsigned self_type::*  bool_type;

    // Member data
    unsigned  v_;

public:
    // Exception base class
    class exception_base  { protected: virtual ~exception_base() throw() {} };

    // Divide-by-zero exception class
    class divide_by_0_error
        : public virtual exception_base
        , public         std::domain_error
    {
    public:
        explicit  divide_by_0_error( std::string const &w )
          : std::domain_error( w )  {}

        virtual  ~divide_by_0_error() throw()  {}
    };

    // Overflow exception class
    class overflowing_error
        : public virtual exception_base
        , public         std::overflow_error
    {
    public:
        explicit  overflowing_error( std::string const &w )
          : std::overflow_error( w )  {}

        virtual  ~overflowing_error() throw()  {}
    };

    // Lifetime management (use automatic dtr & copy-ctr)
              MyOverflowingUnsigned( unsigned v = 0 )  : v_( v )  {}
    explicit  MyOverflowingUnsigned( MyInt const &m )  : v_( m.val )  {}

    // Operators (use automatic copy-assignment); arithmetic & comparison only
    self_type &  operator ++()
    {
        if ( this->v_ == UINT_MAX )  throw overflowing_error( "increment" );
        else ++this->v_;
        return *this;
    }
    self_type &  operator --()
    {
        if ( !this->v_ )  throw overflowing_error( "decrement" );
        else --this->v_;
        return *this;
    }

    operator bool_type() const  { return this->v_ ? &self_type::v_ : 0; }

    bool       operator !() const  { return !this->v_; }
    self_type  operator +() const  { return self_type( +this->v_ ); }
    self_type  operator -() const  { return self_type( -this->v_ ); }

    bool  operator  <(self_type const &r) const  { return this->v_ <  r.v_; }
    bool  operator ==(self_type const &r) const  { return this->v_ == r.v_; }

    self_type &  operator *=( self_type const &r )
    {
        if ( r.v_ && this->v_ > UINT_MAX / r.v_ )
        {
            throw overflowing_error( "oversized factors" );
        }
        this->v_ *= r.v_;
        return *this;
    }
    self_type &  operator /=( self_type const &r )
    {
        if ( !r.v_ )  throw divide_by_0_error( "division" );
        this->v_ /= r.v_;
        return *this;
    }
    self_type &  operator %=( self_type const &r )
    {
        if ( !r.v_ )  throw divide_by_0_error( "modulus" );
        this->v_ %= r.v_;
        return *this;
    }
    self_type &  operator +=( self_type const &r )
    {
        if ( this->v_ > UINT_MAX - r.v_ )
        {
            throw overflowing_error( "oversized addends" );
        }
        this->v_ += r.v_;
        return *this;
    }
    self_type &  operator -=( self_type const &r )
    {
        if ( this->v_ < r.v_ )
        {
            throw overflowing_error( "oversized subtrahend" );
        }
        this->v_ -= r.v_;
        return *this;
    }

    // Input & output
    template < typename Ch, class Tr >
    friend  std::basic_istream<Ch, Tr> &
    operator >>( std::basic_istream<Ch, Tr> &i, self_type &x )
    { return i >> x.v_; }

    template < typename Ch, class Tr >
    friend  std::basic_ostream<Ch, Tr> &
    operator <<( std::basic_ostream<Ch, Tr> &o, self_type const &x )
    { return o << x.v_; }

};  // MyOverflowingUnsigned

inline MyOverflowingUnsigned abs( MyOverflowingUnsigned const &x ) { return x; }

} // namespace

// Specialize numeric_limits for the custom types
namespace std
{

template < >
class numeric_limits< MyInt >
{
    typedef numeric_limits<int>  limits_type;

public:
    static const bool is_specialized = limits_type::is_specialized;

    static MyInt min BOOST_PREVENT_MACRO_SUBSTITUTION () throw()  { return
     limits_type::min BOOST_PREVENT_MACRO_SUBSTITUTION (); }
    static MyInt max BOOST_PREVENT_MACRO_SUBSTITUTION () throw()  { return
     limits_type::max BOOST_PREVENT_MACRO_SUBSTITUTION (); }
    static MyInt lowest() throw()  { return min BOOST_PREVENT_MACRO_SUBSTITUTION
     (); }  // C++11

    static const int digits      = limits_type::digits;
    static const int digits10    = limits_type::digits10;
    static const int max_digits10 = 0;  // C++11
    static const bool is_signed  = limits_type::is_signed;
    static const bool is_integer = limits_type::is_integer;
    static const bool is_exact   = limits_type::is_exact;
    static const int radix       = limits_type::radix;
    static MyInt epsilon() throw()      { return limits_type::epsilon(); }
    static MyInt round_error() throw()  { return limits_type::round_error(); }

    static const int min_exponent   = limits_type::min_exponent;
    static const int min_exponent10 = limits_type::min_exponent10;
    static const int max_exponent   = limits_type::max_exponent;
    static const int max_exponent10 = limits_type::max_exponent10;

    static const bool has_infinity             = limits_type::has_infinity;
    static const bool has_quiet_NaN            = limits_type::has_quiet_NaN;
    static const bool has_signaling_NaN        = limits_type::has_signaling_NaN;
    static const float_denorm_style has_denorm = limits_type::has_denorm;
    static const bool has_denorm_loss          = limits_type::has_denorm_loss;

    static MyInt infinity() throw()      { return limits_type::infinity(); }
    static MyInt quiet_NaN() throw()     { return limits_type::quiet_NaN(); }
    static MyInt signaling_NaN() throw() {return limits_type::signaling_NaN();}
    static MyInt denorm_min() throw()    { return limits_type::denorm_min(); }

    static const bool is_iec559  = limits_type::is_iec559;
    static const bool is_bounded = limits_type::is_bounded;
    static const bool is_modulo  = limits_type::is_modulo;

    static const bool traps                    = limits_type::traps;
    static const bool tinyness_before          = limits_type::tinyness_before;
    static const float_round_style round_style = limits_type::round_style;

};  // std::numeric_limits<MyInt>

template < >
class numeric_limits< MyOverflowingUnsigned >
{
    typedef numeric_limits<unsigned>  limits_type;

public:
    static const bool is_specialized = limits_type::is_specialized;

    static MyOverflowingUnsigned min BOOST_PREVENT_MACRO_SUBSTITUTION () throw()
      { return limits_type::min BOOST_PREVENT_MACRO_SUBSTITUTION (); }
    static MyOverflowingUnsigned max BOOST_PREVENT_MACRO_SUBSTITUTION () throw()
      { return limits_type::max BOOST_PREVENT_MACRO_SUBSTITUTION (); }
    static MyOverflowingUnsigned lowest() throw()
      { return min BOOST_PREVENT_MACRO_SUBSTITUTION (); }  // C++11

    static const int digits      = limits_type::digits;
    static const int digits10    = limits_type::digits10;
    static const int max_digits10 = 0;  // C++11
    static const bool is_signed  = limits_type::is_signed;
    static const bool is_integer = limits_type::is_integer;
    static const bool is_exact   = limits_type::is_exact;
    static const int radix       = limits_type::radix;
    static MyOverflowingUnsigned epsilon() throw()
        { return limits_type::epsilon(); }
    static MyOverflowingUnsigned round_error() throw()
        {return limits_type::round_error();}

    static const int min_exponent   = limits_type::min_exponent;
    static const int min_exponent10 = limits_type::min_exponent10;
    static const int max_exponent   = limits_type::max_exponent;
    static const int max_exponent10 = limits_type::max_exponent10;

    static const bool has_infinity             = limits_type::has_infinity;
    static const bool has_quiet_NaN            = limits_type::has_quiet_NaN;
    static const bool has_signaling_NaN        = limits_type::has_signaling_NaN;
    static const float_denorm_style has_denorm = limits_type::has_denorm;
    static const bool has_denorm_loss          = limits_type::has_denorm_loss;

    static MyOverflowingUnsigned infinity() throw()
        { return limits_type::infinity(); }
    static MyOverflowingUnsigned quiet_NaN() throw()
        { return limits_type::quiet_NaN(); }
    static MyOverflowingUnsigned signaling_NaN() throw()
        { return limits_type::signaling_NaN(); }
    static MyOverflowingUnsigned denorm_min() throw()
        { return limits_type::denorm_min(); }

    static const bool is_iec559  = limits_type::is_iec559;
    static const bool is_bounded = limits_type::is_bounded;
    static const bool is_modulo  = limits_type::is_modulo;

    static const bool traps                    = limits_type::traps;
    static const bool tinyness_before          = limits_type::tinyness_before;
    static const float_round_style round_style = limits_type::round_style;

};  // std::numeric_limits<MyOverflowingUnsigned>

}  // namespace std

namespace {

// This fixture replaces the check of rational's packing at the start of main.
class rational_size_check
{
    typedef INT_TYPE                          int_type;
    typedef ::boost::rational<int_type>  rational_type;

public:
    rational_size_check()
    {
        using ::std::cout;

        char const * const  int_name = BOOST_PP_STRINGIZE( INT_TYPE );

        cout << "Running tests for boost::rational<" << int_name << ">\n\n";

        cout << "Implementation issue: the minimal size for a rational\n"
             << "is twice the size of the underlying integer type.\n\n";

        cout << "Checking to see if space is being wasted.\n"
             << "\tsizeof(" << int_name << ") == " << sizeof( int_type )
             << "\n";
        cout <<  "\tsizeof(boost::rational<" << int_name << ">) == "
             << sizeof( rational_type ) << "\n\n";

        cout << "Implementation has "
             << (
                  (sizeof( rational_type ) > 2u * sizeof( int_type ))
                  ? "included padding bytes"
                  : "minimal size"
                )
             << "\n\n";
    }
};

// This fixture groups all the common settings.
class my_configuration
{
public:
    template < typename T >
    class hook
    {
    public:
        typedef ::boost::rational<T>  rational_type;

    private:
        struct parts { rational_type parts_[ 9 ]; };

        static  parts  generate_rationals()
        {
            rational_type  r1, r2( 0 ), r3( 1 ), r4( -3 ), r5( 7, 2 ),
                           r6( 5, 15 ), r7( 14, -21 ), r8( -4, 6 ),
                           r9( -14, -70 );
            parts result;
            result.parts_[0] = r1;
            result.parts_[1] = r2;
            result.parts_[2] = r3;
            result.parts_[3] = r4;
            result.parts_[4] = r5;
            result.parts_[5] = r6;
            result.parts_[6] = r7;
            result.parts_[7] = r8;
            result.parts_[8] = r9;

            return result;
        }

        parts  p_;  // Order Dependency

    public:
        rational_type  ( &r_ )[ 9 ];  // Order Dependency

        hook() : p_( generate_rationals() ), r_( p_.parts_ ) {}
    };
};

// Instead of controlling the integer type needed with a #define, use a list of
// all available types.  Since the headers #included don't change because of the
// integer #define, only the built-in types and MyInt are available.  (Any other
// arbitrary integer type introduced by the #define would get compiler errors
// because its header can't be #included.)
typedef ::boost::mpl::list<short, int, long>     builtin_signed_test_types;
typedef ::boost::mpl::list<short, int, long, MyInt>  all_signed_test_types;

// Without these explicit instantiations, MSVC++ 6.5/7.0 does not find
// some friend operators in certain contexts.
::boost::rational<short>                 dummy1;
::boost::rational<int>                   dummy2;
::boost::rational<long>                  dummy3;
::boost::rational<MyInt>                 dummy4;
::boost::rational<MyOverflowingUnsigned> dummy5;
::boost::rational<unsigned>              dummy6;

// Should there be regular tests with unsigned integer types?

} // namespace

// Check if rational is the smallest size possible
BOOST_GLOBAL_FIXTURE( rational_size_check )

#if BOOST_CONTROL_RATIONAL_HAS_GCD
// The factoring function template suite
BOOST_AUTO_TEST_SUITE( factoring_suite )

// GCD tests
BOOST_AUTO_TEST_CASE_TEMPLATE( gcd_test, T, all_signed_test_types )
{
    BOOST_CHECK_EQUAL( boost::gcd<T>(  1,  -1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( boost::gcd<T>( -1,   1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( boost::gcd<T>(  1,   1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( boost::gcd<T>( -1,  -1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( boost::gcd<T>(  0,   0), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( boost::gcd<T>(  7,   0), static_cast<T>( 7) );
    BOOST_CHECK_EQUAL( boost::gcd<T>(  0,   9), static_cast<T>( 9) );
    BOOST_CHECK_EQUAL( boost::gcd<T>( -7,   0), static_cast<T>( 7) );
    BOOST_CHECK_EQUAL( boost::gcd<T>(  0,  -9), static_cast<T>( 9) );
    BOOST_CHECK_EQUAL( boost::gcd<T>( 42,  30), static_cast<T>( 6) );
    BOOST_CHECK_EQUAL( boost::gcd<T>(  6,  -9), static_cast<T>( 3) );
    BOOST_CHECK_EQUAL( boost::gcd<T>(-10, -10), static_cast<T>(10) );
    BOOST_CHECK_EQUAL( boost::gcd<T>(-25, -10), static_cast<T>( 5) );
}

// LCM tests
BOOST_AUTO_TEST_CASE_TEMPLATE( lcm_test, T, all_signed_test_types )
{
    BOOST_CHECK_EQUAL( boost::lcm<T>(  1,  -1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( boost::lcm<T>( -1,   1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( boost::lcm<T>(  1,   1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( boost::lcm<T>( -1,  -1), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( boost::lcm<T>(  0,   0), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( boost::lcm<T>(  6,   0), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( boost::lcm<T>(  0,   7), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( boost::lcm<T>( -5,   0), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( boost::lcm<T>(  0,  -4), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( boost::lcm<T>( 18,  30), static_cast<T>(90) );
    BOOST_CHECK_EQUAL( boost::lcm<T>( -6,   9), static_cast<T>(18) );
    BOOST_CHECK_EQUAL( boost::lcm<T>(-10, -10), static_cast<T>(10) );
    BOOST_CHECK_EQUAL( boost::lcm<T>( 25, -10), static_cast<T>(50) );
}

BOOST_AUTO_TEST_SUITE_END()
#endif  // BOOST_CONTROL_RATIONAL_HAS_GCD

// The basic test suite
BOOST_FIXTURE_TEST_SUITE( basic_rational_suite, my_configuration )

// Initialization tests
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_initialization_test, T,
 all_signed_test_types )
{
    my_configuration::hook<T>  h;
    boost::rational<T>  &r1 = h.r_[ 0 ], &r2 = h.r_[ 1 ], &r3 = h.r_[ 2 ],
                        &r4 = h.r_[ 3 ], &r5 = h.r_[ 4 ], &r6 = h.r_[ 5 ],
                        &r7 = h.r_[ 6 ], &r8 = h.r_[ 7 ], &r9 = h.r_[ 8 ];

    BOOST_CHECK_EQUAL( r1.numerator(), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( r2.numerator(), static_cast<T>( 0) );
    BOOST_CHECK_EQUAL( r3.numerator(), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( r4.numerator(), static_cast<T>(-3) );
    BOOST_CHECK_EQUAL( r5.numerator(), static_cast<T>( 7) );
    BOOST_CHECK_EQUAL( r6.numerator(), static_cast<T>( 1) );
    BOOST_CHECK_EQUAL( r7.numerator(), static_cast<T>(-2) );
    BOOST_CHECK_EQUAL( r8.numerator(), static_cast<T>(-2) );
    BOOST_CHECK_EQUAL( r9.numerator(), static_cast<T>( 1) );

    BOOST_CHECK_EQUAL( r1.denominator(), static_cast<T>(1) );
    BOOST_CHECK_EQUAL( r2.denominator(), static_cast<T>(1) );
    BOOST_CHECK_EQUAL( r3.denominator(), static_cast<T>(1) );
    BOOST_CHECK_EQUAL( r4.denominator(), static_cast<T>(1) );
    BOOST_CHECK_EQUAL( r5.denominator(), static_cast<T>(2) );
    BOOST_CHECK_EQUAL( r6.denominator(), static_cast<T>(3) );
    BOOST_CHECK_EQUAL( r7.denominator(), static_cast<T>(3) );
    BOOST_CHECK_EQUAL( r8.denominator(), static_cast<T>(3) );
    BOOST_CHECK_EQUAL( r9.denominator(), static_cast<T>(5) );

    BOOST_CHECK_THROW( boost::rational<T>( 3, 0), boost::bad_rational );
    BOOST_CHECK_THROW( boost::rational<T>(-2, 0), boost::bad_rational );
    BOOST_CHECK_THROW( boost::rational<T>( 0, 0), boost::bad_rational );
}

// Assignment (non-operator) tests
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_assign_test, T, all_signed_test_types )
{
    my_configuration::hook<T>  h;
    boost::rational<T> &       r = h.r_[ 0 ];

    r.assign( 6, 8 );
    BOOST_CHECK_EQUAL( r.numerator(),   static_cast<T>(3) );
    BOOST_CHECK_EQUAL( r.denominator(), static_cast<T>(4) );

    r.assign( 0, -7 );
    BOOST_CHECK_EQUAL( r.numerator(),   static_cast<T>(0) );
    BOOST_CHECK_EQUAL( r.denominator(), static_cast<T>(1) );

    BOOST_CHECK_THROW( r.assign( 4, 0), boost::bad_rational );
    BOOST_CHECK_THROW( r.assign( 0, 0), boost::bad_rational );
    BOOST_CHECK_THROW( r.assign(-7, 0), boost::bad_rational );
}

// Comparison tests
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_comparison_test, T,
 all_signed_test_types )
{
    my_configuration::hook<T>  h;
    boost::rational<T>  &r1 = h.r_[ 0 ], &r2 = h.r_[ 1 ], &r3 = h.r_[ 2 ],
                        &r4 = h.r_[ 3 ], &r5 = h.r_[ 4 ], &r6 = h.r_[ 5 ],
                        &r7 = h.r_[ 6 ], &r8 = h.r_[ 7 ], &r9 = h.r_[ 8 ];

    BOOST_CHECK( r1 == r2 );
    BOOST_CHECK( r2 != r3 );
    BOOST_CHECK( r4 <  r3 );
    BOOST_CHECK( r4 <= r5 );
    BOOST_CHECK( r1 <= r2 );
    BOOST_CHECK( r5 >  r6 );
    BOOST_CHECK( r5 >= r6 );
    BOOST_CHECK( r7 >= r8 );

    BOOST_CHECK( !(r3 == r2) );
    BOOST_CHECK( !(r1 != r2) );
    BOOST_CHECK( !(r1 <  r2) );
    BOOST_CHECK( !(r5 <  r6) );
    BOOST_CHECK( !(r9 <= r2) );
    BOOST_CHECK( !(r8 >  r7) );
    BOOST_CHECK( !(r8 >  r2) );
    BOOST_CHECK( !(r4 >= r6) );

    BOOST_CHECK( r1 == static_cast<T>( 0) );
    BOOST_CHECK( r2 != static_cast<T>(-1) );
    BOOST_CHECK( r3 <  static_cast<T>( 2) );
    BOOST_CHECK( r4 <= static_cast<T>(-3) );
    BOOST_CHECK( r5 >  static_cast<T>( 3) );
    BOOST_CHECK( r6 >= static_cast<T>( 0) );

    BOOST_CHECK( static_cast<T>( 0) == r2 );
    BOOST_CHECK( static_cast<T>( 0) != r7 );
    BOOST_CHECK( static_cast<T>(-1) <  r8 );
    BOOST_CHECK( static_cast<T>(-2) <= r9 );
    BOOST_CHECK( static_cast<T>( 1) >  r1 );
    BOOST_CHECK( static_cast<T>( 1) >= r3 );

    // Extra tests with values close in continued-fraction notation
    boost::rational<T> const  x1( static_cast<T>(9), static_cast<T>(4) );
    boost::rational<T> const  x2( static_cast<T>(61), static_cast<T>(27) );
    boost::rational<T> const  x3( static_cast<T>(52), static_cast<T>(23) );
    boost::rational<T> const  x4( static_cast<T>(70), static_cast<T>(31) );

    BOOST_CHECK( x1 < x2 );
    BOOST_CHECK( !(x1 < x1) );
    BOOST_CHECK( !(x2 < x2) );
    BOOST_CHECK( !(x2 < x1) );
    BOOST_CHECK( x2 < x3 );
    BOOST_CHECK( x4 < x2 );
    BOOST_CHECK( !(x3 < x4) );
    BOOST_CHECK( r7 < x1 );     // not actually close; wanted -ve v. +ve instead
    BOOST_CHECK( !(x2 < r7) );
}

// Increment & decrement tests
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_1step_test, T, all_signed_test_types )
{
    my_configuration::hook<T>  h;
    boost::rational<T>  &r1 = h.r_[ 0 ], &r2 = h.r_[ 1 ], &r3 = h.r_[ 2 ],
                        &r7 = h.r_[ 6 ], &r8 = h.r_[ 7 ];

    BOOST_CHECK(   r1++ == r2 );
    BOOST_CHECK(   r1   != r2 );
    BOOST_CHECK(   r1   == r3 );
    BOOST_CHECK( --r1   == r2 );
    BOOST_CHECK(   r8-- == r7 );
    BOOST_CHECK(   r8   != r7 );
    BOOST_CHECK( ++r8   == r7 );
}

// Absolute value tests
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_abs_test, T, all_signed_test_types )
{
    typedef my_configuration::hook<T>           hook_type;
    typedef typename hook_type::rational_type   rational_type;

    hook_type      h;
    rational_type  &r2 = h.r_[ 1 ], &r5 = h.r_[ 4 ], &r8 = h.r_[ 7 ];

#ifdef BOOST_NO_ARGUMENT_DEPENDENT_LOOKUP
    // This is a nasty hack, required because some compilers do not implement
    // "Koenig Lookup."  Basically, if I call abs(r), the C++ standard says that
    // the compiler should look for a definition of abs in the namespace which
    // contains r's class (in this case boost)--among other places.

    using boost::abs;
#endif

    BOOST_CHECK_EQUAL( abs(r2), r2 );
    BOOST_CHECK_EQUAL( abs(r5), r5 );
    BOOST_CHECK_EQUAL( abs(r8), rational_type(2, 3) );
}

// Unary operator tests
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_unary_test, T, all_signed_test_types )
{
    my_configuration::hook<T>  h;
    boost::rational<T>         &r2 = h.r_[ 1 ], &r3 = h.r_[ 2 ],
                               &r4 = h.r_[ 3 ], &r5 = h.r_[ 4 ];

    BOOST_CHECK_EQUAL( +r5, r5 );

    BOOST_CHECK( -r3 != r3 );
    BOOST_CHECK_EQUAL( -(-r3), r3 );
    BOOST_CHECK_EQUAL( -r4, static_cast<T>(3) );

    BOOST_CHECK( !r2 );
    BOOST_CHECK( !!r3 );

    BOOST_CHECK( ! static_cast<bool>(r2) );
    BOOST_CHECK( r3 );
}

BOOST_AUTO_TEST_SUITE_END()

// The rational arithmetic operations suite
BOOST_AUTO_TEST_SUITE( rational_arithmetic_suite )

// Addition & subtraction tests
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_additive_test, T,
 all_signed_test_types )
{
    typedef boost::rational<T>  rational_type;

    BOOST_CHECK_EQUAL( rational_type( 1, 2) + rational_type(1, 2),
     static_cast<T>(1) );
    BOOST_CHECK_EQUAL( rational_type(11, 3) + rational_type(1, 2),
     rational_type( 25,  6) );
    BOOST_CHECK_EQUAL( rational_type(-8, 3) + rational_type(1, 5),
     rational_type(-37, 15) );
    BOOST_CHECK_EQUAL( rational_type(-7, 6) + rational_type(1, 7),
     rational_type(  1,  7) - rational_type(7, 6) );
    BOOST_CHECK_EQUAL( rational_type(13, 5) - rational_type(1, 2),
     rational_type( 21, 10) );
    BOOST_CHECK_EQUAL( rational_type(22, 3) + static_cast<T>(1),
     rational_type( 25,  3) );
    BOOST_CHECK_EQUAL( rational_type(12, 7) - static_cast<T>(2),
     rational_type( -2,  7) );
    BOOST_CHECK_EQUAL(    static_cast<T>(3) + rational_type(4, 5),
     rational_type( 19,  5) );
    BOOST_CHECK_EQUAL(    static_cast<T>(4) - rational_type(9, 2),
     rational_type( -1,  2) );

    rational_type  r( 11 );

    r -= rational_type( 20, 3 );
    BOOST_CHECK_EQUAL( r, rational_type(13,  3) );

    r += rational_type( 1, 2 );
    BOOST_CHECK_EQUAL( r, rational_type(29,  6) );

    r -= static_cast<T>( 5 );
    BOOST_CHECK_EQUAL( r, rational_type( 1, -6) );

    r += rational_type( 1, 5 );
    BOOST_CHECK_EQUAL( r, rational_type( 1, 30) );

    r += static_cast<T>( 2 );
    BOOST_CHECK_EQUAL( r, rational_type(61, 30) );
}

// Assignment tests
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_assignment_test, T,
 all_signed_test_types )
{
    typedef boost::rational<T>  rational_type;

    rational_type  r;

    r = rational_type( 1, 10 );
    BOOST_CHECK_EQUAL( r, rational_type( 1, 10) );

    r = static_cast<T>( -9 );
    BOOST_CHECK_EQUAL( r, rational_type(-9,  1) );
}

// Multiplication tests
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_multiplication_test, T,
 all_signed_test_types )
{
    typedef boost::rational<T>  rational_type;

    BOOST_CHECK_EQUAL( rational_type(1, 3) * rational_type(-3, 4),
     rational_type(-1, 4) );
    BOOST_CHECK_EQUAL( rational_type(2, 5) * static_cast<T>(7),
     rational_type(14, 5) );
    BOOST_CHECK_EQUAL(  static_cast<T>(-2) * rational_type(1, 6),
     rational_type(-1, 3) );

    rational_type  r = rational_type( 3, 7 );

    r *= static_cast<T>( 14 );
    BOOST_CHECK_EQUAL( r, static_cast<T>(6) );

    r *= rational_type( 3, 8 );
    BOOST_CHECK_EQUAL( r, rational_type(9, 4) );
}

// Division tests
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_division_test, T,
 all_signed_test_types )
{
    typedef boost::rational<T>  rational_type;

    BOOST_CHECK_EQUAL( rational_type(-1, 20) / rational_type(4, 5),
     rational_type(-1, 16) );
    BOOST_CHECK_EQUAL( rational_type( 5,  6) / static_cast<T>(7),
     rational_type( 5, 42) );
    BOOST_CHECK_EQUAL(     static_cast<T>(8) / rational_type(2, 7),
     static_cast<T>(28) );

    BOOST_CHECK_THROW( rational_type(23, 17) / rational_type(),
     boost::bad_rational );
    BOOST_CHECK_THROW( rational_type( 4, 15) / static_cast<T>(0),
     boost::bad_rational );

    rational_type  r = rational_type( 4, 3 );

    r /= rational_type( 5, 4 );
    BOOST_CHECK_EQUAL( r, rational_type(16, 15) );

    r /= static_cast<T>( 4 );
    BOOST_CHECK_EQUAL( r, rational_type( 4, 15) );

    BOOST_CHECK_THROW( r /= rational_type(), boost::bad_rational );
    BOOST_CHECK_THROW( r /= static_cast<T>(0), boost::bad_rational );

    BOOST_CHECK_EQUAL( rational_type(-1) / rational_type(-3),
     rational_type(1, 3) );
}

// Tests for operations on self
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_self_operations_test, T,
 all_signed_test_types )
{
    typedef boost::rational<T>  rational_type;

    rational_type  r = rational_type( 4, 3 );

    r += r;
    BOOST_CHECK_EQUAL( r, rational_type( 8, 3) );

    r *= r;
    BOOST_CHECK_EQUAL( r, rational_type(64, 9) );

    r /= r;
    BOOST_CHECK_EQUAL( r, rational_type( 1, 1) );

    r -= r;
    BOOST_CHECK_EQUAL( r, rational_type( 0, 1) );

    BOOST_CHECK_THROW( r /= r, boost::bad_rational );
}

BOOST_AUTO_TEST_SUITE_END()

// The non-basic rational operations suite
BOOST_AUTO_TEST_SUITE( rational_extras_suite )

#ifndef BOOST_NO_IOSTREAM
// Output test
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_output_test, T, all_signed_test_types )
{
    using namespace std;
    typedef boost::rational<T>  rational_type;

    // Basic test
    ostringstream  oss;

    oss << rational_type( 44, 14 );
    BOOST_CHECK_EQUAL( oss.str(), "22/7" );

    // Width
    oss.clear(); oss.str( "" );
    oss << setw( 5 ) << setfill('*') << rational_type( 1, 2 ) << 'r';
    BOOST_CHECK_EQUAL( oss.str(), "**1/2r" );  // not "****1/2r"

    // Positive-sign
    oss.clear(); oss.str( "" );
    oss << showpos << rational_type( 2, 3 ) << noshowpos;
    BOOST_CHECK_EQUAL( oss.str(), "+2/3" );  // not "+2/+3"

    // Internal padding
    oss.clear(); oss.str( "" );
    oss << setw( 8 ) << internal << rational_type( 36, -15 ) << right << 'r';
    BOOST_CHECK_EQUAL( oss.str(), "-***12/5r" );  // not "-*****12/5r"

    // Showbase prefix
    oss.clear(); oss.str( "" );
    oss << showbase << hex << rational_type( 34, 987 ) << noshowbase << dec;
    BOOST_CHECK_EQUAL( oss.str(), "0x22/3db" );  // not "0x22/0x3db"
}

// Input test, failing
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_input_failing_test, T,
 all_signed_test_types )
{
    std::istringstream  iss( "" );
    boost::rational<T>  r;

    iss >> r;
    BOOST_CHECK( !iss );
    BOOST_CHECK( !iss.bad() );

    iss.clear();
    iss.str( "42" );
    iss >> r;
    BOOST_CHECK( !iss );

    iss.clear();
    iss.str( "57A" );
    iss >> r;
    BOOST_CHECK( !iss );

    iss.clear();
    iss.str( "20-20" );
    iss >> r;
    BOOST_CHECK( !iss );

    iss.clear();
    iss.str( "1/" );
    iss >> r;
    BOOST_CHECK( !iss );

    iss.clear();
    iss.str( "1/ 2" );
    iss >> r;
    BOOST_CHECK( !iss );

    iss.clear();
    iss.str( "1 /2" );
    iss >> r;
    BOOST_CHECK( !iss );

    // Illegal value check(s)
    typedef std::numeric_limits<T>  limits_type;

    iss.clear();
    iss.str( "3/0" );
    iss >> r;
    BOOST_CHECK( !iss );

    if ( limits_type::is_signed && limits_type::is_bounded &&
     limits_type::min BOOST_PREVENT_MACRO_SUBSTITUTION () +
     limits_type::max BOOST_PREVENT_MACRO_SUBSTITUTION () < T(0) )
    {
        std::ostringstream  oss;

        oss << 1 << '/' << limits_type::min BOOST_PREVENT_MACRO_SUBSTITUTION ();
        iss.clear();
        iss.str( oss.str() );
        iss.exceptions( std::ios::failbit );
        BOOST_CHECK( iss.good() );
        BOOST_CHECK_THROW( iss >> r, boost::bad_rational );
        BOOST_CHECK( iss.fail() && !iss.bad() );
        iss.exceptions( std::ios::goodbit );
    }
}

// Input test, passing
BOOST_AUTO_TEST_CASE_TEMPLATE( rational_input_passing_test, T,
 all_signed_test_types )
{
    typedef boost::rational<T>  rational_type;

    std::istringstream  iss( "1/2 12" );
    rational_type       r;
    int                 n = 0;

    BOOST_CHECK( iss >> r >> n );
    BOOST_CHECK_EQUAL( r, rational_type(1, 2) );
    BOOST_CHECK_EQUAL( n, 12 );

    iss.clear();
    iss.str( "34/67" );
    BOOST_CHECK( iss >> r );
    BOOST_CHECK_EQUAL( r, rational_type(34, 67) );

    iss.clear();
    iss.str( "-3/-6" );
    BOOST_CHECK( iss >> r );
    BOOST_CHECK_EQUAL( r, rational_type(1, 2) );
}
#endif  // BOOST_NO_IOSTREAM

// Conversion test
BOOST_AUTO_TEST_CASE( rational_cast_test )
{
    // Note that these are not generic.  The problem is that rational_cast<T>
    // requires a conversion from IntType to T.  However, for a user-defined
    // IntType, it is not possible to define such a conversion except as an
    // "operator T()".  This causes problems with overloading resolution.
    boost::rational<int> const  half( 1, 2 );

    BOOST_CHECK_CLOSE( boost::rational_cast<double>(half), 0.5, 0.01 );
    BOOST_CHECK_EQUAL( boost::rational_cast<int>(half), 0 );
    BOOST_CHECK_EQUAL( boost::rational_cast<MyInt>(half), MyInt() );
    BOOST_CHECK_EQUAL( boost::rational_cast<boost::rational<MyInt> >(half),
     boost::rational<MyInt>(1, 2) );

    // Conversions via explicit-marked constructors
    // (Note that the "explicit" mark prevents conversion to
    // boost::rational<MyOverflowingUnsigned>.)
    boost::rational<MyInt> const  threehalves( 3, 2 );

    BOOST_CHECK_EQUAL( boost::rational_cast<MyOverflowingUnsigned>(threehalves),
     MyOverflowingUnsigned(1u) );
}

#ifndef BOOST_NO_MEMBER_TEMPLATES
// Cross-conversion constructor test
BOOST_AUTO_TEST_CASE( rational_cross_constructor_test )
{
    // This template will be repeated a lot.
    using boost::rational;

    // Create a bunch of explicit conversions.
    rational<int> const                    half_i( 2, 4 );
    rational<unsigned> const               half_u( half_i );
    rational<MyInt> const                  half_mi( half_i );
    rational<MyOverflowingUnsigned> const  half_mu1(half_u), half_mu2(half_mi);

    BOOST_CHECK_EQUAL( half_u.numerator(), 1u );
    BOOST_CHECK_EQUAL( half_u.denominator(), 2u );
    BOOST_CHECK_EQUAL( half_mi.numerator(), MyInt(1) );
    BOOST_CHECK_EQUAL( half_mi.denominator(), MyInt(2) );
    BOOST_CHECK_EQUAL( half_mu1.numerator(), MyOverflowingUnsigned(1u) );
    BOOST_CHECK_EQUAL( half_mu1.denominator(), MyOverflowingUnsigned(2u) );
    BOOST_CHECK_EQUAL( half_mu2.numerator(), MyOverflowingUnsigned(1u) );
    BOOST_CHECK_EQUAL( half_mu2.denominator(), MyOverflowingUnsigned(2u) );

#if 0
    // This will fail since it needs an implicit conversion.
    // (Try it if your compiler supports C++11 lambdas.)
    BOOST_CHECK( [](rational<unsigned> x){return !!x;}(half_i) );
#endif

    // Translation from a built-in unsigned type to a signed one is
    // implementation-defined, so hopefully we won't get a trap value.
    // (We're counting on static_cast<int>(UINT_MAX) being negative.)
    rational<unsigned> const  too_small( 1u, UINT_MAX );
    rational<int>             receiver;

    BOOST_CHECK_THROW( receiver=rational<int>(too_small), boost::bad_rational );
}
#endif  // BOOST_NO_MEMBER_TEMPLATES

// Dice tests (a non-main test)
BOOST_AUTO_TEST_CASE_TEMPLATE( dice_roll_test, T, all_signed_test_types )
{
    typedef boost::rational<T>  rational_type;

    // Determine the mean number of times a fair six-sided die
    // must be thrown until each side has appeared at least once.
    rational_type  r = T( 0 );

    for ( int  i = 1 ; i <= 6 ; ++i )
    {
        r += rational_type( 1, i );
    }
    r *= static_cast<T>( 6 );

    BOOST_CHECK_EQUAL( r, rational_type(147, 10) );
}

BOOST_AUTO_TEST_SUITE_END()

// The bugs, patches, and requests checking suite
BOOST_AUTO_TEST_SUITE( bug_patch_request_suite )

// "rational operator< can overflow"
BOOST_AUTO_TEST_CASE( bug_798357_test )
{
    // Choose values such that rational-number comparisons will overflow if
    // the multiplication method (n1/d1 ? n2/d2 == n1*d2 ? n2*d1) is used.
    // (And make sure that the large components are relatively prime, so they
    // won't partially cancel to make smaller, more reasonable, values.)
    unsigned const  n1 = UINT_MAX - 2u, d1 = UINT_MAX - 1u;
    unsigned const  n2 = d1, d2 = UINT_MAX;
    boost::rational<MyOverflowingUnsigned> const  r1( n1, d1 ), r2( n2, d2 );

    BOOST_REQUIRE_EQUAL( boost::integer::gcd(n1, d1), 1u );
    BOOST_REQUIRE_EQUAL( boost::integer::gcd(n2, d2), 1u );
    BOOST_REQUIRE( n1 > UINT_MAX / d2 );
    BOOST_REQUIRE( n2 > UINT_MAX / d1 );
    BOOST_CHECK( r1 < r2 );
    BOOST_CHECK( !(r1 < r1) );
    BOOST_CHECK( !(r2 < r1) );
}

// "rational::operator< fails for unsigned value types"
BOOST_AUTO_TEST_CASE( patch_1434821_test )
{
    // If a zero-rational v. positive-integer comparison involves negation, then
    // it may fail with unsigned types, which wrap around (for built-ins) or
    // throw/be-undefined (for user-defined types).
    boost::rational<unsigned> const  r( 0u );

    BOOST_CHECK( r < 1u );
}

// "rational.hpp::gcd returns a negative value sometimes"
BOOST_AUTO_TEST_CASE( patch_1438626_test )
{
    // The issue only manifests with 2's-complement integers that use their
    // entire range of bits.  [This means that ln(-INT_MIN)/ln(2) is an integer
    // and INT_MAX + INT_MIN == -1.]  The common computer platforms match this.
#if (INT_MAX + INT_MIN == -1) && ((INT_MAX ^ INT_MIN) == -1)
    // If a GCD routine takes the absolute value of an argument only before
    // processing, it won't realize that -INT_MIN -> INT_MIN (i.e. no change
    // from negation) and will propagate a negative sign to its result.
    BOOST_REQUIRE_EQUAL( boost::integer::gcd(INT_MIN, 6), 2 );

    // That is bad if the rational number type does not check for that
    // possibility during normalization.
    boost::rational<int> const  r1( INT_MIN / 2 + 3, 6 ),
                                r2( INT_MIN / 2 - 3, 6 ), r3 = r1 + r2;

    // If the error happens, the signs of the components will be switched.
    // (The numerators' sum is INT_MIN, and its GCD with 6 would be negated.)
    BOOST_CHECK_EQUAL( r3.numerator(), INT_MIN / 2 );
    BOOST_CHECK_EQUAL( r3.denominator(), 3 );
#endif
}

// The bug/patch numbers for the above 3 tests are from our SourceForge repo
// before we moved to our own SVN & Trac server.  At the time this note is
// written, it seems that SourceForge has reset their tracking numbers at least
// once, so I don't know how to recover those old tickets.  The ticket numbers
// for the following tests are from our SVN/Trac repo.

//"narrowing conversion error with -std=c++0x in operator< with int_type != int"
BOOST_AUTO_TEST_CASE( ticket_5855_test )
{
    // The internals of operator< currently store a structure of two int_type
    // (where int_type is the component type of a boost::rational template
    // class) and two computed types.  These computed types, results of
    // arithmetic operations among int_type values, are either int_type
    // themselves or a larger type that can implicitly convert to int_type.
    // Those conversions aren't usually a problem.  But when an arithmetic
    // operation involving two values of a built-in scalar type smaller than int
    // are involved, the result is an int.  But the resulting int-to-shorter
    // conversion is considered narrowing, resulting in a warning or error on
    // some compilers.  Notably, C++11 compilers are supposed to consider it an
    // error.
    //
    // The solution is to force an explicit conversion, although it's otherwise
    // not needed.  (The compiler can rescind the narrowing warning if the
    // results of the larger type still fit in the smaller one, and that proof
    // can be generated at constexpr time.)
    typedef short                           shorter_than_int_type;
    typedef boost::rational<shorter_than_int_type>  rational_type;

    bool const  dummy = rational_type() < rational_type();

    BOOST_REQUIRE( !dummy );
}

// "rational::assign" doesn't even have the basic guarantee
BOOST_AUTO_TEST_CASE( ticket_9067_test )
{
    using boost::rational;
    using boost::integer::gcd;

    rational<int>  a( 6, -8 );

    // Normalize to maintain invariants
    BOOST_CHECK_EQUAL( a.numerator(), -3 );
    BOOST_CHECK_EQUAL( a.denominator(), 4 );
    BOOST_CHECK( a.denominator() > 0 );
    BOOST_CHECK_EQUAL( gcd(a.numerator(), a.denominator()), 1 );

    // Do we maintain the basic guarantee after a failed component-assign?
    BOOST_CHECK_THROW( a.assign(1, 0), boost::bad_rational );
    BOOST_CHECK_NE( a.denominator(), 0 );
    BOOST_CHECK( a.denominator() > 0 );
    BOOST_CHECK_EQUAL( gcd(a.numerator(), a.denominator()), 1 );

    // Do we get the strong guarantee?
    BOOST_CHECK_EQUAL( a.numerator(), -3 );
    BOOST_CHECK_EQUAL( a.denominator(), 4 );

#if INT_MIN + INT_MAX < 0
    // Try an example without a zero-denominator
    a = rational<int>( -9, 12 );
    BOOST_CHECK_EQUAL( a.numerator(), -3 );
    BOOST_CHECK_EQUAL( a.denominator(), 4 );
    BOOST_CHECK( a.denominator() > 0 );
    BOOST_CHECK_EQUAL( gcd(a.numerator(), a.denominator()), 1 );
    BOOST_CHECK_THROW( a.assign(-(INT_MIN + 1), INT_MIN), boost::bad_rational );
    BOOST_CHECK( a.denominator() > 0 );
    BOOST_CHECK_EQUAL( gcd(a.numerator(), a.denominator()), 1 );
    BOOST_CHECK_EQUAL( a.numerator(), -3 );
    BOOST_CHECK_EQUAL( a.denominator(), 4 );
#endif
}

BOOST_AUTO_TEST_SUITE_END()


#include "ros/ros.h"
#include <nav_msgs/Odometry.h>
// PCL
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/octree/octree.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/common/transforms.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;

int get_filenames(const std::string& dir, std::vector<std::string>& filenames)
{
    fs::path path(dir);
    if (!fs::exists(path))
    {
        return -1;
    }

    fs::directory_iterator end_iter;
    for (fs::directory_iterator iter(path); iter!=end_iter; ++iter)
    {
        if (fs::is_regular_file(iter->status()))
        {
            filenames.push_back(iter->path().string());
        }

        // if (fs::is_directory(iter->status()))
        // {
        //     get_filenames(iter->path().string(), filenames);
        // }
    }

    return filenames.size();
}

Eigen::Affine3f pose_to_eigen(float px,float py,float pz,float qx,float qy,float qz,float qw)
{
  //RotationMatrix to Quaterniond
  Eigen::Quaternionf q;
  q.x() = qx;
  q.y() = qy;
  q.z() = qz;
  q.w() = qw;
  //Quaterniond to RotationMatrix
  Eigen::Matrix3f Rx = q.toRotationMatrix();

  // laser ->world
  Eigen::Affine3f laser_to_world = Eigen::Affine3f::Identity();
  laser_to_world.translation() << px, py, pz;
  laser_to_world.linear() = Rx;

  // world -> laser
  Eigen::Affine3f world_to_laser= Eigen::Affine3f::Identity();
  world_to_laser.matrix() = laser_to_world.matrix().inverse();

  return world_to_laser;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "coloring_node");

  ROS_INFO("coloring_node started...");
  std::string root_path;
  float px,py,pz,qx,qy,qz,qw;
  ros::NodeHandle nh_("~");

  nh_.param<std::string>("root_path", root_path, "");
  nh_.param<float>("pos_x", px,0);
  nh_.param<float>("pos_y", py,0);
  nh_.param<float>("pos_z", pz,0);

  nh_.param<float>("qua_x", qx,0);
  nh_.param<float>("qua_y", qy,0);
  nh_.param<float>("qua_z", qz,0);
  nh_.param<float>("qua_w", qw,1);
  ros::Publisher odom_pub = nh_.advertise<nav_msgs::Odometry>("/odom", 1, true);
  //read pcd
  std::vector<std::string> pcd_names;
  get_filenames(root_path+"/pcd",pcd_names);
  pcl::PointCloud<pcl::PointXYZ>::Ptr target_ptr (new pcl::PointCloud<pcl::PointXYZ>);
  for (size_t i = 0; i < pcd_names.size(); i++)
  {
    /* code */
    std::string fileName = pcd_names[i];
    // load pcd file
    pcl::PointCloud<pcl::PointXYZ>::Ptr frame_ptr (new pcl::PointCloud<pcl::PointXYZ>);
      if(pcl::io::loadPCDFile<pcl::PointXYZ>(fileName, *frame_ptr)==-1) {
          std::cout << "load cloud failed!" << std::endl;
      }
    *target_ptr=*target_ptr+*frame_ptr;
  }
  pcl::PCDWriter writer;
  writer.write<pcl::PointXYZ>(root_path+"/fore.pcd", *target_ptr);
  //transfrom
  pcl::PointCloud<pcl::PointXYZ>::Ptr cam_frame_cloud(new pcl::PointCloud<pcl::PointXYZ>());
  Eigen::Affine3f init_pose = pose_to_eigen(px,py,pz,qx,qy,qz,qw);
  Eigen::AngleAxisf h_vector(M_PI /18 * 19, Eigen::Vector3f(0, 0, 1));
    Eigen::Matrix3f h_rotation_matrix = Eigen::Matrix3f::Identity();
    h_rotation_matrix = h_vector.toRotationMatrix();
    init_pose.rotate(h_rotation_matrix);

    Eigen::AngleAxisf v_vector(M_PI / 6, Eigen::Vector3f(0, 1, 0));
    Eigen::Matrix3f v_rotation_matrix = Eigen::Matrix3f::Identity();
    v_rotation_matrix = v_vector.toRotationMatrix();
    init_pose.rotate(v_rotation_matrix);

  pcl::transformPointCloud(*target_ptr, *cam_frame_cloud, init_pose);
  writer.write<pcl::PointXYZ>(root_path+"/back.pcd", *cam_frame_cloud);
  std::cout << "saved color pointscloud total count " << target_ptr->size() << std::endl;

  //Eigen::Affine3f init_pose = pose_to_eigen(0,0,0,0,0,0,1);
    // Eigen::AngleAxisf h_vector(M_PI /18 * 19, Eigen::Vector3f(0, 0, 1));
    // Eigen::Matrix3f h_rotation_matrix = Eigen::Matrix3f::Identity();
    // h_rotation_matrix = h_vector.toRotationMatrix();
    // init_pose.rotate(h_rotation_matrix);

    // Eigen::AngleAxisf v_vector(M_PI /18 * 3, Eigen::Vector3f(0, 1, 0));
    // Eigen::Matrix3f v_rotation_matrix = Eigen::Matrix3f::Identity();
    // v_rotation_matrix = v_vector.toRotationMatrix();
    // init_pose.rotate(v_rotation_matrix);

    Eigen::Matrix3f rotation_matrix = init_pose.linear();
    Eigen::Quaternionf quaternion(rotation_matrix);

    nav_msgs::Odometry pose;
    pose.header.stamp = ros::Time::now();
    pose.header.frame_id = "map";
    pose.pose.pose.position.x = init_pose.translation().x();
    pose.pose.pose.position.y = init_pose.translation().y();
    pose.pose.pose.position.z = init_pose.translation().z();
    pose.pose.pose.orientation.x = quaternion.x();
    pose.pose.pose.orientation.y = quaternion.y();
    pose.pose.pose.orientation.z = quaternion.z();
    pose.pose.pose.orientation.w = quaternion.w();

  ros::Rate rate(10);
  while(ros::ok())
  {
    odom_pub.publish(pose);

    ros::spinOnce();
    rate.sleep();
  }

  return 0;
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/idiv.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/ceil.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

STF_CASE_TPL( "Check idiv behavior with floating", STF_IEEE_TYPES )
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::idiv;
  using r_t = decltype(idiv(T(), T()));
  STF_TYPE_IS(r_t, bd::as_integer_t<T>);

#ifndef STF_NO_INVALIDS
  STF_EQUAL(idiv(bs::Inf<T>(),  bs::Inf<T>()), bs::Nan<r_t>());
  STF_EQUAL(idiv(bs::Minf<T>(), bs::Minf<T>()), bs::Nan<r_t>());
  STF_EQUAL(idiv(bs::Nan<T>(),  bs::Nan<T>()), bs::Nan<r_t>());
#endif
  STF_EQUAL(idiv(bs::One<T>(),bs::Zero<T>()), bs::Inf<r_t>());
  STF_EQUAL(idiv(bs::Zero<T>(), bs::Zero<T>()), bs::Nan<r_t>());
  STF_EQUAL(idiv(bs::One<T>(), bs::One<T>()), bs::One<r_t>());
}

STF_CASE_TPL( "Check idiv behavior with options", STF_NUMERIC_TYPES )
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::idiv;
  using r_t = decltype(idiv(T(), T()));
  STF_TYPE_IS(r_t, bd::as_integer_t<T>);

  STF_EQUAL(idiv(bs::One<T>(), bs::Two<T>()), bs::Zero<r_t>());

  STF_EQUAL(idiv(bs::One<T>(), bs::Two<T>(), bs::ceil), bs::One<r_t>());
  STF_EQUAL(idiv(bs::One<T>(), bs::Two<T>(), bs::floor), bs::Zero<r_t>());
  STF_EQUAL(idiv(bs::One<T>(), bs::Two<T>(), bs::round), bs::One<r_t>());
  STF_EQUAL(idiv(bs::One<T>(), bs::Two<T>(), bs::round2even), bs::Zero<r_t>());
  STF_EQUAL(idiv(bs::One<T>(), bs::Two<T>(), bs::fix), bs::Zero<r_t>());
}


#pragma once

#include "FieldEX.hpp"
#include <boost/multiprecision/cpp_int.hpp>

class BoostBitField : public FieldEX<boost::multiprecision::uint1024_t> {
private:
  boost::multiprecision::uint1024_t mat;
  boost::multiprecision::uint1024_t ok;
  boost::multiprecision::uint1024_t masks[33][2];
  bool is_first;
public:
  BoostBitField(const std::vector<std::string> &);
  BoostBitField() = delete;
  BoostBitField(const BoostBitField &) = default;
  BoostBitField(BoostBitField &&) = default;

  virtual BoostBitField& operator = (const BoostBitField &) = default;
  virtual BoostBitField& operator = (BoostBitField &&) = default;
  virtual ~BoostBitField() = default;

  virtual const boost::multiprecision::uint1024_t & raw() const noexcept override;
  virtual std::unique_ptr<FieldEX<boost::multiprecision::uint1024_t>> cloneEX() const noexcept override;
  virtual bool appliable(std::shared_ptr < StoneEX<boost::multiprecision::uint1024_t> > s, int x, int y, int reverse, int angle) const noexcept override;
  virtual void apply(std::shared_ptr < StoneEX<boost::multiprecision::uint1024_t> > s, int x, int y, int reverse, int angle) noexcept override;

  virtual bool at(int x, int y) const override;
  virtual bool appliable(std::shared_ptr<Stone> s, int x, int y, int reverse, int angle) const override;
  virtual void apply(std::shared_ptr<Stone> s, int x, int y, int reverse, int angle) override;
  virtual std::unique_ptr<Field> clone() const override;
};


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.
// Adds a sparse matmul on each shard and does a concatenated
// matrix multiply. Requires the matrix to be square for ease
// of specification.
#include "poplibs_support/TestDevice.hpp"

#include <boost/multi_array.hpp>
#include <boost/optional.hpp>
#include <boost/optional/optional_io.hpp>
#include <boost/program_options.hpp>

#include <poplar/Engine.hpp>
#include <poplar/Graph.hpp>

#include "poputil/exceptions.hpp"
#include <poplibs_support/Algorithm.hpp>
#include <poplibs_test/GeneralMatrixMultiply.hpp>
#include <poplibs_test/SparseMatrix.hpp>
#include <poplibs_test/Util.hpp>
#include <poputil/TileMapping.hpp>

#include "poplin/codelets.hpp"
#include "popops/codelets.hpp"

#include "popsparse/MatMul.hpp"
#include "popsparse/SparsePartitioner.hpp"
#include "popsparse/codelets.hpp"

#include <fstream>

// Tolerances used when data is not ignored
#define FLOAT_REL_TOL 0.01
#define HALF_REL_TOL 0.1

using namespace poplar;
using namespace poplar::program;

using namespace poplibs_test::util;
using namespace poplibs_support;

using namespace popsparse;
using namespace popsparse::dynamic;
using namespace poputil;

#define FLOAT_REL_TOL 0.01
#define HALF_REL_TOL 0.1

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  DeviceType deviceType = DeviceType::IpuModel2;
  unsigned numShards = 2;
  boost::optional<unsigned> tilesPerIPU;
  std::string profileJsonPath;
  // matrix multi is is mxm and mxn
  unsigned groups = 1, m = 16, n = 16;
  double sparsityFactor = 0.1;
  Type dataType = HALF;
  Type partialsType = FLOAT;

  std::string matmulOptionsString;

  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Produce help message")
    ("device-type",
     po::value<DeviceType>(&deviceType)->default_value(deviceType),
     deviceTypeHelp)
    ("profile", "Enable profiling and print profiling report")
    ("profile-json", po::value<std::string>(&profileJsonPath)->default_value(profileJsonPath),
     "Path to a file into which the profiling report will be output in json format")
    ("left-square-matrix-size", po::value<unsigned>(&m)->default_value(m),
     "Square matrix size for the left matrix")
    ("num-shards", po::value<unsigned>(&numShards)->default_value(numShards),
     "Number of shards (each shard on an IPU)")
    ("right-matrix-cols", po::value<unsigned>(&n)->default_value(n),
     "Right hand matrix input cols")
    ("tiles-per-ipu", po::value(&tilesPerIPU), "Number of tiles per IPU")
    ("matmul-options", po::value<std::string>(&matmulOptionsString),
     "Options to use for the matrix multiplication, specified as a JSON "
     "string, e.g. {\"key\":\"value\"}")
  ;
  // clang-format on

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  bool profile = vm.count("profile");
  bool profilingEnabled = profile || !profileJsonPath.empty();
  const std::size_t blockRows = 1;
  const std::size_t blockCols = 1;
  const auto blockArea = blockRows * blockCols;

  if (m % blockRows) {
    throw poputil::poplibs_error("size of matrix must be an integer multiple "
                                 "of rows in a block");
  }

  PlanningCache cache;

  OptionFlags matmulOptions;
  // User options specified via --matmul-options override defaults
  if (!matmulOptionsString.empty()) {
    poplar::readJSON(matmulOptionsString, matmulOptions);
  }

  auto device =
      tilesPerIPU ? createTestDevice(deviceType, numShards, *tilesPerIPU, true)
                  : createTestDeviceFullSize(deviceType, numShards, true);
  const auto &target = device.getTarget();
  Graph mainGraph(target);
  popops::addCodelets(mainGraph);
  poplin::addCodelets(mainGraph);
  popsparse::addCodelets(mainGraph);
  Sequence uploadProg, prog, downloadProg;

  std::vector<poplar::Graph> shardedGraphs;

  for (unsigned i = 0; i != numShards; ++i) {
    auto shard = mainGraph.createVirtualGraph(
        i * target.getTilesPerIPU(), (i + 1) * target.getTilesPerIPU());
    shardedGraphs.push_back(std::move(shard));
  }

  const auto sparsityType =
      blockArea == 1 ? SparsityType::Element : SparsityType::Block;
  SparsityParams sparsityParams(sparsityType, SparsityStructure::Unstructured,
                                {blockRows, blockCols});
  const auto params = MatMulParams::createWithNzRatio(
      std::move(sparsityParams), sparsityFactor, groups, m, m, n);

  // No support for groups yet
  assert(groups == 1);

  std::vector<SparseTensor> lhsMM;
  // We need the first rhs and the last output but we keep all anyway
  // even if they are not used.
  std::vector<Tensor> rhsMM, outMM;
  std::vector<std::pair<std::string, char *>> tmap;
  std::vector<std::unique_ptr<char[]>> rawMetaInfoMM, rawNzInfoMM;

  if (numShards == 1) {
    std::cerr << "failure: number of shards must be greater than 1\n";
    return 1;
  }
  for (unsigned i = 0; i != numShards; ++i) {
    const auto shardNum = std::to_string(i);
    auto &shardGraph = shardedGraphs[i];
    auto lhs =
        createSparseDenseMatMulLHS(shardGraph, dataType, params,
                                   "lhsMM" + shardNum, matmulOptions, &cache);
    lhsMM.push_back(lhs);
    auto rhs =
        i == 0
            ? createSparseDenseMatMulRHS(shardGraph, dataType, params,
                                         "rhsMM" + shardNum, matmulOptions,
                                         &cache)
            : poputil::copyToIpu(
                  mainGraph, outMM.back(), prog, 1, "inter-ipu-copy" + shardNum,
                  poplar::TensorCloneMethod::PRESERVE_ORDER_AND_ALIASES);
    rhsMM.push_back(rhs);
    auto out = sparseDenseMatMul(shardGraph, lhs, rhs, prog, false, false,
                                 "MM" + shardNum, matmulOptions, &cache);
    outMM.push_back(out);
    std::unique_ptr<char[]> rawMetaInfo = allocateHostMemoryForTensor(
        lhs.getMetaInfoTensor(), "lhsMM" + shardNum + ".meta", mainGraph,
        uploadProg, downloadProg, tmap);
    rawMetaInfoMM.push_back(std::move(rawMetaInfo));
    std::unique_ptr<char[]> rawNzInfo = allocateHostMemoryForTensor(
        lhs.getNzValuesTensor(), "lhsMM" + shardNum + ".values", mainGraph,
        uploadProg, downloadProg, tmap);
    rawNzInfoMM.push_back(std::move(rawNzInfo));
  }

  auto rawRHS = allocateHostMemoryForTensor(rhsMM.front(), "rhsMM0", mainGraph,
                                            uploadProg, downloadProg, tmap);
  auto rawOut = allocateHostMemoryForTensor(outMM.back(), "out", mainGraph,
                                            uploadProg, downloadProg, tmap);

  Sequence controlProg(std::move(uploadProg), std::move(prog));
  controlProg.add(downloadProg);

  using EType = float;
  Partitioner<EType> partitioner(params, dataType, target, matmulOptions,
                                 &cache);

  OptionFlags engineOptions;
  if (profilingEnabled) {
    engineOptions.set("debug.instrument", "true");
  }
  Engine engine(mainGraph, std::move(controlProg), engineOptions);
  attachStreams(engine, tmap);

  std::mt19937 randomEngine;
  std::array<std::size_t, 2> blockDims = {blockRows, blockCols};
  std::vector<CSRMatrix<EType>> csrMatrices(numShards, blockDims);
  for (unsigned i = 0; i != numShards; ++i) {
    std::tie(csrMatrices[i].nzValues, csrMatrices[i].columnIndices,
             csrMatrices[i].rowIndices) =
        poplibs_test::sparse::buildCSRMatrix<EType, std::size_t>(
            randomEngine, {m, m}, {blockRows, blockCols}, sparsityFactor,
            {0, 0}, {m, m}, 1.0, false);
  }

  boost::multi_array<double, 2> hostRHS(
      boost::extents[rhsMM.front().dim(1)][rhsMM.front().dim(2)]);
  writeRandomBinaryValues(target, dataType, hostRHS, -1.0, 1.0, randomEngine);
  boost::multi_array<double, 2> hostOut(
      boost::extents[outMM.back().dim(1)][outMM.back().dim(2)]);

  copy(target, hostRHS, dataType, rawRHS.get());
  for (unsigned i = 0; i != numShards; ++i) {
    const auto bucketsMM = partitioner.createSparsityDataImpl(csrMatrices[i]);
    copy(target, bucketsMM.metaInfo, UNSIGNED_SHORT, rawMetaInfoMM[i].get());
    copy(target, bucketsMM.nzValues, dataType, rawNzInfoMM[i].get());
  }

  device.bind([&](const Device &d) { engine.loadAndRun(d); });

  bool matchesModel = true;
  const double relTolerance = dataType == HALF ? HALF_REL_TOL : FLOAT_REL_TOL;
  copy(target, outMM[1].elementType(), rawOut.get(), hostOut);

  boost::multi_array<double, 2> modelOutPrev(
      boost::extents[outMM.front().dim(1)][outMM.front().dim(2)]);

  // Concatenated matrix multiply
  for (unsigned i = 0; i != numShards; ++i) {
    boost::multi_array<double, 2> hostDenseLHS(boost::extents[m][m]);
    hostDenseLHS = poplibs_test::sparse::csrToDenseMatrix(
        csrMatrices[i].nzValues.data(), csrMatrices[i].columnIndices.data(),
        csrMatrices[i].rowIndices.data(), csrMatrices[i].nzValues.size(), m, m,
        blockRows, blockCols);
    const auto rhsTensor = i == 0 ? rhsMM.front() : outMM[i - 1];
    boost::multi_array<double, 2> rhs(
        boost::extents[rhsTensor.dim(1)][rhsTensor.dim(2)]);
    rhs = i == 0 ? hostRHS : modelOutPrev;
    boost::multi_array<double, 2> modelOutput(
        boost::extents[outMM[i].dim(1)][outMM[i].dim(2)]);
    poplibs_test::gemm::generalMatrixMultiply(hostDenseLHS, rhs, modelOutput,
                                              false, false);
    modelOutPrev = modelOutput;
  }

  matchesModel &= checkIsClose("out", hostOut, modelOutPrev, relTolerance);

  if (profilingEnabled) {
    engine.printProfileSummary(std::cout, {{"showExecutionSteps", "true"}});
  }

  if (!matchesModel) {
    std::cerr << "Validation failed\n";
    return 1;
  }

  return 0;
}


/* Sarah Tang
 * Trajectory tracker to track an ellipse of given height/width and period.
 * x(t) = [Ax*cos(2*pi*t/T); Ay*sin(2*pi*t/T); 0] + init_pos.
 * Yaw is constant. */

#include <actionlib/server/simple_action_server.h>
#include <kr_mav_msgs/PositionCommand.h>
#include <kr_tracker_msgs/CircleTrackerAction.h>
#include <kr_tracker_msgs/TrackerStatus.h>
#include <kr_trackers_manager/Tracker.h>
#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <tf/transform_datatypes.h>

#include <Eigen/Dense>
#include <cmath>

class CircleTracker : public kr_trackers_manager::Tracker
{
 public:
  CircleTracker(void);

  void Initialize(const ros::NodeHandle &nh);
  bool Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd);
  void Deactivate();

  kr_mav_msgs::PositionCommand::ConstPtr update(const nav_msgs::Odometry::ConstPtr &msg);

  uint8_t status() const;

 private:
  void goal_callback();
  void preempt_callback();

  typedef actionlib::SimpleActionServer<kr_tracker_msgs::CircleTrackerAction> ServerType;
  // Action server that takes a trajectory.
  // Must be a pointer, because plugin does not support a constructor
  // with inputs, but an action server must be initialized with a Nodehandle.
  std::shared_ptr<ServerType> tracker_server_;

  // Is the tracker active?
  bool active_;
  // Do we have odometry?
  bool have_odom_;

  // Time trajectory started.
  ros::Time traj_start_time_;
  // traj_started_ indicates whether to reset start_time_.
  bool traj_started_;

  // Only track for position.
  // Distance traveled to get to last goal.
  float current_traj_length_;

  // Coefficients are ordered such that omega(t) = traj_coeffs_[i]*t^i.
  std::vector<float> omega_coeffs_;
  float Ax_, Ay_, omega_des_;

  // traj_completed_ indicates whether the trajectory is completed.
  bool traj_completed_;
  // Total time we want to track the trajectory for.
  float traj_duration_;
  // Time it takes to ramp up to the desired angular velocity.
  // ramp_dist_ is the theta at the end of the ramp phase.
  float ramp_time_, ramp_dist_;
  // Time that robot should stop circling (includes ramp up time).
  // circle_dist_ is the theta at the end of both phases.
  float circle_time_, circle_dist_;
  // Average angular acceleration for ramp up.
  float alpha_des_;

  // This is set if traj_completed_ = true;
  // Save the final position so we don't have to keep evaluating.
  // This holds the final position of the trajectory itself:
  // if use_offset_ = true, then the robot will evaluate position to
  // pos = final_pos_ + offset_pos_.
  Eigen::Vector3f final_pos_;

  // Offset to apply to the trajectory position.
  Eigen::Vector3f offset_pos_;

  // Record the current state.
  Eigen::Vector3f current_pos_;
  float current_yaw_;
  float constant_yaw_;

  // Pubish trigger for start and goal.
  ros::Publisher pub_start_;
  ros::Publisher pub_end_;
};

CircleTracker::CircleTracker(void)
    : active_(false),
      have_odom_(false),
      traj_started_(false),
      current_traj_length_(0.0),
      Ax_(0.0),
      Ay_(0.0),
      omega_des_(0.0),
      traj_completed_(false),
      traj_duration_(-1.0),
      ramp_time_(-1.0),
      circle_time_(-1.0)
{
}

void CircleTracker::Initialize(const ros::NodeHandle &nh)
{
  ros::NodeHandle priv_nh(nh, "circle_tracker");

  priv_nh.param("alpha_des", alpha_des_, static_cast<float>(M_PI / 20.0));

  // Set up the action server.
  tracker_server_ = std::shared_ptr<ServerType>(new ServerType(priv_nh, "CircleTracker", false));
  tracker_server_->registerGoalCallback(boost::bind(&CircleTracker::goal_callback, this));
  tracker_server_->registerPreemptCallback(boost::bind(&CircleTracker::preempt_callback, this));

  tracker_server_->start();

  pub_start_ = priv_nh.advertise<std_msgs::Empty>("traj_start", 10);
  pub_end_ = priv_nh.advertise<std_msgs::Empty>("traj_end", 10);
}

bool CircleTracker::Activate(const kr_mav_msgs::PositionCommand::ConstPtr &cmd)
{
  if(!have_odom_)
  {
    ROS_WARN("CircleTracker::Activate: could not activate because no odom recieved - not activating.");
    active_ = false;
    return active_;
  }

  if(!tracker_server_->isActive())
  {
    ROS_WARN("CircleTracker::Activate: server has no active goal - not activating.");
    active_ = false;
    return active_;
  }

  if(omega_coeffs_.empty())
  {
    ROS_WARN("CircleTracker::Activate: internal trajectory not initialized - not activating.");
    active_ = false;
    return active_;
  }

  active_ = true;
  traj_started_ = false;
  current_traj_length_ = 0.0;
  traj_completed_ = false;

  // Calculate the offset needed so that
  // x(0) + offset = current_pos_.
  // Here, x(0) is [Ax 0 0].
  offset_pos_ = current_pos_ - Eigen::Vector3f(Ax_, 0.0, 0.0);
  constant_yaw_ = current_yaw_;
  //   std::cout << " RELATIVE ACTIVATE CALCULATED AS " << offset_pos_(0) << " " << offset_pos_(1) << " " <<
  //   offset_pos_(2) << " " << offset_yaw_ << "\n";

  return active_;
}

void CircleTracker::Deactivate(void)
{
  if(tracker_server_->isActive())
  {
    ROS_WARN("CircleTracker::Deactivate: deactivated tracker while still tracking position trajectory.");

    kr_tracker_msgs::CircleTrackerResult result;
    result.duration = std::max(0.0f, static_cast<float>((ros::Time::now() - traj_start_time_).toSec()));
    result.length = current_traj_length_;
    tracker_server_->setAborted(result);
  }

  active_ = false;
  have_odom_ = false;
  traj_started_ = false;
  current_traj_length_ = 0.0;
  traj_completed_ = false;
}

kr_mav_msgs::PositionCommand::ConstPtr CircleTracker::update(const nav_msgs::Odometry::ConstPtr &msg)
{
  // Record distance between last position and current.
  const float dx =
      Eigen::Vector3f((current_pos_(0) - msg->pose.pose.position.x), (current_pos_(1) - msg->pose.pose.position.y),
                      (current_pos_(2) - msg->pose.pose.position.z))
          .norm();

  current_pos_(0) = msg->pose.pose.position.x;
  current_pos_(1) = msg->pose.pose.position.y;
  current_pos_(2) = msg->pose.pose.position.z;
  current_yaw_ = tf::getYaw(msg->pose.pose.orientation);

  have_odom_ = true;

  if(!active_)
  {
    return kr_mav_msgs::PositionCommand::Ptr();
  }

  current_traj_length_ += dx;

  const ros::Time t_now = ros::Time::now();
  kr_mav_msgs::PositionCommand::Ptr cmd(new kr_mav_msgs::PositionCommand);
  cmd->header.stamp = t_now;
  cmd->header.frame_id = msg->header.frame_id;

  if(!traj_started_)
  {
    // Start the trajectory.
    traj_start_time_ = ros::Time::now();
    traj_started_ = true;
    traj_completed_ = false;

    current_traj_length_ = 0.0;
  }

  const float traj_time = std::max(0.0f, static_cast<float>((ros::Time::now() - traj_start_time_).toSec()));
  constexpr float kEps = 1e-6;

  // Process position trajectory.
  if(traj_completed_)
  {
    cmd->position.x = final_pos_(0);
    cmd->position.y = final_pos_(1);
    cmd->position.z = final_pos_(2);

    cmd->yaw = constant_yaw_;

    cmd->velocity.x = 0.0;
    cmd->velocity.y = 0.0;
    cmd->velocity.z = 0.0;
    cmd->acceleration.x = 0.0;
    cmd->acceleration.y = 0.0;
    cmd->acceleration.z = 0.0;
    cmd->jerk.x = 0.0;
    cmd->jerk.y = 0.0;
    cmd->jerk.z = 0.0;

    cmd->yaw_dot = 0.0;
  }
  else
  {
    float theta, theta_dot, theta_ddot, theta_dddot;

    // theta = omega_des_ * traj_time;
    // theta_dot = omega_des_;
    // theta_ddot = 0.0;
    // theta_dddot = 0.0;
    // theta_d4dot = 0.0;
    // theta_d5dot = 0.0;
    // theta_d6dot = 0.0;

    // Ramping up the angular velocity.
    if(traj_time < ramp_time_)
    {
      // ROS_INFO("Circle Tracker: Accelerating...");

      /*      theta = 0.5 * alpha_des_ * traj_time_ * traj_time_;
            theta_dot = alpha_des_ * traj_time_;
            theta_ddot = alpha_des_;
            theta_dddot = 0.0;*/
      const float dT = traj_time / ramp_time_;
      // std::cout << " ramp time i s" << ramp_time_ << " and dt is " << dT << "\n";
      const float dT2 = dT * dT, dT3 = dT2 * dT, dT4 = dT3 * dT, dT5 = dT4 * dT, dT6 = dT5 * dT;
      theta = omega_coeffs_.at(0) * dT + 0.5 * omega_coeffs_.at(1) * dT2 + 1.0 / 3.0 * omega_coeffs_.at(2) * dT3 +
              0.25 * omega_coeffs_.at(3) * dT4 + 1.0 / 5.0 * omega_coeffs_.at(4) * dT5 +
              1.0 / 6.0 * omega_coeffs_.at(5) * dT6;
      theta_dot = omega_coeffs_.at(0) + omega_coeffs_.at(1) * dT + omega_coeffs_.at(2) * dT2 +
                  omega_coeffs_.at(3) * dT3 + omega_coeffs_.at(4) * dT4 + omega_coeffs_.at(5) * dT5;
      theta_dot /= ramp_time_;
      theta_ddot = omega_coeffs_.at(1) + 2.0 * omega_coeffs_.at(2) * dT + 3.0 * omega_coeffs_.at(3) * dT2 +
                   4.0 * omega_coeffs_.at(4) * dT3 + 5.0 * omega_coeffs_.at(5) * dT4;
      const float ramp_time_2 = ramp_time_ * ramp_time_;
      theta_ddot /= ramp_time_2;
      theta_dddot = 2.0 * omega_coeffs_.at(2) + 6.0 * omega_coeffs_.at(3) * dT + 12.0 * omega_coeffs_.at(4) * dT2 +
                    20.0 * omega_coeffs_.at(5) * dT3;
      const float ramp_time_3 = ramp_time_2 * ramp_time_;
      theta_dddot /= (ramp_time_3);
    }
    // Constant segment.
    else if(traj_time < circle_time_)
    {
      // ROS_INFO("Circle Tracker: Coasting...");
      std_msgs::Empty empty_msg;
      pub_start_.publish(empty_msg);

      // Calculate time in this phase.
      const float dT = traj_time - ramp_time_;

      theta = omega_des_ * dT + ramp_dist_;
      theta_dot = omega_des_;
      theta_ddot = 0.0;
      theta_dddot = 0.0;
    }
    // Ramping down the angular velocity.
    else
    {
      // const float dT = traj_time - circle_time_;
      //       theta = omega_des_ * dT + circle_dist_;
      // theta_dot = omega_des_;
      // theta_ddot = 0.0;
      // theta_dddot = 0.0;
      // theta_d4dot = 0.0;
      // theta_d5dot = 0.0;
      // theta_d6dot = 0.0;

      // ROS_INFO("Circle Tracker: Decelerating...");

      // Publish message indicating trajectory end.
      std_msgs::Empty empty_msg;
      pub_end_.publish(empty_msg);

      // const float dT = traj_time - circle_time_;
      // theta = -0.5 * alpha_des_ * dT * dT + omega_des_ * dT + circle_dist_;
      // theta_dot = -alpha_des_ * dT + omega_des_;
      // theta_ddot = -alpha_des_;
      // theta_dddot = 0.0;

      const float dT = 1.0 - (traj_time - circle_time_) / ramp_time_;
      const float dT2 = dT * dT, dT3 = dT2 * dT, dT4 = dT3 * dT, dT5 = dT4 * dT, dT6 = dT5 * dT;
      theta = omega_coeffs_.at(0) * dT + 0.5 * omega_coeffs_.at(1) * dT2 + 1.0 / 3.0 * omega_coeffs_.at(2) * dT3 +
              0.25 * omega_coeffs_.at(3) * dT4 + 1.0 / 5.0 * omega_coeffs_.at(4) * dT5 +
              1.0 / 6.0 * omega_coeffs_.at(5) * dT6;
      theta = circle_dist_ + ramp_dist_ - theta;
      theta_dot = omega_coeffs_.at(0) + omega_coeffs_.at(1) * dT + omega_coeffs_.at(2) * dT2 +
                  omega_coeffs_.at(3) * dT3 + omega_coeffs_.at(4) * dT4 + omega_coeffs_.at(5) * dT5;
      theta_dot /= ramp_time_;
      theta_ddot = omega_coeffs_.at(1) + 2.0 * omega_coeffs_.at(2) * dT + 3.0 * omega_coeffs_.at(3) * dT2 +
                   4.0 * omega_coeffs_.at(4) * dT3 + 5.0 * omega_coeffs_.at(5) * dT4;
      const float ramp_time_2 = ramp_time_ * ramp_time_;
      theta_ddot /= ramp_time_2;
      theta_dddot = 2.0 * omega_coeffs_.at(2) + 6.0 * omega_coeffs_.at(3) * dT + 12.0 * omega_coeffs_.at(4) * dT2 +
                    20.0 * omega_coeffs_.at(5) * dT3;
      const float ramp_time_3 = ramp_time_2 * ramp_time_;
      theta_dddot /= (ramp_time_3);
    }

    const float sin_t = sin(theta);
    const float cos_t = cos(theta);

    const float pos_x = Ax_ * cos_t;
    const float pos_y = Ay_ * sin_t;
    const float pos_z = 0.0;

    const float vel_x = Ax_ * (-sin_t * theta_dot);
    const float vel_y = Ay_ * (cos_t * theta_dot);
    const float vel_z = 0.0;

    const float theta_dot_2 = theta_dot * theta_dot;
    const float acc_x = Ax_ * (-cos_t * theta_dot_2 - sin_t * theta_ddot);
    const float acc_y = Ay_ * (-sin_t * theta_dot_2 + cos_t * theta_ddot);
    const float acc_z = 0.0;

    const float theta_dot_3 = theta_dot_2 * theta_dot;
    const float jerk_x = Ax_ * (sin_t * theta_dot_3 - 3.0 * cos_t * theta_dot * theta_ddot - sin_t * theta_dddot);
    const float jerk_y = Ay_ * (-cos_t * theta_dot_3 - 3.0 * sin_t * theta_dot * theta_ddot + cos_t * theta_dddot);
    const float jerk_z = 0.0;

    // Note, these values do NOT yet include the offset.
    cmd->position.x = pos_x;
    cmd->position.y = pos_y;
    cmd->position.z = pos_z;
    cmd->yaw = constant_yaw_;

    cmd->velocity.x = vel_x;
    cmd->velocity.y = vel_y;
    cmd->velocity.z = vel_z;
    cmd->yaw_dot = 0.0;

    cmd->acceleration.x = acc_x;
    cmd->acceleration.y = acc_y;
    cmd->acceleration.z = acc_z;

    cmd->jerk.x = jerk_x;
    cmd->jerk.y = jerk_y;
    cmd->jerk.z = jerk_z;

    // Check if we completed the trajectory.
    if(traj_time >= traj_duration_ - kEps)
    {
      traj_completed_ = true;
      final_pos_ = Eigen::Vector3f(pos_x, pos_y, pos_z);
    }
  }

  cmd->position.x += offset_pos_(0);
  cmd->position.y += offset_pos_(1);
  cmd->position.z += offset_pos_(2);

  // If trajectory is completed, indicate this in the action.
  if(tracker_server_->isActive() && traj_completed_)
  {
    kr_tracker_msgs::CircleTrackerResult result;
    result.duration = traj_time;
    result.length = current_traj_length_;
    tracker_server_->setSucceeded(result);
    current_traj_length_ = 0.0;
  }
  // Otherwise, send feedback command.
  else
  {
    kr_tracker_msgs::CircleTrackerFeedback feedback;
    feedback.duration = traj_time;
    tracker_server_->publishFeedback(feedback);
  }

  return cmd;
}

void CircleTracker::goal_callback()
{
  // If another goal is already active, cancel that goal
  // and track this one instead.
  if(tracker_server_->isActive())
  {
    ROS_INFO("CircleTracker trajectory aborted because new goal recieved.");
    kr_tracker_msgs::CircleTrackerResult result;
    result.duration = std::max(0.0f, static_cast<float>((ros::Time::now() - traj_start_time_).toSec()));
    result.length = current_traj_length_;

    tracker_server_->setAborted(result);
  }

  // Pointer to the goal recieved.
  const auto msg = tracker_server_->acceptNewGoal();

  // If preempt has been requested, then set this goal to preempted
  // and make no changes to the tracker state.
  if(tracker_server_->isPreemptRequested())
  {
    ROS_INFO("CircleTracker trajectory preempted immediately after it was recieved.");
    kr_tracker_msgs::CircleTrackerResult result;
    result.duration = 0.0;
    result.length = 0.0;
    tracker_server_->setPreempted(result);
    return;
  }

  // Calculate the trajectory.
  omega_des_ = 2.0 * M_PI / msg->T;
  Ax_ = msg->Ax;
  Ay_ = msg->Ay;
  // Calculate the ramp time.
  const float dT = omega_des_ / alpha_des_;

  Eigen::MatrixXf Ainv(6, 6);
  Ainv << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, -10.0, -6.0, -1.5,
      10.0, -4.0, 0.5, 15.0, 8.0, 1.5, -15.0, 7.0, -1.0, -6.0, -3.0, -0.5, 6.0, -3.0, 0.5;
  Eigen::VectorXf b(6);
  b << 0.0, 0.0, 0.0, omega_des_ * dT, 0.0, 0.0;
  // std::cout << "A_inv" << Ainv(3, 0) << " " << Ainv(3, 1) << " " << Ainv(3, 2) << " " << Ainv(3, 3) << " " << Ainv(3,
  // 4) << " " << Ainv(3, 5) << " and b " << b(3) << "\n";
  Eigen::VectorXf omega_coeffs_vec = Ainv * b;
  // std::cout << " rowwise multiply " << Ainv.row(3) * b << "\n";
  omega_coeffs_.resize(6);
  // Track the angle traveled when ramping up.
  // We have coefficients for omega(t).
  // We need to evaluate theta(t) = int_0^t omega(t) at 1.
  ramp_dist_ = 0.0;
  // Scale the trajectory back.
  // const std::vector<float> T_powers = {1, dT, dT * dT, dT*dT * dT, dT*dT*dT * dT, dT*dT*dT*dT * dT};
  for(int ii = 0; ii < 6; ++ii)
  {
    //  std::cout << " the condition here is " << b(ii) << "\n";
    omega_coeffs_.at(ii) = omega_coeffs_vec(ii);  // omega_coeffs_vec(ii) / T_powers.at(ii);
    // std::cout << " coefficient " << ii << " is " << omega_coeffs_vec(ii) << "\n";
    ramp_dist_ += omega_coeffs_.at(ii) * 1.0 / static_cast<float>(ii + 1);
  }

  // Calculate the offset needed so that
  // x(0) + offset = current_pos_.
  offset_pos_ = current_pos_ - Eigen::Vector3f(Ax_, 0.0, 0.0);
  //  std::cout << " RELATIVE CALCULATED AS " << offset_pos_(0) << " " << offset_pos_(1) << " " << offset_pos_(2) << " "
  //  << offset_yaw_ << "\n";

  ramp_time_ = dT;
  circle_time_ = msg->duration + ramp_time_;
  traj_duration_ = 2.0 * ramp_time_ + msg->duration;

  // Calculate the distances needed.
  circle_dist_ = ramp_dist_ + omega_des_ * msg->duration;

  // std::cout << " CIRCLE DESIGNED: ramp time " << ramp_time_ << " and circle time " << circle_time_ << " and total
  // duration " << traj_duration_ << " the ramp dist " << ramp_dist_ * 180.0 / M_PI << " and circle dist " <<
  // circle_dist_ * 180.0 / M_PI << " to get to angular velocity " << omega_des_ << "\n";

  traj_started_ = false;
  traj_completed_ = false;
}

void CircleTracker::preempt_callback()
{
  // Send a message reporting about the trajectory that was executed.
  kr_tracker_msgs::CircleTrackerResult result;
  result.duration = std::max(0.0f, static_cast<float>((ros::Time::now() - traj_start_time_).toSec()));
  result.length = current_traj_length_;

  if(tracker_server_->isActive())
  {
    ROS_INFO("CircleTracker trajectory aborted by preempt command.");
    tracker_server_->setAborted(result);
  }
  else
  {
    ROS_INFO("CircleTracker trajectory preempted by preempt command.");
    tracker_server_->setPreempted(result);
  }

  traj_started_ = true;
  traj_completed_ = true;
  traj_duration_ = 0.0;
  current_traj_length_ = 0.0;

  final_pos_ = current_pos_;
}

uint8_t CircleTracker::status() const
{
  return tracker_server_->isActive() ? static_cast<uint8_t>(kr_tracker_msgs::TrackerStatus::ACTIVE) :
                                       static_cast<uint8_t>(kr_tracker_msgs::TrackerStatus::SUCCEEDED);
}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(CircleTracker, kr_trackers_manager::Tracker);


// C++ standard library headers
#include <chrono>
#include <fstream>
#include <iostream>
#include <map>
#include <sstream>
#include <string>
#include <vector>

// OpenMP multithreading headers
#include <omp.h>

// Boost library headers
#include <boost/io/ios_state.hpp>

#define BOOST_MP_MIN_EXPONENT_DIGITS 3

#include <boost/multiprecision/cpp_bin_float.hpp>

// Project-specific headers
#include "Particle.hpp"
#include "SphericalECGJacobiVariationalOptimizer.hpp"

typedef boost::multiprecision::number<
        boost::multiprecision::cpp_bin_float<100>> real_t;

// typedef double real_t;

int get_num_threads() {
    int result = 1;
#pragma omp parallel
    {
        if (omp_get_thread_num() == 0) {
            result = omp_get_num_threads();
        }
    }
    return result;
}

int main(int, char **) {
    std::cout << std::setprecision(
            std::numeric_limits<real_t>::max_digits10 - 1);

    std::vector<zsvm::Particle<real_t>> particles;
    const std::string mass_carrier("mass");
    const std::string charge_carrier("charge");

    std::map<std::string, real_t> electron_carriers;
    electron_carriers.insert({mass_carrier, 1});
    electron_carriers.insert({charge_carrier, -1});

    std::map<std::string, real_t> positron_carriers;
    positron_carriers.insert({mass_carrier, 1});
    positron_carriers.insert({charge_carrier, +1});

    particles.emplace_back(0, +1, electron_carriers);
    particles.emplace_back(0, -1, electron_carriers);
    particles.emplace_back(1, +1, positron_carriers);
    // particles.emplace_back(1, -1, positron_carriers);

    const std::size_t num_threads =
            static_cast<std::size_t>(get_num_threads());
    if (num_threads == 1) {
        std::cout << "Performing serial basis expansion.\n";
    } else {
        std::cout << "Performing basis expansion using "
                  << num_threads << " threads.\n";
    }

    const std::size_t target_size = 100'000;
    const std::size_t trials_per_thread = 1;
    const std::size_t max_iterations = 10;

    std::ifstream basis_input_file("basis_0000000600.txt");
    std::vector<std::string> basis_input_lines;
    for (std::string line; std::getline(basis_input_file, line);) {
        basis_input_lines.push_back(line);
    }

    std::vector<std::vector<std::string>> basis_input_items;
    for (const auto &line : basis_input_lines) {
        std::stringstream line_stream(line);
        basis_input_items.emplace_back();
        for (std::string item; std::getline(line_stream, item, '\t');) {
            if (!item.empty()) {
                basis_input_items.back().push_back(item);
            }
        }
    }

    basis_input_items.erase(
            std::remove_if(basis_input_items.begin(), basis_input_items.end(),
                           [](const std::vector<std::string> &items) {
                               return items.empty();
                           }),
            basis_input_items.end());

//    for (const auto &line : basis_input_items) {
//        std::cout << "LINE(";
//        for (const auto &item : line) {
//            std::cout << "ITEM(" << item << "), ";
//        }
//        std::cout << ")\n";
//    }
//    std::cout << std::endl;

    std::vector<std::vector<real_t>> input_basis;
    for (const auto &line : basis_input_items) {
        input_basis.emplace_back();
        for (const auto &item : line) {
            std::stringstream item_stream(item);
            real_t x;
            item_stream >> x;
            input_basis.back().push_back(x);
        }
    }

//    for (const auto &basis_element : input_basis) {
//        std::cout << "LINE(";
//        for (const auto &coefficient : basis_element) {
//            std::cout << "NUMBER(" << coefficient << "), ";
//        }
//        std::cout << ")\n";
//    }
//    std::cout << std::endl;

    std::chrono::high_resolution_clock::time_point start =
            std::chrono::high_resolution_clock::now();

    std::vector<real_t> best_basis_element;
    std::vector<std::vector<real_t>> thread_basis_elements(num_threads);
    std::vector<real_t> thread_energies(num_threads);

    std::size_t min_index;
    bool any_found;

#pragma omp parallel
    {
        const int thread_num = omp_get_thread_num();
        zsvm::SphericalECGJacobiVariationalOptimizer<real_t> optimizer(
                3, particles, mass_carrier, charge_carrier);
        optimizer.set_basis(input_basis);

        for (std::size_t basis_size = optimizer.get_basis().size();
             basis_size <= target_size;
             basis_size = optimizer.get_basis().size()) {
            bool found = optimizer.expand_amoeba(
                    trials_per_thread, 1, max_iterations);
            if (found) {
                thread_basis_elements[thread_num] =
                        optimizer.last_basis_element();
                thread_energies[thread_num] =
                        optimizer.get_ground_state_energy();
            } else {
                thread_energies[thread_num] =
                        std::numeric_limits<real_t>::max();
            }
#pragma omp barrier
            if (thread_num == 0) {
                min_index = static_cast<std::size_t>(
                        std::min_element(thread_energies.begin(),
                                         thread_energies.end()) -
                        thread_energies.begin());
                if (thread_energies[min_index] ==
                    std::numeric_limits<real_t>::max()) {
                    any_found = false;
                    std::cout << basis_size << '\t'
                              << "FAILED TO IMPROVE ENERGY" << std::endl;
                } else {
                    any_found = true;
                    std::cout << basis_size << '\t'
                              << thread_energies[min_index] << std::endl;
                }
            }
#pragma omp barrier
            if (found && any_found) {
                optimizer.replace_basis_element(
                        thread_basis_elements[min_index]);
            } else if (!found && any_found) {
                optimizer.add_basis_element(
                        thread_basis_elements[min_index]);
            }
#pragma omp barrier
            if (thread_num == 0 && any_found) {
                std::ostringstream file_name;
                file_name << "basis_" << std::setfill('0') << std::setw(10)
                          << basis_size << ".txt";
                std::ofstream basis_file(file_name.str());
                basis_file << std::scientific << std::showpos
                           << std::setprecision(std::numeric_limits<
                                   real_t>::max_digits10 - 1);
                for (const auto &basis_element : optimizer.get_basis()) {
                    for (std::size_t i = 0; i < basis_element.size(); ++i) {
                        if (i > 0) { basis_file << '\t'; }
                        basis_file << basis_element[i];
                    }
                    basis_file << '\n';
                }
            }
        }
    }

    std::chrono::high_resolution_clock::time_point stop =
            std::chrono::high_resolution_clock::now();
    std::cout << "Time elapsed (seconds): "
              << std::chrono::duration_cast<
                      std::chrono::milliseconds>(
                      stop - start).count() / 1000.0 << std::endl;

//    if (argc != 2) {
//        std::cout << "Usage: " << argv[0] << " SCRIPT_FILE" << std::endl;
//        return 1;
//    }
//    std::string script_file_name(argv[1]);
//    {
//        std::ifstream script_file(script_file_name);
//        if (!script_file.good()) {
//            std::cout << "Error: could not open script file "
//                      << script_file_name << "." << std::endl;
//            return 2;
//        }
//    }
//    std::cout << std::scientific;
//    zsvm::ScriptInterpreter interpreter(script_file_name);
//    interpreter.run();
//    return 0;
}