//---------------------------------------------------------------------------//
// Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_MARSHALLING_ALGEBRA_CURVES_HPP
#define CRYPTO3_MARSHALLING_ALGEBRA_CURVES_HPP

#include <nil/crypto3/algebra/curves/bls12.hpp>

#include <nil/crypto3/multiprecision/cpp_int.hpp>

#include <boost/concept/assert.hpp>

#include <iterator>

using namespace nil::crypto3;

namespace nil {
    namespace marshalling {
        template<typename CurveType>
        struct curve_element_serializer { };

        // ZCash serialization format for BLS12-381
        // https://datatracker.ietf.org/doc/html/draft-irtf-cfrg-pairing-friendly-curves-09#appendix-C
        template<>
        struct curve_element_serializer<algebra::curves::bls12_381> {
            typedef algebra::curves::bls12_381 curve_type;

            typedef typename curve_type::template g1_type<>::value_type g1_value_type;
            typedef typename curve_type::template g2_type<>::value_type g2_value_type;

            typedef typename curve_type::template g1_type<algebra::curves::coordinates::affine>::value_type
                g1_affine_value_type;
            typedef typename curve_type::template g2_type<algebra::curves::coordinates::affine>::value_type
                g2_affine_value_type;

            typedef typename g1_value_type::field_type::value_type g1_field_value_type;
            typedef typename g2_value_type::field_type::value_type g2_field_value_type;

            typedef typename g1_field_value_type::integral_type integral_type;

            constexpr static const unsigned sizeof_field_element = 48;
            typedef std::array<std::uint8_t, sizeof_field_element> compressed_g1_octets;
            typedef std::array<std::uint8_t, 2 * sizeof_field_element> uncompressed_g1_octets;
            typedef std::array<std::uint8_t, 2 * sizeof_field_element> compressed_g2_octets;
            typedef std::array<std::uint8_t, 4 * sizeof_field_element> uncompressed_g2_octets;

            // Serialization procedure according to
            // https://datatracker.ietf.org/doc/html/draft-irtf-cfrg-pairing-friendly-curves-09#appendix-C.1
            static inline compressed_g1_octets point_to_octets_compress(const g1_value_type &point) {
                compressed_g1_octets result = {0};
                g1_affine_value_type point_affine = point.to_affine();
                auto m_byte = evaluate_m_byte(point_affine, true);
                // TODO: check possibilities for TA
                if (!(I_bit & m_byte)) {
                    multiprecision::export_bits(
                        point_affine.X.data.template convert_to<integral_type>(), result.rbegin(), 8, false);
                }
                result[0] |= m_byte;
                return result;
            }

            static inline uncompressed_g1_octets point_to_octets(const g1_value_type &point) {
                uncompressed_g1_octets result = {0};
                g1_affine_value_type point_affine = point.to_affine();
                auto m_byte = evaluate_m_byte(point_affine, false);
                // TODO: check possibilities for TA
                if (!(I_bit & m_byte)) {
                    multiprecision::export_bits(
                        point_affine.Y.data.template convert_to<integral_type>(), result.rbegin(), 8, false);
                    multiprecision::export_bits(point_affine.X.data.template convert_to<integral_type>(),
                                                result.rbegin() + sizeof_field_element,
                                                8,
                                                false);
                }
                result[0] |= m_byte;
                return result;
            }

            static inline compressed_g2_octets point_to_octets_compress(const g2_value_type &point) {
                compressed_g2_octets result = {0};
                g2_affine_value_type point_affine = point.to_affine();
                auto m_byte = evaluate_m_byte(point_affine, true);
                // TODO: check possibilities for TA
                if (!(I_bit & m_byte)) {
                    multiprecision::export_bits(
                        point_affine.X.data[0].data.template convert_to<integral_type>(), result.rbegin(), 8, false);
                    multiprecision::export_bits(point_affine.X.data[1].data.template convert_to<integral_type>(),
                                                result.rbegin() + sizeof_field_element,
                                                8,
                                                false);
                }
                result[0] |= m_byte;
                return result;
            }

            static inline uncompressed_g2_octets point_to_octets(const g2_value_type &point) {
                uncompressed_g2_octets result = {0};
                g2_affine_value_type point_affine = point.to_affine();
                auto m_byte = evaluate_m_byte(point_affine, false);
                // TODO: check possibilities for TA
                if (!(I_bit & m_byte)) {
                    multiprecision::export_bits(
                        point_affine.Y.data[0].data.template convert_to<integral_type>(), result.rbegin(), 8, false);
                    multiprecision::export_bits(point_affine.Y.data[1].data.template convert_to<integral_type>(),
                                                result.rbegin() + sizeof_field_element,
                                                8,
                                                false);
                    multiprecision::export_bits(point_affine.X.data[0].data.template convert_to<integral_type>(),
                                                result.rbegin() + 2 * sizeof_field_element,
                                                8,
                                                false);
                    multiprecision::export_bits(point_affine.X.data[1].data.template convert_to<integral_type>(),
                                                result.rbegin() + 3 * sizeof_field_element,
                                                8,
                                                false);
                }
                result[0] |= m_byte;
                return result;
            }

            // TODO: use iterators
            // Deserialization procedure according to
            // https://datatracker.ietf.org/doc/html/draft-irtf-cfrg-pairing-friendly-curves-09#appendix-C.2
            template<typename PointOctetsRange,
                     typename = typename std::enable_if<
                         std::is_same<std::uint8_t, typename PointOctetsRange::value_type>::value>::type>
            static inline g1_value_type octets_to_g1_point(const PointOctetsRange &octets) {
                BOOST_CONCEPT_ASSERT((boost::SinglePassRangeConcept<PointOctetsRange>));

                const std::uint8_t m_byte = *octets.begin() & 0xE0;
                BOOST_ASSERT(m_byte != 0x20 && m_byte != 0x60 && m_byte != 0xE0);

                PointOctetsRange point_octets;
                std::copy(octets.begin(), octets.end(), point_octets.begin());
                *point_octets.begin() &= 0x1F;

                if (m_byte & C_bit) {
                    return compressed_to_g1_point(point_octets, m_byte);
                }
                return uncompressed_to_g1_point(point_octets, m_byte);
            }

            // TODO: use iterators
            template<typename PointOctetsRange,
                     typename = typename std::enable_if<
                         std::is_same<std::uint8_t, typename PointOctetsRange::value_type>::value>::type>
            static inline g2_value_type octets_to_g2_point(const PointOctetsRange &octets) {
                BOOST_CONCEPT_ASSERT((boost::SinglePassRangeConcept<PointOctetsRange>));

                const std::uint8_t m_byte = *octets.begin() & 0xE0;
                BOOST_ASSERT(m_byte != 0x20 && m_byte != 0x60 && m_byte != 0xE0);

                PointOctetsRange point_octets;
                std::copy(octets.begin(), octets.end(), point_octets.begin());
                *point_octets.begin() &= 0x1F;

                if (m_byte & C_bit) {
                    return compressed_to_g2_point(point_octets, m_byte);
                }
                return uncompressed_to_g2_point(point_octets, m_byte);
            }

        protected:
            constexpr static const std::uint8_t C_bit = 0x80;
            constexpr static const std::uint8_t I_bit = 0x40;
            constexpr static const std::uint8_t S_bit = 0x20;
            // constexpr static const typename g1_field_value_type::integral_type half_p =
            //     (g1_field_value_type::modulus - integral_type(1)) / integral_type(2);

            template<typename PointOctetsRange,
                     typename = typename std::enable_if<
                         std::is_same<std::uint8_t, typename PointOctetsRange::value_type>::value>::type>
            static inline g1_value_type compressed_to_g1_point(PointOctetsRange &point_octets, std::uint8_t m_byte) {
                BOOST_ASSERT(std::distance(point_octets.begin(), point_octets.end()) == sizeof_field_element);

                if (m_byte & I_bit) {
                    BOOST_ASSERT(point_octets.end() == std::find(point_octets.begin(), point_octets.end(), true));
                    return g1_value_type();    // point at infinity
                }

                integral_type x;
                multiprecision::import_bits(x, point_octets.rbegin(), point_octets.rend(), 8, false);
                g1_field_value_type x_mod(x);
                g1_field_value_type y2_mod = x_mod.pow(3) + g1_field_value_type(4);
                BOOST_ASSERT(y2_mod.is_square());
                g1_field_value_type y_mod = y2_mod.sqrt();
                bool Y_bit = sign_gf_p(y_mod);
                if (Y_bit == bool(m_byte & S_bit)) {
                    g1_value_type result(x_mod, y_mod, g1_field_value_type::one());
                    BOOST_ASSERT(result.is_well_formed());
                    return result;
                }
                g1_value_type result(x_mod, -y_mod, g1_field_value_type::one());
                BOOST_ASSERT(result.is_well_formed());
                return result;
            }

            template<typename PointOctetsRange,
                     typename = typename std::enable_if<
                         std::is_same<std::uint8_t, typename PointOctetsRange::value_type>::value>::type>
            static inline g1_value_type uncompressed_to_g1_point(PointOctetsRange &point_octets, std::uint8_t m_byte) {
                BOOST_ASSERT(std::distance(point_octets.begin(), point_octets.end()) == 2 * sizeof_field_element);

                if (m_byte & I_bit) {
                    BOOST_ASSERT(point_octets.end() == std::find(point_octets.begin(), point_octets.end(), true));
                    return g1_value_type();    // point at infinity
                }

                integral_type x, y;
                multiprecision::import_bits(
                    y, point_octets.rbegin(), point_octets.rbegin() + sizeof_field_element, 8, false);
                multiprecision::import_bits(
                    x, point_octets.rbegin() + sizeof_field_element, point_octets.rend(), 8, false);
                g1_value_type result(g1_field_value_type(x), g1_field_value_type(y), g1_field_value_type::one());
                BOOST_ASSERT(result.is_well_formed());
                return result;
            }

            template<typename PointOctetsRange,
                     typename = typename std::enable_if<
                         std::is_same<std::uint8_t, typename PointOctetsRange::value_type>::value>::type>
            static inline g2_value_type compressed_to_g2_point(PointOctetsRange &point_octets, std::uint8_t m_byte) {
                BOOST_ASSERT(std::distance(point_octets.begin(), point_octets.end()) == 2 * sizeof_field_element);

                if (m_byte & I_bit) {
                    BOOST_ASSERT(point_octets.end() == std::find(point_octets.begin(), point_octets.end(), true));
                    return g2_value_type();    // point at infinity
                }

                integral_type x_0, x_1;
                multiprecision::import_bits(
                    x_0, point_octets.rbegin(), point_octets.rbegin() + sizeof_field_element, 8, false);
                multiprecision::import_bits(
                    x_1, point_octets.rbegin() + sizeof_field_element, point_octets.rend(), 8, false);
                g2_field_value_type x_mod(x_0, x_1);
                g2_field_value_type y2_mod = x_mod.pow(3) + g2_field_value_type(4, 4);
                BOOST_ASSERT(y2_mod.is_square());
                g2_field_value_type y_mod = y2_mod.sqrt();
                bool Y_bit = sign_gf_p(y_mod);
                if (Y_bit == bool(m_byte & S_bit)) {
                    g2_value_type result(x_mod, y_mod, g2_field_value_type::one());
                    BOOST_ASSERT(result.is_well_formed());
                    return result;
                }
                g2_value_type result(x_mod, -y_mod, g2_field_value_type::one());
                BOOST_ASSERT(result.is_well_formed());
                return result;
            }

            template<typename PointOctetsRange,
                     typename = typename std::enable_if<
                         std::is_same<std::uint8_t, typename PointOctetsRange::value_type>::value>::type>
            static inline g2_value_type uncompressed_to_g2_point(PointOctetsRange &point_octets, std::uint8_t m_byte) {
                BOOST_ASSERT(std::distance(point_octets.begin(), point_octets.end()) == 4 * sizeof_field_element);

                if (m_byte & I_bit) {
                    BOOST_ASSERT(point_octets.end() == std::find(point_octets.begin(), point_octets.end(), true));
                    return g2_value_type();    // point at infinity
                }

                integral_type x_0, x_1, y_0, y_1;
                multiprecision::import_bits(
                    y_0, point_octets.rbegin(), point_octets.rbegin() + sizeof_field_element, 8, false);
                multiprecision::import_bits(y_1,
                                            point_octets.rbegin() + sizeof_field_element,
                                            point_octets.rbegin() + 2 * sizeof_field_element,
                                            8,
                                            false);
                multiprecision::import_bits(x_0,
                                            point_octets.rbegin() + 2 * sizeof_field_element,
                                            point_octets.rbegin() + 3 * sizeof_field_element,
                                            8,
                                            false);
                multiprecision::import_bits(
                    x_1, point_octets.rbegin() + 3 * sizeof_field_element, point_octets.rend(), 8, false);
                g2_value_type result(g2_field_value_type(g1_field_value_type(x_0), g1_field_value_type(x_1)),
                                     g2_field_value_type(g1_field_value_type(y_0), g1_field_value_type(y_1)),
                                     g2_field_value_type::one());
                BOOST_ASSERT(result.is_well_formed());
                return result;
            }

            static inline bool sign_gf_p(const g1_field_value_type &v) {
                static const typename g1_field_value_type::integral_type half_p =
                    (g1_field_value_type::modulus - integral_type(1)) / integral_type(2);

                if (v > half_p) {
                    return true;
                }
                return false;
            }

            static inline bool sign_gf_p(const g2_field_value_type &v) {
                if (v.data[1] == 0) {
                    return sign_gf_p(v.data[0]);
                }
                return sign_gf_p(v.data[1]);
            }

            template<typename GroupValueType>
            static inline std::uint8_t evaluate_m_byte(const GroupValueType &point, bool compression) {
                std::uint8_t result = 0;
                if (compression) {
                    result |= C_bit;
                }
                // TODO: check condition of infinite point
                if (point.is_zero()) {
                    result |= I_bit;
                } else if (compression && sign_gf_p(point.Y)) {
                    result |= S_bit;
                }
                return result;
            }
        };
    }    // namespace marshalling
}    // namespace nil

#endif    // CRYPTO3_MARSHALLING_ALGEBRA_CURVES_HPP


#include <stdio.h>
#include <armadillo>
#include "mcpdft.h"
#include "energy.h"

using namespace mcpdft;

int main(int argc, char *argv[]) {
    if ( argc < 2 ) {
       std::cout << "An argument is needed!" << std::endl;
       printf("Usage: %s <test_case> <functional>\n", argv[0]);
       return 1;
    }

    // MCPDFT* mc = new MCPDFT(test_case);
    MCPDFT *mc;
    mc = new MCPDFT(argv[1]);

    // getting the value of the reference energy
    double eref = mc->get_eref();
    // printf("eref = %-20.15lf\n",eref);

    /* building alpha and beta 1-electron reduced
     * density matrices (1RDMs)
     */
    // mc->build_opdm();
    arma::mat D1a(mc->get_D1a());
    arma::mat D1b(mc->get_D1b());

    /* building the alpha-beta block of the 2-electron reduced
     * density matrix (2RDM)
     */
    mc->build_tpdm();
    arma::mat D2ab(mc->get_D2ab());

    // calculating the MCPDFT energy correction
    std::string functional(argv[2]);
    double e =  mcpdft_energy(mc,functional,D1a,D1b,D2ab);

    printf("=================================================\n");
    printf("   Reference energy      =  %-20.12lf\n",  eref);
    printf("   MCPDFT energy         =  %-20.12lf\n",     e);
    printf("   E(MCPDFT) - E(Ref)    =  %-20.2le\n", e-eref);
    printf("=================================================\n\n");

    delete mc;

    return 0;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <global.hpp>
//#include <Eigen/Dense>
//#include <boost/shared_ptr.hpp>

#include <boost/random/mersenne_twister.hpp>

using namespace Eigen;

template<class T>
class Clusterer
{
public:
  Clusterer(const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx, uint32_t K,
    boost::mt19937* pRndGen);
  virtual ~Clusterer();

//  void initialize(const Matrix<T,Dynamic,Dynamic>& x);

  virtual void updateLabels() = 0;
  virtual void updateCenters() = 0;
  virtual MatrixXu mostLikelyInds(uint32_t n,
      Matrix<T,Dynamic,Dynamic>& deviates) = 0;
  virtual T avgIntraClusterDeviation() = 0;

  const VectorXu& z() const {return z_;};
  const Matrix<T,Dynamic,Dynamic>& centroids() const {return ps_;};

  uint32_t getK(){return K_;};
  uint32_t K(){return K_;};

protected:
  uint32_t K_;
  const uint32_t D_;
  uint32_t N_;
  boost::shared_ptr<Matrix<T,Dynamic,Dynamic> > spx_; // pointer to data
  Matrix<T,Dynamic,Dynamic> ps_; // centroids on the sphere
  Matrix<uint32_t,Dynamic,1> Ns_; // counts for each cluster
  VectorXu z_; // labels
  boost::mt19937* pRndGen_;
};

// ----------------------------- impl -----------------------------------------

template<class T>
Clusterer<T>::Clusterer(
    const boost::shared_ptr<Matrix<T,Dynamic,Dynamic> >& spx, uint32_t K,
    boost::mt19937* pRndGen)
  : K_(K), D_(spx->rows()), N_(spx->cols()), spx_(spx), ps_(D_,K_), Ns_(K_),
    z_(N_), pRndGen_(pRndGen)
{};

template<class T>
Clusterer<T>::~Clusterer()
{};


#pragma once

#include <cmath>
#include <vector>
#include <chrono>
#include <functional>
#include <algorithm>

#include <boost/version.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/algorithm/copy.hpp>
#if BOOST_VERSION >= 107200
#include <boost/math/statistics/univariate_statistics.hpp>
#else
#include <boost/math/tools/univariate_statistics.hpp>
#define CLT_BENCHMARK2_OLD_BOOST
#endif
#include <boost/math/distributions/normal.hpp>

#include "util.hpp"

namespace clt::bench {

#ifdef CLT_BENCHMARK2_OLD_BOOST
CLT_message("Workaround patch for old boost.");

namespace workaround {

template <class T> inline auto mean_and_sample_variance(const T &v)
{
    return std::make_tuple(boost::math::tools::mean(v),
                           boost::math::tools::sample_variance(v));
};

template <class T> inline auto interquartile_range(T &v)
{
    using boost::math::tools::median;

    std::sort(v.begin(), v.end());
    const size_t n = v.size();
    const ssize_t n_half = n / 2;
    auto it_lb = v.begin();
    auto it_le = std::next(it_lb, n_half);
    auto it_ue = v.end();
    auto it_ub = std::next(it_ue, -n_half);
    const auto lm = median(it_lb, it_le);
    const auto um = median(it_ub, it_ue);
    return um - lm;
};

} // namespace workaround
#endif

template <class T> inline auto calc_stats(const T &data)
{
    using vec_fp_t = std::vector<double>;
    using duration_t = typename T::value_type;
    using ratio_t = typename duration_t::period;
    using boost::copy;
    using boost::adaptors::transformed;
#ifndef CLT_BENCHMARK2_OLD_BOOST
    using boost::math::statistics::mean_and_sample_variance;
    using boost::math::statistics::median;
    using boost::math::statistics::median_absolute_deviation;
    using boost::math::statistics::interquartile_range;
#else
    CLT_message("Workaround patch for old boost.");
    using workaround::mean_and_sample_variance;
    using boost::math::tools::median;
    using boost::math::tools::median_absolute_deviation;
    using workaround::interquartile_range;
#endif

    constexpr double nscale = ratio_t::num;
    constexpr double dscale = ratio_t::den;

    vec_fp_t fp_data(data.size());
    copy(data | transformed([nscale, dscale](auto &&x) {
             return x.count() * nscale / dscale;
         }),
         fp_data.begin());
    const auto [smean, uvar] = mean_and_sample_variance(fp_data);
    const auto smedian = median(fp_data);
    const auto smad = median_absolute_deviation(fp_data, smedian);
    const auto siqr = interquartile_range(fp_data);
    return std::make_tuple(smean, uvar, smedian, smad, siqr);
}

/**
 * Output alpha and bound.
 * alpha-percent CI is [smean - bound, smean + bound].
 */
template <class RealType>
inline auto confidence_interval_bound(const RealType uvar,
                                      const size_t nsamples,
                                      const double alpha = 0.95)
{
    using std::sqrt;
    using norm_t = boost::math::normal_distribution<>;
    using boost::math::quantile;

    assert(0 <= alpha);
    assert(alpha <= 1);
    const auto residue = (1 - alpha) / 2;
    const auto ustdev = std::sqrt(uvar / nsamples);
    const auto stdnorm = norm_t{};
    const auto tail = quantile(stdnorm, 1 - residue);
    const auto bound = tail * ustdev;
    return std::make_tuple(alpha, bound);
}

struct Experiment {
    using timer_t = std::chrono::high_resolution_clock;
    using duration_t = timer_t::duration;
    using vec_duration_t = std::vector<duration_t>;

    std::function<void()> setup_func_;
    std::function<void()> target_func_;
    std::function<bool()> check_func_;
    size_t num_setup_;
    size_t num_reptition_;
    std::chrono::seconds limit_time_;

    Experiment()
        : setup_func_([]() {}), target_func_([]() {}),
          check_func_([]() { return true; }), num_setup_(10000),
          num_reptition_(500), limit_time_(5)
    {
    }
    size_t max_total_number_of_execution() const noexcept
    {
        return num_setup_ * num_reptition_;
    }
    vec_duration_t run();
};
} // namespace clt::bench


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/broadcast.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/logical.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;

template <typename T, typename... N, typename Env>
void test(Env& $, brigand::list<N...> const&)
{
  static const std::size_t C = sizeof...(N);
  using p_t = bs::pack<T,C>;

  p_t p( (T(1+N::value))... );

  STF_EQUAL( bs::broadcast< -1>(p), bs::Zero<p_t>() );
  STF_EQUAL( bs::broadcast<  0>(p), p_t(T(1))       );
  STF_EQUAL( bs::broadcast<C/2>(p), p_t(T(C/2+1))   );
  STF_EQUAL( bs::broadcast<C-1>(p), p_t(T(C))       );
  STF_EQUAL( bs::broadcast<C*2>(p), bs::Zero<p_t>() );
}

STF_CASE_TPL( "Check broadcast<N>() behavior", STF_NUMERIC_TYPES )
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T>($, brigand::range<std::size_t, 0, N  >{});
  test<T>($, brigand::range<std::size_t, 0, N/2>{});
  test<T>($, brigand::range<std::size_t, 0, N*2>{});
}


// ---------------------------------------------------------------------------
//
//  This file is part of PermLib.
//
// Copyright (c) 2009-2011 Thomas Rehn <thomas@carmen76.de>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. The name of the author may not be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
// OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
// IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
// NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
// THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// ---------------------------------------------------------------------------

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

#include <iostream>
#include <fstream>
#include <vector>

#include <permlib/permlib_api.h>

#include "test-common.h"

using namespace permlib;
using namespace permlib::test;

BOOST_AUTO_TEST_CASE( orbit_ulong_trivial )
{
	const PermutationGroup group = construct<PERMUTATION,TRANSVERSAL>(info_trivial);
	std::list<boost::shared_ptr<OrbitAsSet> > o = orbits(group);

	BOOST_CHECK_EQUAL( o.size(), info_trivial.n );
}

BOOST_AUTO_TEST_CASE( orbit_stabilizer_ulong_test1997 )
{
	const PermutationGroup group = construct<PERMUTATION,TRANSVERSAL>(info_1997);
	std::list<boost::shared_ptr<OrbitAsSet> > o = orbits(group);

	BOOST_CHECK_EQUAL( o.size(), 1 );

	const unsigned long DeltaSize = 3;
	unsigned long Delta[DeltaSize] = {0,1,4};
	boost::shared_ptr<PermutationGroup> stab1 = setStabilizer(group, Delta, Delta+DeltaSize);
	BOOST_REQUIRE( stab1 );
	o = orbits(*stab1);

	BOOST_FOREACH(const boost::shared_ptr<OrbitAsSet>& orb, o) {
		if (orb->contains(Delta[0])) {
			BOOST_REQUIRE_EQUAL( orb->size(), DeltaSize );
			for (unsigned int i = 1; i < DeltaSize; ++i) {
				BOOST_CHECK( orb->contains(Delta[i]) );
			}
		}
	}

	BOOST_CHECK_EQUAL(o.size(), 3);
}

typedef std::vector<unsigned long> Vector;
template<class PERM>
struct VectorAction {
	Vector operator()(const PERM& p, const Vector& v) {
		Vector ret(v);
		for (unsigned int i = 0; i < v.size(); ++i) {
			ret[i] = v[p / i];
		}
		return ret;
	}
};

BOOST_AUTO_TEST_CASE( orbit_stabilizer_vector_test1997 )
{
	const PermutationGroup group = construct<PERMUTATION,TRANSVERSAL>(info_1997);

	Vector v(group.n);
	for (unsigned int i = 0; i < v.size(); ++i)
		v[i] = i;

	std::list<Vector> vectors;
	vectors.push_back(v);

	std::list<boost::shared_ptr<OrbitSet<PERMUTATION,Vector> > > o = orbits<Vector, VectorAction<PERMUTATION> >(group, vectors.begin(), vectors.end());

	BOOST_REQUIRE_EQUAL( o.size(), 1 );
	BOOST_CHECK_EQUAL( o.front()->size(), group.order() );
	BOOST_CHECK( o.front()->contains(v) );

	boost::shared_ptr<PermutationGroup> stab1 = setStabilizer(group, v.begin(), v.end());
	BOOST_REQUIRE( stab1 );
	o = orbits<Vector, VectorAction<PERMUTATION> >(*stab1, vectors.begin(), vectors.end());

	BOOST_REQUIRE_EQUAL( o.size(), 1 );
	BOOST_CHECK_EQUAL( o.front()->size(), stab1->order() );
	BOOST_CHECK( o.front()->contains(v) );

	OrbitSet<PERMUTATION,Vector>::const_iterator orbIt = o.front()->begin();
	const unsigned int magicNumber = 4267;
	BOOST_REQUIRE_GT(o.front()->size(), magicNumber);
	// #magicNumber is a number between 0 and 5616-1
	// the hope is that v != w
	std::advance(orbIt, magicNumber);
	Vector w = *orbIt;
	vectors.push_back(w);

	o = orbits<Vector, VectorAction<PERMUTATION> >(group, vectors.begin(), vectors.end());

	BOOST_REQUIRE_EQUAL( o.size(), 1 );
	BOOST_CHECK( o.front()->contains(v) );
	BOOST_CHECK( o.front()->contains(w) );
}

BOOST_AUTO_TEST_CASE( vector_stabilizer_test1997 )
{
	const PermutationGroup group = construct<PERMUTATION,TRANSVERSAL>(info_1997);

	Vector v(group.n);
	for (unsigned int i = 0; i < v.size(); ++i)
		v[i] = i%3;

	boost::shared_ptr<PermutationGroup> stab = vectorStabilizer(group, v.begin(), v.end());
	BOOST_REQUIRE( stab );
	// correct order computed with GAP
	BOOST_CHECK_EQUAL( stab->order(), 2 );
}

BOOST_AUTO_TEST_CASE( notlexmin_test1997 )
{
	const PermutationGroup group = construct<PERMUTATION,TRANSVERSAL>(info_1997);

	const unsigned int base[] = {3,5,6,8,9};
	const unsigned int zeros[] = {0,5,6};
	const unsigned int ones[] = {3,8,9,11};

	// [4,6,7,9,10] is not lex-min in orbit because e.g. [ 1, 12, 3, 11, 13 ] is lex-smaller
	BOOST_CHECK( isNotLexMinSet(group, base, base+5, zeros, zeros+3, ones, ones+4) );
}


#pragma once

#include <boost/python.hpp>
#include <boost/python/extract.hpp>
#include <boost/python/numpy.hpp>
#include <utility>

#include "corridor/cubic_spline/cubic_interpolation_2d.h"
#include "corridor/cubic_spline/cubic_spline.h"
#include "corridor/cubic_spline/cubic_spline_coefficients.h"
#include "corridor/cubic_spline/cubic_spline_utilities.h"
#include "utility.hpp"

namespace py = boost::python;
namespace np = boost::python::numpy;
namespace cr = corridor;
namespace cs = cr::cubic_spline;

// /////////////////////////////////////////////////////////////////////////////
// Cubic Spline wrapper
// /////////////////////////////////////////////////////////////////////////////

struct CubicSplineWrapper {
  // CPP object
  cs::CubicSpline cubic_spline_;

  CubicSplineWrapper(const int id, const py::list& node_x,
                     const py::list& node_y) {
    using namespace corridor;
    CartesianPoints2D points = toCartesianPoints(node_x, node_y);
    cubic_spline_ = cs::CubicSpline(id, points);
  }

  CubicSplineWrapper(const int id, const py::list& node_x,
                     const py::list& node_y, const py::list& py_first_tangent,
                     const py::list& py_last_tangent) {
    using namespace corridor;
    CartesianPoints2D points = toCartesianPoints(node_x, node_y);
    CartesianVector2D first_tangent = to_cartesian_vector(py_first_tangent);
    CartesianVector2D last_tangent = to_cartesian_vector(py_last_tangent);
    cubic_spline_ = cs::CubicSpline(id, points, first_tangent, last_tangent);
  }
  const cr::RealType GetTotalLength() { return cubic_spline_.GetTotalLength(); }

  py::dict getPolyline(const corridor::RealType delta_l) {
    // Constructs a Cartesian polyline from the cubic spline.
    // delta_s defines the sampling rate of the arc-length
    using namespace corridor;
    CartesianPoints2D polyline;
    RealType query_l = 0.0;
    RealType max_length = cubic_spline_.GetTotalLength();
    while (query_l <= max_length) {
      polyline.emplace_back(cubic_spline_.GetPositionAt(query_l));
      query_l += delta_l;
    }
    if (query_l > max_length) {
      // Add last point
      polyline.emplace_back(cubic_spline_.GetPositionAt(max_length));
    }
    // convert to python data structures
    const std::pair<py::list, py::list> py_polyline = to_py_lists(polyline);
    // Fill dictionary
    py::dict polyline_dict;
    polyline_dict["x"] = py_polyline.first;
    polyline_dict["y"] = py_polyline.second;
    return polyline_dict;
  }
};


#ifndef MATH_HPP
#define MATH_HPP

#include <armadillo>

arma::mat33 build_psi_tht_phi_TM(const double &psi, const double &tht, const double &phi);
arma::mat33 skew_sym(arma::vec3 vec);

#endif


#pragma once
#include "numeric.hpp"
#include <Eigen/Core>
//#include <boost/math/special_functions/erf.hpp>

// inspiration: Google Ceres solver > Jet.h

namespace common
{
	// Dual Number class for automatic differentiation
	template<typename T, int N>
	class Dual_Number
	{
	protected:
		// Value part.
		T f_;

		// The infinitesimal part.
		//
		// We allocate Jets on the stack and other places they might not be aligned
		// to X(=16 [SSE], 32 [AVX] etc)-byte boundaries, which would prevent the safe
		// use of vectorisation.  If we have C++11, we can specify the alignment.
		// However, the standard gives wide latitude as to what alignments are valid,
		// and it might be that the maximum supported alignment *guaranteed* to be
		// supported is < 16, in which case we do not specify an alignment, as this
		// implies the host is not a modern x86 machine.  If using < C++11, we cannot
		// specify alignment.

#if defined(EIGEN_DONT_VECTORIZE)
		typedef Eigen::Matrix<T, N, 1, Eigen::DontAlign> VecX;
		Eigen::Matrix<T, N, 1, Eigen::DontAlign> grad_;
#else
		// Enable vectorisation iff the maximum supported scalar alignment is >=
		// 16 bytes, as this is the minimum required by Eigen for any vectorisation.
		//
		// NOTE: It might be the case that we could get >= 16-byte alignment even if
		//       max_align_t < 16.  However we can't guarantee that this
		//       would happen (and it should not for any modern x86 machine) and if it
		//       didn't, we could get misaligned Jets.
		static constexpr int kAlignOrNot =
			// Work around a GCC 4.8 bug
			// (https://gcc.gnu.org/bugzilla/show_bug.cgi?id=56019) where
			// std::max_align_t is misplaced.
#if defined (__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ == 8
			alignof(::max_align_t) >= 16
#else
			alignof(std::max_align_t) >= 16
#endif
			? Eigen::AutoAlign : Eigen::DontAlign;

#if defined(EIGEN_MAX_ALIGN_BYTES)
		// Eigen >= 3.3 supports AVX & FMA instructions that require 32-byte alignment
		// (greater for AVX512).  Rather than duplicating the detection logic, use
		// Eigen's macro for the alignment size.
		//
		// NOTE: EIGEN_MAX_ALIGN_BYTES can be > 16 (e.g. 32 for AVX), even though
		//       kMaxAlignBytes will max out at 16.  We are therefore relying on
		//       Eigen's detection logic to ensure that this does not result in
		//       misaligned Jets.
#define CERES_JET_ALIGN_BYTES EIGEN_MAX_ALIGN_BYTES
#else
		// Eigen < 3.3 only supported 16-byte alignment.
#define CERES_JET_ALIGN_BYTES 16
#endif

		// Default to the native alignment if 16-byte alignment is not guaranteed to
		// be supported.  We cannot use alignof(T) as if we do, GCC 4.8 complains that
		// the alignment 'is not an integer constant', although Clang accepts it.
		static constexpr size_t kAlignment = kAlignOrNot == Eigen::AutoAlign
			? CERES_JET_ALIGN_BYTES : alignof(double);

#undef CERES_JET_ALIGN_BYTES
		typedef Eigen::Matrix<T, N, 1, kAlignOrNot> VecX;
		alignas(kAlignment)Eigen::Matrix<T, N, 1, kAlignOrNot> grad_;
#endif

	public:
		enum { DIMENSION = N };

		Dual_Number() : f_() { grad_.setZero(); }
		template<typename V>
		explicit Dual_Number(const V& f) { f_ = T(f); grad_.setZero(); }

		// FALLBACK OPERATORS
		explicit Dual_Number(const Dual_Number<float, N>& f) { f_ = T(f.f()); grad_ = f.grad().template cast<T>(); }
		explicit operator double() const { return double(f_); }
		explicit operator int() const { return int(f_); }
		template <typename V>
		Dual_Number<V, N> cast() const { return { V(f_), grad_.template cast<V>() }; }

		Dual_Number(const T& f, int k) {
			f_ = f;
			grad_.setZero();
			grad_[k] = T(1.0);
		}
		void setPartial(const T& f, int k) {
			f_ = f;
			grad_.setZero();
			grad_[k] = T(1.0);
		}

		// Constructor from scalar and vector part
		// The use of Eigen::DenseBase allows Eigen expressions
		// to be passed in without being fully evaluated until
		// they are assigned to v
		template<typename Derived>
		EIGEN_STRONG_INLINE Dual_Number(const T& a, const Eigen::DenseBase<Derived> &v)
			: f_(a), grad_(v) {
		}

		const T& f() const { return f_; }
		T& f() { return f_; }
		const VecX& grad() const { return grad_; }
		VecX& grad() { return grad_; }

		/// basic operators

		Dual_Number& operator+= (const Dual_Number& rhs)
		{
			f_ += rhs.f_;
			grad_ += rhs.grad_;
			return *this;
		}

		Dual_Number& operator-= (const Dual_Number& rhs)
		{
			f_ -= rhs.f_;
			grad_ -= rhs.grad_;
			return *this;
		}

		Dual_Number& operator*= (const Dual_Number& rhs)
		{
			f_ *= rhs.f_;
			grad_.noalias() = grad_ * rhs.f_ + rhs.grad_*f_;
			return *this;
		}

		Dual_Number& operator/= (const Dual_Number& rhs)
		{
			const T rhs_inverse = T(1.0) / rhs.f_;
			f_ *= rhs_inverse;
			grad_ = (grad_ - f_ * rhs.grad_) * rhs_inverse;
			return *this;
		}

		Dual_Number operator* (const double& rhs)
		{
			return{ f_ * rhs, grad_ * rhs };
		}

		Dual_Number operator/ (const double& rhs)
		{
			return{ f_ / rhs, grad_ / rhs };
		}

		Dual_Number& operator*= (const double & rhs)
		{
			f_ *= rhs;
			grad_ *= rhs;
			return *this;
		}

		Dual_Number& operator/= (const double& rhs)
		{
			f_ /= rhs;
			grad_ /= rhs;
			return *this;
		}

		Dual_Number operator- (const double& rhs)
		{
			return{ f_ - rhs, grad_ };
		}

		Dual_Number operator+ (const double& rhs)
		{
			return{ f_ + rhs, grad_ };
		}
	};

	// Unary +
	template<typename T, int N> inline
		Dual_Number<T, N> const& operator+(const Dual_Number<T, N>& f) {
		return f;
	}

	// Unary -
	template<typename T, int N> inline
		Dual_Number<T, N> operator-(const Dual_Number<T, N>&f) {
		return{ -f.f(), -f.grad() };
	}

	// Binary +
	template<typename T, int N> inline
		Dual_Number<T, N> operator+(const Dual_Number<T, N>& f,
			const Dual_Number<T, N>& g) {
		return{ f.f() + g.f(), f.grad() + g.grad() };
	}

	// Binary + with a scalar: x + s
	template<typename T, int N> inline
		Dual_Number<T, N> operator+(const Dual_Number<T, N>& f, T s) {
		return{ f.f() + s, f.grad() };
	}

	// Binary + with a scalar: s + x
	template<typename T, int N> inline
		Dual_Number<T, N> operator+(T s, const Dual_Number<T, N>& f) {
		return{ f.f() + s, f.grad() };
	}

	// Binary -
	template<typename T, int N> inline
		Dual_Number<T, N> operator-(const Dual_Number<T, N>& f,
			const Dual_Number<T, N>& g) {
		return{ f.f() - g.f(), f.grad() - g.grad() };
	}

	// Binary - with a scalar: x - s
	template<typename T, int N> inline
		Dual_Number<T, N> operator-(const Dual_Number<T, N>& f, T s) {
		return{ f.f() - s, f.grad() };
	}

	// Binary - with a scalar: s - x
	template<typename T, int N> inline
		Dual_Number<T, N> operator-(T s, const Dual_Number<T, N>& f) {
		return{ s - f.f(), -f.grad() };
	}

	// Binary *
	template<typename T, int N> inline
		Dual_Number<T, N> operator*(const Dual_Number<T, N>& f,
			const Dual_Number<T, N>& g) {
		return{ f.f() * g.f(), f.f() * g.grad() + f.grad() * g.f() };
	}

	// Binary * with a scalar: x * s
	template<typename T, int N> inline
		Dual_Number<T, N> operator*(const Dual_Number<T, N>& f, T s) {
		return{ f.f() * s, f.grad() * s };
	}

	// Binary * with a scalar: s * x
	template<typename T, int N> inline
		Dual_Number<T, N> operator*(T s, const Dual_Number<T, N>& f) {
		return{ f.f() * s, f.grad() * s };
	}

	// Binary /
	template<typename T, int N> inline
		Dual_Number<T, N> operator/(const Dual_Number<T, N>& f,
			const Dual_Number<T, N>& g) {
		// This uses:
		//
		//   a + u   (a + u)(b - v)   (a + u)(b - v)
		//   ----- = -------------- = --------------
		//   b + v   (b + v)(b - v)        b^2
		//
		// which holds because v*v = 0.
		const T g_a_inverse = T(1.0) / g.f();
		const T f_a_by_g_a = f.f() * g_a_inverse;
		return{ f_a_by_g_a, (f.grad() - f_a_by_g_a * g.grad()) * g_a_inverse };
	}

	// Binary / with a scalar: s / x
	template<typename T, int N> inline
		Dual_Number<T, N> operator/(T s, const Dual_Number<T, N>& g) {
		const T minus_s_g_a_inverse2 = -s / (g.f() * g.f());
		return{ s / g.f(), g.grad() * minus_s_g_a_inverse2 };
	}

	// Binary / with a scalar: x / s
	template<typename T, int N> inline
		Dual_Number<T, N> operator/(const Dual_Number<T, N>& f, T s) {
		const T s_inverse = 1.0 / s;
		return{ f.f() * s_inverse, f.grad() * s_inverse };
	}

	// Binary comparison operators for both scalars and jets.
#define DEFINE_DN_COMPARISON_OPERATOR(op) \
	template<typename T, int N> inline \
	bool operator op(const T& s, const Dual_Number<T, N>& g) { \
	  return s op g.f(); \
	} \
	template<typename T, int N> inline \
	bool operator op(const Dual_Number<T, N>& f, const T& s) { \
	  return f.f() op s; \
	} \
	template<typename T, int N> inline \
	bool operator op(int s, const Dual_Number<T, N>& g) { \
	  return s op g.f(); \
	} \
	template<typename T, int N> inline \
	bool operator op(const Dual_Number<T, N>& f, int s) { \
	  return f.f() op s; \
	}
	DEFINE_DN_COMPARISON_OPERATOR(< );
	DEFINE_DN_COMPARISON_OPERATOR(<= );
	DEFINE_DN_COMPARISON_OPERATOR(> );
	DEFINE_DN_COMPARISON_OPERATOR(>= );
	DEFINE_DN_COMPARISON_OPERATOR(== );
	DEFINE_DN_COMPARISON_OPERATOR(!= );

	/*template<typename T, int N> inline \
	bool operator op(const Dual_Number<T, N>& f, const Dual_Number<T, N>& g) { \
	return f.f() op g.f(); \
	} \*/

	template<typename T, int N> inline
		bool operator <(const Dual_Number<T, N>& f, const Dual_Number<T, N>& g) {
		if (f.f() < g.f())
			return true;
		if (f.f() == g.f() && std::lexicographical_compare(&f.grad()[0], &f.grad()[0] + N, &g.grad()[0], &g.grad()[0] + N))
			return true;
		return false;
	}

	template<typename T, int N> inline
		bool operator >(const Dual_Number<T, N>& f, const Dual_Number<T, N>& g) {
		return g < f;
	}

	template<typename T, int N> inline
		bool operator ==(const Dual_Number<T, N>& f, const Dual_Number<T, N>& g) {
		return f.f() == g.f() && std::equal(&f.grad()[0], &f.grad()[0] + N, &g.grad()[0], &g.grad()[0] + N);
	}

#undef DEFINE_DN_COMPARISON_OPERATOR

	using std::abs;
	using std::acos;
	using std::asin;
	using std::atan;
	using std::atan2;
	using std::cbrt;
	using std::ceil;
	using std::cos;
	using std::cosh;
	using std::erf;
	using std::erfc;
	//using boost::math::erf_inv;
	//using boost::math::erfc_inv;
	using std::exp;
	using std::exp2;
	using std::floor;
	using std::fmax;
	using std::fmin;
	using std::hypot;
	using std::isfinite;
	using std::isinf;
	using std::isnan;
	using std::isnormal;
	using std::log;
	using std::log2;
	using std::pow;
	using std::sin;
	using std::sinh;
	using std::sqrt;
	using std::tan;
	using std::tanh;

	inline double normSq(double x) { return Square(x); }
	template<typename T, int N> inline
		T normSq(const Dual_Number<T, N>& f)
	{
		return normSq(f.f()) + f.grad().squaredNorm();
	}

#pragma region Solvers

	template <typename FUN, typename V>
	inline V bisection(const FUN& functor, const V& targetValue, const double epsilon = 1e-12, const int max_iter = 1000)
	{
		// Guess plausible upper and lower bound
		V lowerbound = targetValue, upbound = targetValue;
		while (functor(lowerbound) > targetValue) lowerbound /= V(1.05);
		while (functor(upbound) < targetValue) upbound *= V(1.05);

		// Perform a bisection until epsilon accuracy is not reached
		for (int i = 0; i < max_iter && (epsilon < upbound - lowerbound); ++i)
			//while (epsilon < upbound - lowerbound)
		{
			const V mid = V(.5) * (lowerbound + upbound);
			if (functor(mid) > targetValue)
				upbound = mid;
			else
				lowerbound = mid;
		}
		return .5 * (lowerbound + upbound);
	}

	template <typename FUN, typename T, int N>
	inline Dual_Number<T, N> bisection(const FUN& functor, const Dual_Number<T, N>& targetValue, const double epsilon = 1e-12, const int max_iter = 1000)
	{
		T sourceValue = bisection(functor, targetValue.f(), epsilon, max_iter);

		auto functor_derivative = functor.derivative(sourceValue);

		return{
			/*f */ sourceValue,
			/*f'*/ targetValue.grad() / functor_derivative // inverse function theorem
		};
	}

#pragma endregion

	template<typename T>
	struct Ops {
		static bool IsScalar() {
			return true;
		}
		static const T& GetScalar(const T& t) {
			return t;
		}
		static void SetScalar(const T& scalar, T& t) {
			t = scalar;
		}
		//static void ScaleDerivative(double scale_by, const T& value) {
		//	// For double, there is no derivative to scale.
		//	(void)scale_by;  // Ignored.
		//	(void)value;  // Ignored.
		//}
		/*template<int N, int M>
		static void ChainRule(const Eigen::Matrix<T, M, N>& fx, T& dx)
		{
			(void)fx;
			(void)dx;
		}*/
		static T ChainRule(const T& fx, const T& x)
		{
			(void)x;
			return fx;
		}
	};

	template<typename T, int N>
	struct Ops<Dual_Number<T, N> > {
		static bool IsScalar() {
			return false;
		}
		static const T& GetScalar(const Dual_Number<T, N>& t) {
			return t.f();
		}
		static void SetScalar(const T& scalar, Dual_Number<T, N>& t) {
			t.f() = scalar;
		}
		//static void ScaleDerivative(double scale_by, const Dual_Number<T, N>& value) {
		//	value->v *= scale_by;
		//}
		/*template<int M>
		static void ChainRule(const Eigen::Matrix<T, M, N>& fx, T& dx)
		{
			(void)fx;
			(void)dx;
		}*/
		static Dual_Number<T, N> ChainRule(const Dual_Number<T, N>& fx, const Dual_Number<T, N>& dx)
		{
			return{
				fx.f(),
				fx.grad().array() * dx.grad().array() // dfxdx
			};
		}
	};

#pragma region Regular math functions

	// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule.

	// abs(x + h) ~= x + h or -(x + h)
	template <typename T, int N> inline
		Dual_Number<T, N> abs(const Dual_Number<T, N>& f) {
		return f.f() < T(0.0) ? -f : f;
	}

	// log(a + h) ~= log(a) + h / a
	template <typename T, int N> inline
		Dual_Number<T, N> log(const Dual_Number<T, N>& f) {
		const T a_inverse = T(1.0) / f.f();
		return{ log(f.f()), f.grad() * a_inverse };
	}

	// exp(a + h) ~= exp(a) + exp(a) h
	template <typename T, int N> inline
		Dual_Number<T, N> exp(const Dual_Number<T, N>& f) {
		const T tmp = exp(f.f());
		return{ tmp, tmp * f.grad() };
	}

	// http://mathworld.wolfram.com/Erf.html
	template <typename T, int N> inline
		Dual_Number<T, N> erf(const Dual_Number<T, N>& f) {
		return{ erf(f.f()), ((2 / sqrt(M_PI)) * exp(-Square(f.f()))) * f.grad() };
	}

	// http://mathworld.wolfram.com/Erfc.html
	template <typename T, int N> inline
		Dual_Number<T, N> erfc(const Dual_Number<T, N>& f) {
		return{ erfc(f.f()), ((-2 / sqrt(M_PI)) * exp(-Square(f.f()))) * f.grad() };
	}

	/*// http://mathworld.wolfram.com/InverseErf.html
	template <typename T, int N> inline
		Dual_Number<T, N> erf_inv(const Dual_Number<T, N>& f) {
		const T tmp = erf_inv(f.f());
		return{ tmp, ((sqrt(M_PI)/2.0)*exp(Square(tmp))) * f.grad() };
	}

	// http://mathworld.wolfram.com/InverseErfc.html
	template <typename T, int N> inline
		Dual_Number<T, N> erfc_inv(const Dual_Number<T, N>& f) {
		const T tmp = erfc_inv(f.f());
		return{ tmp, ((sqrt(M_PI) / 2.0)*exp(Square(tmp))) * f.grad() };
	}*/

	// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a))
	template <typename T, int N> inline
		Dual_Number<T, N> sqrt(const Dual_Number<T, N>& f) {
		const T tmp = sqrt(f.f());
		const T two_a_inverse = T(1.0) / (T(2.0) * tmp);
		return{ tmp, f.grad() * two_a_inverse };
	}

	// cos(a + h) ~= cos(a) - sin(a) h
	template <typename T, int N> inline
		Dual_Number<T, N> cos(const Dual_Number<T, N>& f) {
		return{ cos(f.f()), -sin(f.f()) * f.grad() };
	}

	// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h
	template <typename T, int N> inline
		Dual_Number<T, N> acos(const Dual_Number<T, N>& f) {
		const T tmp = -T(1.0) / sqrt(T(1.0) - f.f() * f.f());
		return{ acos(f.f()), tmp * f.grad() };
	}

	// sin(a + h) ~= sin(a) + cos(a) h
	template <typename T, int N> inline
		Dual_Number<T, N> sin(const Dual_Number<T, N>& f) {
		return{ sin(f.f()), cos(f.f()) * f.grad() };
	}

	// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h
	template <typename T, int N> inline
		Dual_Number<T, N> asin(const Dual_Number<T, N>& f) {
		const T tmp = T(1.0) / sqrt(T(1.0) - f.f() * f.f());
		return{ asin(f.f()), tmp * f.grad() };
	}

	// tan(a + h) ~= tan(a) + (1 + tan(a)^2) h
	template <typename T, int N> inline
		Dual_Number<T, N> tan(const Dual_Number<T, N>& f) {
		const T tan_a = tan(f.f());
		const T tmp = T(1.0) + tan_a * tan_a;
		return{ tan_a, tmp * f.grad() };
	}

	// atan(a + h) ~= atan(a) + 1 / (1 + a^2) h
	template <typename T, int N> inline
		Dual_Number<T, N> atan(const Dual_Number<T, N>& f) {
		const T tmp = T(1.0) / (T(1.0) + f.f() * f.f());
		return{ atan(f.f()), tmp * f.grad() };
	}

	// sinh(a + h) ~= sinh(a) + cosh(a) h
	template <typename T, int N> inline
		Dual_Number<T, N> sinh(const Dual_Number<T, N>& f) {
		return{ sinh(f.f()), cosh(f.f()) * f.grad() };
	}

	// cosh(a + h) ~= cosh(a) + sinh(a) h
	template <typename T, int N> inline
		Dual_Number<T, N> cosh(const Dual_Number<T, N>& f) {
		return{ cosh(f.f()), sinh(f.f()) * f.grad() };
	}

	// tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h
	template <typename T, int N> inline
		Dual_Number<T, N> tanh(const Dual_Number<T, N>& f) {
		const T tanh_a = tanh(f.f());
		const T tmp = T(1.0) - tanh_a * tanh_a;
		return{ tanh_a, tmp * f.grad() };
	}

	// The floor function should be used with extreme care as this operation will
	// result in a zero derivative which provides no information to the solver.
	//
	// floor(a + h) ~= floor(a) + 0
	template <typename T, int N> inline
		Dual_Number<T, N> floor(const Dual_Number<T, N>& f) {
		return Dual_Number<T, N>(floor(f.f()));
	}

	// The ceil function should be used with extreme care as this operation will
	// result in a zero derivative which provides no information to the solver.
	//
	// ceil(a + h) ~= ceil(a) + 0
	template <typename T, int N> inline
		Dual_Number<T, N> ceil(const Dual_Number<T, N>& f) {
		return Dual_Number<T, N>(ceil(f.f()));
	}

	// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2)
	//
	// In words: the rate of change of theta is 1/r times the rate of
	// change of (x, y) in the positive angular direction.
	template <typename T, int N> inline
		Dual_Number<T, N> atan2(const Dual_Number<T, N>& g, const Dual_Number<T, N>& f) {
		// Note order of arguments:
		//
		//   f = a + da
		//   g = b + db

		T const tmp = T(1.0) / (f.f() * f.f() + g.f() * g.f());
		return{ atan2(g.f(), f.f()), tmp * (-g.f() * f.grad() + f.f() * g.grad()) };
	}

	// pow -- base is a differentiable function, exponent is a constant.
	// (a+da)^p ~= a^p + p*a^(p-1) da
	template <typename T, int N> inline
		Dual_Number<T, N> pow(const Dual_Number<T, N>& f, double g) {
		T const tmp = g * pow(f.f(), g - T(1.0));
		return{ pow(f.f(), g), tmp * f.grad() };
	}

	/*
	// pow -- base is a constant, exponent is a differentiable function.
	// (a)^(p+dp) ~= a^p + a^p log(a) dp
	template <typename T, int N> inline
	Dual_Number<T, N> pow(double f, const Dual_Number<T, N>& g) {
	T const tmp = pow(f, g.f());
	return{ tmp, log(f) * tmp * g.grad() };
	}

	// pow -- both base and exponent are differentiable functions.
	// (a+da)^(b+db) ~= a^b + b * a^(b-1) da + a^b log(a) * db
	template <typename T, int N> inline
	Dual_Number<T, N> pow(const Dual_Number<T, N>& f, const Dual_Number<T, N>& g) {
	T const tmp1 = pow(f.f(), g.f());
	T const tmp2 = g.f() * pow(f.f(), g.f() - T(1.0));
	T const tmp3 = tmp1 * log(f.f());

	return{ tmp1, tmp2 * f.grad() + tmp3 * g.grad() };
	}*/

	// pow -- base is a constant, exponent is a differentiable function.
	// We have various special cases, see the comment for pow(Jet, Jet) for
	// analysis:
	//
	// 1. For f > 0 we have: (f)^(g + dg) ~= f^g + f^g log(f) dg
	//
	// 2. For f == 0 and g > 0 we have: (f)^(g + dg) ~= f^g
	//
	// 3. For f < 0 and integer g we have: (f)^(g + dg) ~= f^g but if dg
	// != 0, the derivatives are not defined and we return NaN.

	template <typename T, int N> inline
		Dual_Number<T, N> pow(double f, const Dual_Number<T, N>& g) {
		if (f == 0 && g.f() > 0) {
			// Handle case 2.
			return Dual_Number<T, N>(T(0.0));
		}
		if (f < 0 && g.f() == floor(g.f())) {
			// Handle case 3.
			Dual_Number<T, N> ret(pow(f, g.f()));
			for (int i = 0; i < N; i++) {
				if (g.grad()[i] != T(0.0)) {
					// Return a NaN when g.grad() != 0.
					ret.grad()[i] = std::numeric_limits<T>::quiet_NaN();
				}
			}
			return ret;
		}
		// Handle case 1.
		T const tmp = pow(f, g.f());
		return{ tmp, log(f) * tmp * g.grad() };
	}

	// pow -- both base and exponent are differentiable functions. This has a
	// variety of special cases that require careful handling.
	//
	// 1. For f > 0:
	//    (f + df)^(g + dg) ~= f^g + f^(g - 1) * (g * df + f * log(f) * dg)
	//    The numerical evaluation of f * log(f) for f > 0 is well behaved, even for
	//    extremely small values (e.g. 1e-99).
	//
	// 2. For f == 0 and g > 1: (f + df)^(g + dg) ~= 0
	//    This cases is needed because log(0) can not be evaluated in the f > 0
	//    expression. However the function f*log(f) is well behaved around f == 0
	//    and its limit as f-->0 is zero.
	//
	// 3. For f == 0 and g == 1: (f + df)^(g + dg) ~= 0 + df
	//
	// 4. For f == 0 and 0 < g < 1: The value is finite but the derivatives are not.
	//
	// 5. For f == 0 and g < 0: The value and derivatives of f^g are not finite.
	//
	// 6. For f == 0 and g == 0: The C standard incorrectly defines 0^0 to be 1
	//    "because there are applications that can exploit this definition". We
	//    (arbitrarily) decree that derivatives here will be nonfinite, since that
	//    is consistent with the behavior for f == 0, g < 0 and 0 < g < 1.
	//    Practically any definition could have been justified because mathematical
	//    consistency has been lost at this point.
	//
	// 7. For f < 0, g integer, dg == 0: (f + df)^(g + dg) ~= f^g + g * f^(g - 1) df
	//    This is equivalent to the case where f is a differentiable function and g
	//    is a constant (to first order).
	//
	// 8. For f < 0, g integer, dg != 0: The value is finite but the derivatives are
	//    not, because any change in the value of g moves us away from the point
	//    with a real-valued answer into the region with complex-valued answers.
	//
	// 9. For f < 0, g noninteger: The value and derivatives of f^g are not finite.

	template <typename T, int N> inline
		Dual_Number<T, N> pow(const Dual_Number<T, N>& f, const Dual_Number<T, N>& g) {
		if (f.f() == 0.0 && g.f() >= 1.0) {
			// Handle cases 2 and 3.
			if (g.f() > 1.0) {
				return Dual_Number<T, N>(T(0.0));
			}
			return f;
		}
		if (f.f() < 0.0 && g.f() == floor(g.f())) {
			// Handle cases 7 and 8.
			T const tmp = g.f() * pow(f.f(), g.f() - T(1.0));
			Dual_Number<T, N> ret(pow(f.f(), g.f()), tmp * f.grad());
			for (int i = 0; i < N; i++) {
				if (g.grad()[i] != T(0.0)) {
					// Return a NaN when g.grad() != 0.
					ret.grad()[i] = std::numeric_limits<T>::quiet_NaN();
				}
			}
			return ret;
		}
		// Handle the remaining cases. For cases 4,5,6,9 we allow the log() function
		// to generate -HUGE_VAL or NaN, since those cases result in a nonfinite
		// derivative.
		T const tmp1 = pow(f.f(), g.f());
		T const tmp2 = g.f() * pow(f.f(), g.f() - T(1.0));
		T const tmp3 = tmp1 * log(f.f());
		return Dual_Number<T, N>(tmp1, tmp2 * f.grad() + tmp3 * g.grad());
	}

#pragma endregion

#pragma region Bessel

	// Bessel functions of the first kind with integer order equal to 0, 1, n.
	//
	// Microsoft has deprecated the j[0,1,n]() POSIX Bessel functions in favour of
	// _j[0,1,n]().  Where available on MSVC, use _j[0,1,n]() to avoid deprecated
	// function errors in client code (the specific warning is suppressed when
	// Ceres itself is built).
	inline double BesselJ0(double x) {
#if defined(_MSC_VER) && _MSC_VER >= 1800
		return _j0(x);
#else
		return j0(x);
#endif
	}
	inline double BesselJ1(double x) {
#if defined(_MSC_VER) && _MSC_VER >= 1800
		return _j1(x);
#else
		return j1(x);
#endif
	}
	inline double BesselJn(int n, double x) {
#if defined(_MSC_VER) && _MSC_VER >= 1800
		return _jn(n, x);
#else
		return jn(n, x);
#endif
	}

	// For the formulae of the derivatives of the Bessel functions see the book:
	// Olver, Lozier, Boisvert, Clark, NIST Handbook of Mathematical Functions,
	// Cambridge University Press 2010.
	//
	// Formulae are also available at http://dlmf.nist.gov

	// See formula http://dlmf.nist.gov/10.6#E3
	// j0(a + h) ~= j0(a) - j1(a) h
	template <typename T, int N> inline
		Dual_Number<T, N> BesselJ0(const Dual_Number<T, N>& f) {
		return{ BesselJ0(f.f()), -BesselJ1(f.f()) * f.grad() };
	}

	// See formula http://dlmf.nist.gov/10.6#E1
	// j1(a + h) ~= j1(a) + 0.5 ( j0(a) - j2(a) ) h
	template <typename T, int N> inline
		Dual_Number<T, N> BesselJ1(const Dual_Number<T, N>& f) {
		return{ BesselJ1(f.f()),
			T(0.5) * (BesselJ0(f.f()) - BesselJn(2, f.f())) * f.grad() };
	}

	// See formula http://dlmf.nist.gov/10.6#E1
	// j_n(a + h) ~= j_n(a) + 0.5 ( j_{n-1}(a) - j_{n+1}(a) ) h
	template <typename T, int N> inline
		Dual_Number<T, N> BesselJn(int n, const Dual_Number<T, N>& f) {
		return{ BesselJn(n, f.f()),
			T(0.5) * (BesselJn(n - 1, f.f()) - BesselJn(n + 1, f.f())) * f.grad() };
	}

#pragma endregion

#pragma region Dual Number classification
	// Classification. It is not clear what the appropriate semantics are for
	// these classifications. This picks that IsFinite and isnormal are "all"
	// operations, i.e. all elements of the jet must be finite for the jet itself
	// to be finite (or normal). For IsNaN and IsInfinite, the answer is less
	// clear. This takes a "any" approach for IsNaN and IsInfinite such that if any
	// part of a jet is nan or inf, then the entire jet is nan or inf. This leads
	// to strange situations like a jet can be both IsInfinite and IsNaN, but in
	// practice the "any" semantics are the most useful for e.g. checking that
	// derivatives are sane.

	// The jet is finite if all parts of the jet are finite.
	template <typename T, int N> inline
		bool isfinite(const Dual_Number<T, N>& f) {
		if (!isfinite(f.f())) {
			return false;
		}
		for (int i = 0; i < N; ++i) {
			if (!isfinite(f.grad()[i])) {
				return false;
			}
		}
		return true;
	}

	// The jet is infinite if any part of the jet is infinite.
	template <typename T, int N> inline
		bool isinf(const Dual_Number<T, N>& f) {
		if (isinf(f.f())) {
			return true;
		}
		for (int i = 0; i < N; i++) {
			if (isinf(f.grad()[i])) {
				return true;
			}
		}
		return false;
	}

	// The jet is NaN if any part of the jet is NaN.
	template <typename T, int N> inline
		bool isnan(const Dual_Number<T, N>& f) {
		if (isnan(f.f())) {
			return true;
		}
		for (int i = 0; i < N; ++i) {
			if (isnan(f.grad()[i])) {
				return true;
			}
		}
		return false;
	}

	// The jet is normal if all parts of the jet are normal.
	template <typename T, int N> inline
		bool isnormal(const Dual_Number<T, N>& f) {
		if (!isnormal(f.f())) {
			return false;
		}
		for (int i = 0; i < N; ++i) {
			if (!isnormal(f.grad()[i])) {
				return false;
			}
		}
		return true;
	}
#pragma endregion

#pragma region Differentiation

	template <typename T, int N, int M, typename FUN>
	void gradient(
		const FUN& OP,
		const Eigen::Matrix<T, N, 1>& param,
		Eigen::Matrix<T, M, 1>& value,
		Eigen::Matrix<T, M, N>& grad)
	{
		Eigen::Matrix<Dual_Number<T, N>, N, 1> dn_param;
		for (auto i = 0; i < N; ++i)
		{
			dn_param[i].setPartial(param[i], i);
		}

		Eigen::Matrix<Dual_Number<T, N>, M, 1> f = OP(dn_param);

		for (auto i = 0; i < M; ++i)
		{
			value[i] = f[i].f();
			grad.row(i) = f[i].grad();
		}
	}

	template <typename V, typename FUN>
	void derivative(const FUN& OP, const V& param, V& value, V& grad)
	{
		Dual_Number<V, 1> dn_param;
		dn_param.setPartial(param, 0);
		Dual_Number<V, 1> f = OP(dn_param);
		value = f.f();
		grad = f.grad()[0];
	}

	template <typename V, typename FUN>
	V derivative(const FUN& OP, const V& param)
	{
		Dual_Number<V, 1> dn_param(param, 0);
		Dual_Number<V, 1> f = OP(dn_param);
		return f.grad()[0];
	}

#define DEFINE_REAL_DERIVATIVE \
	template <typename T> \
	inline T derivative(const T& x) const \
	{ \
		return common::derivative(*this, x); \
	}

#define DEFINE_VECTOR_GRADIENT(N,M) \
	template <typename T> \
	inline Eigen::Matrix<T, M, N> gradient(const Eigen::Matrix<T, N, 1>& param) const \
	{ \
		Eigen::Matrix<common::Dual_Number<T, N>, N, 1> dn_param; \
		for (int i = 0; i < N; ++i) \
			dn_param[i].setPartial(param[i], i); \
		Eigen::Matrix<common::Dual_Number<T, N>, M, 1> f_ = this->operator()(dn_param);  \
		Eigen::Matrix<T, M, N> grad; \
		for (int i = 0; i < M; ++i) \
			grad.row(i) = f_[i].grad(); \
		return grad; \
	}

#define DIFFERENTIATE(FUN,N,M) \
	template <typename T> \
	inline std::pair<Eigen::Matrix<T, M, 1>, Eigen::Matrix<T, M, N>> FUN ## _gradient(const Eigen::Matrix<T, N, 1>& param) const \
	{ \
		Eigen::Matrix<common::Dual_Number<T, N>, N, 1> dn_param; \
		for (int i = 0; i < N; ++i) \
			dn_param[i].setPartial(param[i], i); \
		Eigen::Matrix<common::Dual_Number<T, N>, M, 1> f_ = FUN(dn_param);  \
		Eigen::Matrix<T, M, N> grad; \
		Eigen::Matrix<T, M, 1> val; \
		for (int i = 0; i < M; ++i) { \
			grad.row(i) = f_[i].grad(); \
			val(i) = f_[i].f();\
		} \
		return { val, grad }; \
	}

#define DIFFERENTIATE_VECTOR_SCALAR(FUN,N) \
	template <typename T> \
	inline std::pair<T, Eigen::Matrix<T, 1, N>> FUN ## _gradient(const Eigen::Matrix<T, N, 1>& param) const \
	{ \
		Eigen::Matrix<common::Dual_Number<T, N>, N, 1> dn_param; \
		for (int i = 0; i < N; ++i) \
			dn_param[i].setPartial(param[i], i); \
		common::Dual_Number<T, N> f_ = FUN(dn_param); \
		return { f_.f(), f_.grad() }; \
	}

#define DEFINE_SCALAR_VECTOR_GRADIENT(N) \
	template <typename T> \
	inline Eigen::Matrix<T, 1, N> gradient(const Eigen::Matrix<T, N, 1>& param) const \
	{ \
		Eigen::Matrix<common::Dual_Number<T, N>, N, 1> dn_param; \
		for (int i = 0; i < N; ++i) \
			dn_param[i].setPartial(param[i], i); \
		common::Dual_Number<T, N> f_ = this->operator()(dn_param);  \
		return f_.grad(); \
	}\
	\
	template <typename T> \
	inline common::Dual_Number<T, N> value_and_gradient(const Eigen::Matrix<T, N, 1>& param) const \
	{ \
		Eigen::Matrix<common::Dual_Number<T, N>, N, 1> dn_param; \
		for (int i = 0; i < N; ++i) \
			dn_param[i].setPartial(param[i], i); \
		return this->operator()(dn_param);  \
	}

#define OPERATOR_SCALAR_SCALAR(EXPRESSION) \
	template <typename T> T operator()(const T& x) const { \
		return EXPRESSION; \
	}

#define OPERATOR_VECTOR_VECTOR(N, M, EXPRESSION) \
	template <typename T> \
	Eigen::Matrix<T,M,1> operator()(const Eigen::Matrix<T,N,1>& x) const { \
		return EXPRESSION; \
	}

#define OPERATOR_VECTOR_VECTOR_BLOCK(N, M, EXPRESSION) \
	template <typename T> \
	Eigen::Matrix<T,M,1> operator()(const Eigen::Matrix<T,N,1>& x) const { \
		EXPRESSION\
	}

#define DIFFERENTIABLE(TYPENAME, EXPRESSION) \
struct TYPENAME { \
	OPERATOR_SCALAR_SCALAR(EXPRESSION) \
	DEFINE_REAL_DERIVATIVE\
}

#define DIFFERENTIABLE_VECTOR_VECTOR(TYPENAME, N, M, EXPRESSION) \
struct TYPENAME { \
	OPERATOR_VECTOR_VECTOR(N, M, EXPRESSION) \
	DEFINE_VECTOR_GRADIENT(N,M)\
}

#pragma endregion

	template <typename T, int N>
	inline std::ostream &operator<<(std::ostream &s, const Dual_Number<T, N>& z) {
		return s << "[" << z.f() << " ; " << z.grad().transpose() << "]";
	}
}

namespace Eigen {

	// Creating a specialization of NumTraits enables placing Dual_Number objects inside
	// Eigen arrays, getting all the goodness of Eigen combined with autodiff.
	template<typename T, int N>
	struct NumTraits<common::Dual_Number<T, N> > {
		typedef common::Dual_Number<T, N> Real;
		typedef common::Dual_Number<T, N> NonInteger;
		typedef common::Dual_Number<T, N> Nested;
		typedef common::Dual_Number<T, N> Literal;

		static inline common::Dual_Number<T, N> dummy_precision() {
			//return common::Dual_Number<T, N>(1e-12);
			return Real(NumTraits<T>::dummy_precision());
		}

		static inline Real epsilon() {
			//return Real(std::numeric_limits<T>::epsilon());
			return Real(NumTraits<T>::epsilon());
		}

		enum {
			IsComplex = NumTraits<T>::IsComplex,
			IsInteger = NumTraits<T>::IsInteger,
			IsSigned = NumTraits<T>::IsSigned,
			ReadCost = (N + 1) * NumTraits<T>::ReadCost,
			AddCost = (N + 1) * NumTraits<T>::AddCost,
			MulCost = N * NumTraits<T>::AddCost + (2 * N + 1) * NumTraits<T>::MulCost,
			HasFloatingPoint = 1,
			RequireInitialization = 1
		};

		template<bool Vectorized>
		struct Div {
			enum {
#define EIGEN_VECTORIZE_AVX // TODO
#if defined(EIGEN_VECTORIZE_AVX)
				AVX = true,
#else
				AVX = false,
#endif

				// Assuming that for Jets, division is as expensive as
				// multiplication.
				Cost = 3
			};
		};
	};

}  // namespace Eigen


/*
 * Copyright 2014, Andrea Del Prete, LAAS-CNRS
 *
 */

#ifndef __sot_torque_control_NumericalDifference_H__
#define __sot_torque_control_NumericalDifference_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

#if defined(WIN32)
#if defined(numerical_difference_EXPORTS)
#define SOTNUMERICALDIFFERENCE_EXPORT __declspec(dllexport)
#else
#define SOTNUMERICALDIFFERENCE_EXPORT __declspec(dllimport)
#endif
#else
#define SOTNUMERICALDIFFERENCE_EXPORT
#endif

//#define VP_DEBUG 1        /// enable debug output
//#define VP_DEBUG_MODE 20

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* HELPER */
#include <boost/circular_buffer.hpp>
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
#include <sot/core/stop-watch.hh>

/* Polynomial estimators */
#include <sot/torque_control/utils/lin-estimator.hh>
#include <sot/torque_control/utils/quad-estimator.hh>

namespace dynamicgraph {
namespace sot {
namespace torque_control {

/**
 * This Entity takes as inputs a signal and estimates its first two time derivatives.
 */
class SOTNUMERICALDIFFERENCE_EXPORT NumericalDifference : public ::dynamicgraph::Entity {
  DYNAMIC_GRAPH_ENTITY_DECL();

 public: /* --- SIGNALS --- */
  DECLARE_SIGNAL_IN(x, dynamicgraph::Vector);
  DECLARE_SIGNAL_OUT(x_filtered, dynamicgraph::Vector);
  DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
  DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);

  /// The following inner signals are used because this entity has some output signals
  /// whose related quantities are computed at the same time by the same algorithm
  /// To avoid the risk of recomputing the same things twice, we create an inner signal that groups together
  /// all the quantities that are computed together. Then the single output signals will depend
  /// on this inner signal, which is the one triggering the computations.
  /// Inner signals are not exposed, so that nobody can access them.

  /// This signal contains the estimated positions, velocities and accelerations.
  DECLARE_SIGNAL_INNER(x_dx_ddx, dynamicgraph::Vector);

 protected:
  double m_dt;     /// sampling timestep of the input signal
  double m_delay;  /// delay introduced by the estimation
  int x_size;
  /// std::vector to use with the filters
  /// All the variables whose name contains 'filter' are outputs of the filters
  std::vector<double> m_ddx_filter_std;  /// 2nd derivative
  std::vector<double> m_dx_filter_std;   /// 1st derivative
  std::vector<double> m_x_filter_std;    /// filtered output
  std::vector<double> m_x_std;           /// x signal

  /// polynomial-fitting filters
  PolyEstimator* m_filter;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /** --- CONSTRUCTOR ---- */
  NumericalDifference(const std::string& name);

  /** Initialize the NumericalDifference.
   * @param timestep Period (in seconds) after which the sensors' data are updated.
   * @param sigSize  Size of the input signal.
   * @param delay    Delay (in seconds) introduced by the estimation.
   *                 This should be a multiple of timestep.
   * @param polyOrder Order of the approximation polynomial. int 1 or 2.
   * @note The estimationDelay is half of the length of the window used for the
   * polynomial fitting. The larger the delay, the smoother the estimations.
   */
  void init(const double& timestep, const int& sigSize, const double& delay, const int& polyOrder);

 protected:
  void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* = "", int = 0) {
    logger_.stream(t) << ("[" + name + "] " + msg) << '\n';
  }

 public: /* --- ENTITY INHERITANCE --- */
  virtual void display(std::ostream& os) const;

};  // class NumericalDifference

}  // namespace torque_control
}  // namespace sot
}  // namespace dynamicgraph

#endif  // #ifndef __sot_torque_control_NumericalDifference_H__


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#include <boost/multiprecision/cpp_dec_float.hpp>

using namespace boost::multiprecision;

int main()
{
   cpp_dec_float<50> a;
}


#include <boost/tuple/tuple.hpp>
#include <boost/array.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/irange.hpp>
#include <boost/bind.hpp>

#include <Mesh.h>
#include <ScalarTransportEquation.h>
#include <CrankNicolson.h>
#include <FirstOrderExplicit.h>

int main()
{
	const int MESH_SIZE = 32;
	const double CFL = 0.8;  //Courant–Friedrichs–Lewy condition
	int n;
	std::cout
			<< "Enter 1 for Crank Nicolson; 2 for First Order Explicit for time integration"
			<< std::endl;
	std::cin >> n;
	auto analysis =
			(n == 1) ?
					dynamic_cast<Analysis*>(new CrankNicolson(MESH_SIZE,
							MESH_SIZE)) :
					dynamic_cast<Analysis*>(new FirstOrderExplicit(MESH_SIZE,
							MESH_SIZE));

	auto result = analysis->Analyze(CFL);
	delete analysis;
	/* Plot scalar with time*/

	Gnuplot gp;

	gp << "set key top" << std::endl;
	gp << "set view map" << std::endl;
	gp << "set xrange [0:1]" << std::endl;
	gp << "set yrange [0:1]" << std::endl;
	gp << "set cbrange [-0.15:0.15]" << std::endl;
	gp << "set contour base" << std::endl;
	gp << "set cntrparam levels incr -0.15,0.02,0.15 " << std::endl;
	gp << "set multiplot layout 2,2 rowsfirst title 'Analysis results'"
			<< std::endl;

	double output_freq = Mesh::tf / 4.;
	double i = 1;
	for (auto plot : result)
	{
		gp << "set title 't = " << std::fixed << std::setprecision(2)
				<< (i++) * output_freq << "s'" << std::endl;
		gp << "plot '-' w image" << std::endl;
		gp.send1d(plot);
	}
}


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

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

#define ENABLE_GTEST 1
#define ENABLE_DEBUGGING 0
#define UDD 1

//#include "common/stdafx.h"
#include <vector>
//#include <array>
#include "bolt/cl/bolt.h"
//#include "bolt/cl/scan.h"
#include "bolt/cl/functional.h"
#include "bolt/cl/reduce_by_key.h"
#include "bolt/unicode.h"
#include "bolt/miniDump.h"

#include <gtest/gtest.h>
//#include <boost/shared_array.hpp>

#include <boost/program_options.hpp>
#include "test_common.h"

#if !ENABLE_GTEST

template<
    typename InputIterator1,
    typename InputIterator2,
    typename OutputIterator1,
    typename OutputIterator2,
    typename BinaryFunction>
std::pair<OutputIterator1, OutputIterator2>
gold_reduce_by_key(
    InputIterator1 keys_first,
    InputIterator1 keys_last,
    InputIterator2 values_first,
    OutputIterator1 keys_output,
    OutputIterator2 values_output,
    BinaryFunction binary_op)
{
    typedef std::iterator_traits< InputIterator1 >::value_type kType;
    typedef std::iterator_traits< InputIterator2 >::value_type vType;
    typedef std::iterator_traits< OutputIterator1 >::value_type koType;
    typedef std::iterator_traits< OutputIterator2 >::value_type voType;
       static_assert( std::is_convertible< vType, voType >::value,
        "InputIterator2 and OutputIterator's value types are not convertible." );

   unsigned int numElements = static_cast< unsigned int >( std::distance( keys_first, keys_last ) );

    // do zeroeth element
    *values_output = *values_first;
    *keys_output = *keys_first;
    unsigned int count = 1;
    // rbk oneth element and beyond

    values_first++;
    for ( InputIterator1 key = (keys_first+1); key != keys_last; key++)
    {
        // load keys
        kType currentKey  = *(key);
        kType previousKey = *(key-1);

        // load value
        voType currentValue = *values_first;
        voType previousValue = *values_output; //Sure?: Damn sure

        previousValue = *values_output;
        // within segment
        if (currentKey == previousKey)
        {
            //std::cout << "continuing segment" << std::endl;
            voType r = binary_op( previousValue, currentValue);
            *values_output = r;
            *keys_output = currentKey;

        }
        else // new segment
        {
            values_output++;
            keys_output++;
            *values_output = currentValue;
            *keys_output = currentKey;
            count++; //To count the number of elements in the output array
        }
        values_first++;
    }

    //return result;
    //Print values here: Temporary, things gonna change after pair()

    //std::cout<<"Number of ele = "<<count<<std::endl;

    return std::make_pair(keys_output, values_output);

}

int _tmain(int argc, _TCHAR* argv[])
{

    int length = 10000;
    std::vector< int > keys( length);
    // keys = {1, 2, 2, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5, 5,...}
    int segmentLength = 0;
    int segmentIndex = 0;
    int key = 1;
    std::vector< int > refInput( length );
    std::vector< int > input( length );
    for (int i = 0; i < length; i++)
    {
        if(std::rand()%3 == 1) key++;
        keys[i] = key;
        refInput[i] = std::rand()%4;
        input[i] = refInput[i];
    }

    // input and output vectors for device and reference

    std::vector< int > koutput( length );
    std::vector< int > voutput( length );
    //std::vector< int > refInput( length );
    std::vector< int > krefOutput( length );
    std::vector< int > vrefOutput( length );
    std::fill(krefOutput.begin(),krefOutput.end(),0);
    std::fill(vrefOutput.begin(),vrefOutput.end(),0);
    // call reduce_by_key

    bolt::cl::equal_to<int> binary_predictor;
    bolt::cl::plus<int> binary_operator;

    bolt::cl::reduce_by_key( keys.begin(), keys.end(), input.begin(), koutput.begin(), voutput.begin(), binary_predictor, binary_operator);
    //for(unsigned int i = 0; i < 256 ; i++)
    //{
    //    std::cout<<"Ikey "<<keys[i]<<" IValues "<<input[i]<<" -> OKeys "<<koutput[i]<<" OValues "<<voutput[i]<<std::endl;
    //}
    gold_reduce_by_key( keys.begin(), keys.end(), refInput.begin(), krefOutput.begin(), vrefOutput.begin(),std::plus<int>());

    cmpArrays(krefOutput, koutput);
    cmpArrays(vrefOutput, voutput);

    return 0;
}

#else

template<
    typename InputIterator1,
    typename InputIterator2,
    typename OutputIterator1,
    typename OutputIterator2,
    typename BinaryFunction>
std::pair<OutputIterator1, OutputIterator2>
gold_reduce_by_key( InputIterator1 keys_first,
                    InputIterator1 keys_last,
                    InputIterator2 values_first,
                    OutputIterator1 keys_output,
                    OutputIterator2 values_output,
                    BinaryFunction binary_op )
{
    typedef std::iterator_traits< InputIterator1 >::value_type kType;
    typedef std::iterator_traits< InputIterator2 >::value_type vType;
    typedef std::iterator_traits< OutputIterator1 >::value_type koType;
    typedef std::iterator_traits< OutputIterator2 >::value_type voType;
    static_assert( std::is_convertible< vType, voType >::value,
                   "InputIterator2 and OutputIterator's value types are not convertible." );

   unsigned int numElements = static_cast< unsigned int >( std::distance( keys_first, keys_last ) );

    // do zeroeth element
    *values_output = *values_first;
    *keys_output = *keys_first;
    unsigned int count = 1;
    // rbk oneth element and beyond

    values_first++;
    for ( InputIterator1 key = (keys_first+1); key != keys_last; key++)
    {
        // load keys
        kType currentKey  = *(key);
        kType previousKey = *(key-1);

        // load value
        voType currentValue = *values_first;
        voType previousValue = *values_output;

        previousValue = *values_output;
        // within segment
        if (currentKey == previousKey)
        {
            voType r = binary_op( previousValue, currentValue);
            *values_output = r;
            *keys_output = currentKey;

        }
        else // new segment
        {
            values_output++;
            keys_output++;
            *values_output = currentValue;
            *keys_output = currentKey;
            count++; //To count the number of elements in the output array
        }
        values_first++;
    }

    std::cout<<count<<std::endl;
    return std::make_pair(keys_output+1, values_output+1);
}

TEST(ReduceByKeyBasic, IntegerTest)
{
    int length = 1<<24;
    std::vector< int > keys( length);
    // keys = {1, 2, 2, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5, 5,...}
    int segmentLength = 0;
    int segmentIndex = 0;
    int key = 1;
    std::vector< int > refInput( length );
    std::vector< int > input( length );
    for (int i = 0; i < length; i++)
    {
        if(std::rand()%3 == 1) key++;
        keys[i] = key;
        refInput[i] = std::rand()%4;
        input[i] = refInput[i];
    }

    // input and output vectors for device and reference

    std::vector< int > koutput( length );
    std::vector< int > voutput( length );
    std::vector< int > krefOutput( length );
    std::vector< int > vrefOutput( length );
    std::fill(krefOutput.begin(),krefOutput.end(),0);
    std::fill(vrefOutput.begin(),vrefOutput.end(),0);

    bolt::cl::equal_to<int> binary_predictor;
    bolt::cl::plus<int> binary_operator;

    // call reduce_by_key
    auto p = bolt::cl::reduce_by_key( keys.begin(), keys.end(), input.begin(), koutput.begin(), voutput.begin(), binary_predictor, binary_operator);

#if 0

    for(unsigned int i = 0; i < 256 ; i++)
    {
        std::cout<<"Ikey "<<keys[i]<<" IValues "<<input[i]<<" -> OKeys "<<koutput[i]<<" OValues "<<voutput[i]<<std::endl;
    }

#endif

    auto refPair = gold_reduce_by_key( keys.begin(), keys.end(), refInput.begin(), krefOutput.begin(), vrefOutput.begin(),std::plus<int>());

    //cmpArrays2(krefOutput, koutput, refPair.first, p.first);
    cmpArrays(krefOutput, koutput);
    cmpArrays(vrefOutput, voutput);
   // cmpArrays2(vrefOutput, voutput, refPair.second, p.second);
}

TEST(ReduceByKeyPairCheck, IntegerTest2)
{
    int length = 1<<24;
    std::vector< int > keys( length);
    // keys = {1, 2, 2, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5, 5,...}
    int segmentLength = 0;
    int segmentIndex = 0;
    int key = 1;
    std::vector< int > refInput( length );
    std::vector< int > input( length );

    for (int i = 0; i < length; i++)
    {
        if(std::rand()%3 == 1) key++;
        keys[i] = key;
        refInput[i] = std::rand()%4;
        input[i] = refInput[i];
    }keys[1] = keys[2] = keys[3] = keys[4] =1;
    keys[5] = keys[6] = keys[7] = keys[8] =2; keys[9] = 3;

    // input and output vectors for device and reference

    std::vector< int > koutput( length );
    std::vector< int > voutput( length );
    std::vector< int > krefOutput( length );
    std::vector< int > vrefOutput( length );
    std::fill(krefOutput.begin(),krefOutput.end(),0);
    std::fill(vrefOutput.begin(),vrefOutput.end(),0);
    std::fill(koutput.begin(),koutput.end(),0);
    std::fill(voutput.begin(),voutput.end(),0);

    bolt::cl::equal_to<int> binary_predictor;
    bolt::cl::plus<int> binary_operator;

    typedef std::pair<std::vector<int>::iterator,std::vector<int>::iterator> StdPairIterator;
    typedef bolt::cl::pair<std::vector<int>::iterator,std::vector<int>::iterator> DevicePairIterator;

    // call reduce_by_key
    DevicePairIterator dv_pair =
    bolt::cl::reduce_by_key(
        keys.begin(),
        keys.end(),
        input.begin(),
        koutput.begin(),
        voutput.begin(),
        binary_predictor,
        binary_operator);

#if 0

    for(unsigned int i = 0; i < length ; i++)
    {
        std::cout<<"Ikey "<<keys[i]<<" IValues "<<input[i]<<" -> OKeys "<<koutput[i]<<" OValues "<<voutput[i]<<std::endl;
    }

#endif

    DevicePairIterator gold_pair =
    gold_reduce_by_key( keys.begin(), keys.end(), refInput.begin(), krefOutput.begin(), vrefOutput.begin(),std::plus<int>());

    size_t sizeAfterCall = gold_pair.first - krefOutput.begin();
    size_t sizeAfterDeviceCall = dv_pair.first - koutput.begin();

    std::cout<<sizeAfterCall<<" Is the gold key size after call!"<<std::endl;
    std::cout<<sizeAfterDeviceCall<<" Is the dv key size after call!"<<std::endl;

    krefOutput.resize(sizeAfterCall);
    vrefOutput.resize(sizeAfterCall);
    koutput.resize(sizeAfterDeviceCall);
    voutput.resize(sizeAfterDeviceCall);

#if 0

    for(unsigned int i = 0; i < sizeAfterDeviceCall ; i++)
    {
        std::cout<<" -> OKeys "<<koutput[i]<<" OValues "<<voutput[i]<<std::endl;
    }

#endif

    cmpArrays(krefOutput, koutput);
    cmpArrays(vrefOutput, voutput);
}

TEST(ReduceByKeyBasic, IntegerTestOddSizes)
{
    int length;

    int num,n,i, count=0;

   for(num=1<<16;count<=20;num++)
   {
      for(i=2;i<num;i++)
      {
         if(num%i==0)
         break;
      }
      if(num==i)
      {
      //  printf("****%d\n", length);
        length = num;
        std::vector< int > keys( length);
        // keys = {1, 2, 2, 3, 3, 3, 4, 4, 4, 4, 5, 5, 5, 5, 5,...}
        int segmentLength = 0;
        int segmentIndex = 0;
        int key = 1;
        std::vector< int > refInput( length );
        std::vector< int > input( length );
        for (int i = 0; i < length; i++)
        {
            if(std::rand()%3 == 1) key++;
            keys[i] = key;
            refInput[i] = std::rand()%4;
            input[i] = refInput[i];
        }

        // input and output vectors for device and reference

        std::vector< int > koutput( length );
        std::vector< int > voutput( length );
        std::vector< int > krefOutput( length );
        std::vector< int > vrefOutput( length );
        std::fill(krefOutput.begin(),krefOutput.end(),0);
        std::fill(vrefOutput.begin(),vrefOutput.end(),0);

        bolt::cl::equal_to<int> binary_predictor;
        bolt::cl::plus<int> binary_operator;

        // call reduce_by_key
        auto p = bolt::cl::reduce_by_key( keys.begin(), keys.end(), input.begin(), koutput.begin(), voutput.begin(), binary_predictor, binary_operator);

    #if 0

        for(unsigned int i = 0; i < 256 ; i++)
        {
            std::cout<<"Ikey "<<keys[i]<<" IValues "<<input[i]<<" -> OKeys "<<koutput[i]<<" OValues "<<voutput[i]<<std::endl;
        }

    #endif

        auto refPair = gold_reduce_by_key( keys.begin(), keys.end(), refInput.begin(), krefOutput.begin(), vrefOutput.begin(),std::plus<int>());

        //cmpArrays2(krefOutput, koutput, refPair.first, p.first);
        cmpArrays(krefOutput, koutput);
        cmpArrays(vrefOutput, voutput);
        // cmpArrays2(vrefOutput, voutput, refPair.second, p.second);
        count++;
      }
   }

}

BOLT_FUNCTOR(uddfltint,
struct uddfltint
{
    float x;
    int y;

    bool operator==(const uddfltint& rhs) const
    {
        bool equal = true;
        float ths = 0.00001; // thresh hold single(float)
        equal = ( x == rhs.x ) ? equal : false;
        if (rhs.y < ths && rhs.y > -ths)
            equal = ( (1.0*y - rhs.y) < ths && (1.0*y - rhs.y) > -ths) ? equal : false;
        else
            equal = ( (1.0*y - rhs.y)/rhs.y < ths && (1.0*y - rhs.y)/rhs.y > -ths) ? equal : false;
        return equal;
    }
    void operator++()
    {
        x += 1;
        y += 1.0f;
    }

    bool operator>(int rhs) const
    {

      bool greater = true;
      greater = ( x > rhs ) ? greater : false;
      greater = ( y > rhs ) ? greater : false;
      return greater;

    }
    void operator=(int rhs)
    {
      x = rhs;
      y = rhs;
    }

    bool operator!=(int rhs) const
    {
        bool nequal = true;
        float ths = 0.00001; // thresh hold single(float)
        nequal = ( x != rhs ) ? nequal : false;
        if (rhs < ths && rhs > -ths)
            nequal = ( (1.0*y - rhs) < ths && (1.0*y - rhs) > -ths) ? false : nequal;
        else
            nequal = ( (1.0*y - rhs)/rhs < ths && (1.0*y - rhs)/rhs > -ths) ? false : nequal;
        return nequal;
    }

    void operator-(int rhs)
    {
        x -= rhs;
        y -= rhs;
    }
};
);

BOLT_FUNCTOR(uddfltint_equal_to,
struct uddfltint_equal_to
{
    bool operator()(const uddfltint& lhs, const uddfltint& rhs) const
    {
        return lhs == rhs;
    };
};);

BOLT_FUNCTOR(uddfltint_plus,
struct uddfltint_plus
{
    uddfltint operator()(const uddfltint &lhs, const uddfltint &rhs) const
    {
        uddfltint _result;
        _result.x = lhs.x+rhs.x;
        _result.y = lhs.y+rhs.y;
        return _result;
    }
};
);

#if UDD
TEST(ReduceByKeyPairUDDTest, UDDFloatIntTest)
{
    int length = 1024;
    std::vector< uddfltint > keys( length);
    uddfltint key;
    key.x = 1;
    key.y = 1.0;
    std::vector< uddfltint > refInput( length );
    std::vector< uddfltint > input( length );

    for (int i = 0; i < length; i++)
    {
        if(std::rand()%5 == 1) key++;
        keys[i] = key;
        refInput[i].x = std::rand()%4;
        refInput[i].y = std::rand()%4;
        input[i] = refInput[i];
    }

    std::vector< uddfltint > koutput( length );
    std::vector< uddfltint > voutput( length );
    std::vector< uddfltint > krefOutput( length );
    std::vector< uddfltint > vrefOutput( length );

    // Instead of using fill
    krefOutput.clear();krefOutput.resize(length);
    vrefOutput.clear();vrefOutput.resize(length);
    voutput.clear();voutput.resize(length);
    koutput.clear();koutput.resize(length);

    uddfltint_equal_to binary_predictor;
    uddfltint_plus binary_operator;

    typedef std::pair<std::vector<uddfltint>::iterator,
            std::vector<uddfltint>::iterator> StdPairIterator;
    typedef bolt::cl::pair<std::vector<uddfltint>::iterator,
            std::vector<uddfltint>::iterator> DevicePairIterator;

    DevicePairIterator gold_pair =
    gold_reduce_by_key( keys.begin(),
                        keys.end(),
                        refInput.begin(),
                        krefOutput.begin(),
                        vrefOutput.begin(),
                        binary_operator);

    // call reduce_by_key
    DevicePairIterator dv_pair =
    bolt::cl::reduce_by_key(
        keys.begin(),
        keys.end(),
        input.begin(),
        koutput.begin(),
        voutput.begin(),
        binary_predictor,
        binary_operator);

    size_t sizeAfterCall = gold_pair.first - krefOutput.begin();
    size_t sizeAfterDeviceCall = dv_pair.first - koutput.begin();

    krefOutput.resize(sizeAfterCall);
    vrefOutput.resize(sizeAfterCall);
    koutput.resize(sizeAfterDeviceCall);
    voutput.resize(sizeAfterDeviceCall);

#if 0

    for(unsigned int i = 0; i < sizeAfterDeviceCall ; i++)
    {
        std::cout<<" -> OKeys "<<koutput[i].x<<" OValues "<<voutput[i].x<<std::endl;
    }

#endif

    cmpArrays(krefOutput, koutput);
    cmpArrays(vrefOutput, voutput);

}

#endif

int _tmain(int argc, _TCHAR* argv[])
{
    //  Register our minidump generating logic
    bolt::miniDumpSingleton::enableMiniDumps( );

    //  Initialize googletest; this removes googletest specific flags from command line
    ::testing::InitGoogleTest( &argc, &argv[ 0 ] );

    bool print_clInfo = false;
    cl_uint userPlatform = 0;
    cl_uint userDevice = 0;
    cl_device_type deviceType = CL_DEVICE_TYPE_DEFAULT;

    try
    {
        // Declare supported options below, describe what they do
        boost::program_options::options_description desc( "Scan GoogleTest command line options" );
        desc.add_options()
            ( "help,h",         "produces this help message" )
            ( "queryOpenCL,q",  "Print queryable platform and device info and return" )
            ( "platform,p",     boost::program_options::value< cl_uint >( &userPlatform )->default_value( 0 ),	"Specify the platform under test" )
            ( "device,d",       boost::program_options::value< cl_uint >( &userDevice )->default_value( 0 ),	"Specify the device under test" )
            ;

        boost::program_options::variables_map vm;
        boost::program_options::store( boost::program_options::parse_command_line( argc, argv, desc ), vm );
        boost::program_options::notify( vm );

        if( vm.count( "help" ) )
        {
            //	This needs to be 'cout' as program-options does not support wcout yet
            std::cout << desc << std::endl;
            return 0;
        }

        if( vm.count( "queryOpenCL" ) )
        {
            print_clInfo = true;
        }

        //  The following 3 options are not implemented yet; they are meant to be used with ::clCreateContextFromType()
        if( vm.count( "gpu" ) )
        {
            deviceType	= CL_DEVICE_TYPE_GPU;
        }

        if( vm.count( "cpu" ) )
        {
            deviceType	= CL_DEVICE_TYPE_CPU;
        }

        if( vm.count( "all" ) )
        {
            deviceType	= CL_DEVICE_TYPE_ALL;
        }

    }
    catch( std::exception& e )
    {
        std::cout << _T( "Scan GoogleTest error condition reported:" ) << std::endl << e.what() << std::endl;
        return 1;
    }

    //  Query OpenCL for available platforms
    cl_int err = CL_SUCCESS;

    // Platform vector contains all available platforms on system
    std::vector< cl::Platform > platforms;
    //std::cout << "HelloCL!\nGetting Platform Information\n";
    bolt::cl::V_OPENCL( cl::Platform::get( &platforms ), "Platform::get() failed" );

    if( print_clInfo )
    {
        bolt::cl::control::printPlatforms( );
        return 0;
    }

    //  Do stuff with the platforms
    std::vector<cl::Platform>::iterator i;
    if(platforms.size() > 0)
    {
        for(i = platforms.begin(); i != platforms.end(); ++i)
        {
            if(!strcmp((*i).getInfo<CL_PLATFORM_VENDOR>(&err).c_str(), "Advanced Micro Devices, Inc."))
            {
                break;
            }
        }
    }
    bolt::cl::V_OPENCL( err, "Platform::getInfo() failed" );

    // Device info
    std::vector< cl::Device > devices;
    bolt::cl::V_OPENCL( platforms.front( ).getDevices( CL_DEVICE_TYPE_ALL, &devices ), "Platform::getDevices() failed" );

    cl::Context myContext( devices.at( userDevice ) );
    cl::CommandQueue myQueue( myContext, devices.at( userDevice ) );
    bolt::cl::control::getDefault( ).setCommandQueue( myQueue );

    std::string strDeviceName = bolt::cl::control::getDefault( ).getDevice( ).getInfo< CL_DEVICE_NAME >( &err );
    bolt::cl::V_OPENCL( err, "Device::getInfo< CL_DEVICE_NAME > failed" );

    std::cout << "Device under test : " << strDeviceName << std::endl;

    int retVal = RUN_ALL_TESTS( );

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

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

    //  Print helpful message at termination if we detect errors, to help users figure out what to do next
    if( failedTests )
    {
        bolt::tout << _T( "\nFailed tests detected in test pass; please run test again with:" ) << std::endl;
        bolt::tout << _T( "\t--gtest_filter=<XXX> to select a specific failing test of interest" ) << std::endl;
        bolt::tout << _T( "\t--gtest_catch_exceptions=0 to generate minidump of failing test, or" ) << std::endl;
        bolt::tout << _T( "\t--gtest_break_on_failure to debug interactively with debugger" ) << std::endl;
        bolt::tout << _T( "\t    (only on googletest assertion failures, not SEH exceptions)" ) << std::endl;
    }

  return retVal;
}

#endif