/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

#ifndef STAPL_CONTAINERS_NORMAL_PARTITION_HPP
#define STAPL_CONTAINERS_NORMAL_PARTITION_HPP

#include <stapl/domains/indexed.hpp>
#include <numeric>
#include <boost/math/distributions/normal.hpp> // for normal_distribution
#include <boost/iterator/counting_iterator.hpp>
#include <iomanip>

namespace stapl {

namespace detail {

//////////////////////////////////////////////////////////////////////
/// @brief Creates pairs from const references. This exists to avoid the C++11
/// make_pair, which uses move construction.
/// @param t1 The first value of the pair
/// @param t2 The second value of the pair
/// @return A pair of t1 and t2
//////////////////////////////////////////////////////////////////////
template<typename T1, typename T2>
std::pair<T1, T2>
non_move_make_pair(T1 const& t1, T2 const& t2)
{
  return std::make_pair(t1, t2);
}

}

//////////////////////////////////////////////////////////////////////
/// @brief Partition in which domains created form a standard normal
/// distribution.
///
/// Subdomains that are near the mean index have more elements than others,
/// which have sizes that taper off based on a given standard deviation value.
///
/// @code
///   0.45 ++---------+----------+-----------+----------+----------+---------++
///        +          +          +           +      number of elements ###### +
///    0.4 ++                             ######                             ++
///        |                           ###      ###                           |
///   0.35 ++                         ##          ##                         ++
///        |                         ##            ##                         |
///    0.3 ++                      ##                ##                      ++
///        |                      ##                  ##                      |
///   0.25 ++                    ##                    ##                    ++
///    0.2 ++                   ##                      ##                   ++
///        |                  ##                          ##                  |
///   0.15 ++                ##                            ##                ++
///        |               ###                              ###               |
///    0.1 ++            ###                                  ###            ++
///        |           ###                                      ###           |
///   0.05 ++        ###                                          ###        ++
///        +   ###### +          +           +          +          + ######   +
///      0 #####------+----------+-----------+----------+----------+------#####
///        2          3          4           5          6          7          8
///                                  partition index
/// @endcode
/// @tparam Dom Type of the domain to be partitioned
//////////////////////////////////////////////////////////////////////
template <typename Dom>
struct normal_partition
{
  /// The domain of the partitioner itself (i.e., the domain [0, ..., p-1])
  typedef indexed_domain<size_t>           domain_type;
  /// Type used to describe the i'th subdomain
  typedef typename domain_type::index_type index_type;
  /// Type of the subdomains produced by the partition
  typedef Dom                              value_type;
  /// Type of the GIDs in the subdomains
  typedef typename value_type::index_type  gid_type;
  /// Type of the size of the subdomains
  typedef typename value_type::size_type   size_type;

protected:
  /// The global domain
  value_type                     m_domain;
  /// The index that where the normal distribution will be centered
  size_t                         m_mean;
  /// Standard deviation value to dictate the shape of the normal curve
  double                         m_sigma;
  /// The number of partitions to generate
  size_t                         m_parts;
  /// The sizes of each partition
  std::vector<size_t>            m_sizes;
  /// Lower bounds (inclusive) of the subdomains
  std::map<gid_type, index_type> m_offsets;
  /// Upper bounds (exclusive) of the subdomains
  std::map<gid_type, index_type> m_upper_bound;

public:
  normal_partition(normal_partition const& other)
    : m_domain(other.m_domain), m_mean(other.m_mean), m_sigma(other.m_sigma),
      m_parts(other.m_parts), m_sizes(other.m_sizes),
      m_offsets(other.m_offsets), m_upper_bound(other.m_upper_bound)
  { }

  //////////////////////////////////////////////////////////////////////
  /// Create a partition with a domain, the number of partitions and values
  /// describing the shape of the normal distribution.
  /// @param dom The domain to partition
  /// @param parts The number of partitions to generate
  /// @param mean The index of the partition that will be the mean of the
  /// distribution.
  /// @param sigma The standard deviation of the distribution
  //////////////////////////////////////////////////////////////////////
  normal_partition(value_type const& dom, const size_t parts,
                   const size_t mean, const double sigma = 1.0)
    : m_domain(dom), m_mean(mean), m_sigma(sigma), m_parts(parts)
  {
    using boost::math::normal;

    try {
      // Construct a standard normal distribution
      normal ns(m_mean, m_sigma);

      size_t size   = m_domain.size();
      size_t unused = size;
      for (size_t z = 0; z < m_parts; ++z)
      {
        // get the size for the current partition
        size_t s = floor(pdf(ns, z)*size);

        size_t size_to_push = std::min(s, unused);
        m_sizes.push_back(size_to_push);
        unused -= size_to_push;
      }

      // add unused gids to the mean partition
      if (unused > 0)
        m_sizes[mean] += unused;

      // compute offsets (lower bounds) for the partitions
      std::vector<size_t> offsets(m_sizes.size());
      std::partial_sum(m_sizes.begin(), m_sizes.end(), offsets.begin());

      m_offsets[m_domain.first()] = 0;
      std::transform(
        boost::counting_iterator<gid_type>(1),
        boost::counting_iterator<gid_type>(m_sizes.size()), offsets.begin(),
        std::inserter(m_offsets, m_offsets.begin()),
        detail::non_move_make_pair<gid_type, index_type>
      );

      // compute upper bounds for partitions
      typename std::map<gid_type, index_type>::const_iterator cit =
        m_offsets.begin();

      for (; cit != m_offsets.end(); ++cit)
        if (m_sizes[cit->first] > 0)
          m_upper_bound.insert(
            std::make_pair(cit->second + m_sizes[cit->first], cit->first)
          );

    } catch (const std::exception& e) {
        std::cout << std::endl << "Exception thrown with message: "
                  << e.what() << std::endl;
    }

  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Get the partition's domain [0, .., p-1]
  //////////////////////////////////////////////////////////////////////
  domain_type domain() const
  { return domain_type(m_parts); }

  //////////////////////////////////////////////////////////////////////
  /// @brief Set the global domain to partition
  /// @param dom The domain to partition
  /// @bug This should recompute the bounds of the partition
  //////////////////////////////////////////////////////////////////////
  void set_domain(value_type const& dom)
  {
    stapl_assert(false, "set_domain not supported");
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Return the subdomain at a given index. This is the inverse
  /// of @ref find.
  /// @param idx The index of the subdomain, which should be from 0 to
  /// @ref size() - 1.
  //////////////////////////////////////////////////////////////////////
  value_type operator[](index_type idx) const
  {
    // special case for empty domains
    if (m_sizes[idx] == 0)
      return value_type();

    index_type first =
      m_domain.advance(m_domain.first(), m_offsets.find(idx)->second);
    return value_type(first, m_domain.advance(first, m_sizes[idx]) - 1);
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Return the number of partitions generated.
  //////////////////////////////////////////////////////////////////////
  size_type size() const
  {
    return m_sizes.size();
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Return the original domain that was used to create subdomains.
  //////////////////////////////////////////////////////////////////////
  value_type const& global_domain() const
  {
    return m_domain;
  }

  //////////////////////////////////////////////////////////////////////
  /// @brief Returns the index of the partition that contains the gid @c g
  /// @param g gid to find
  /// @return The index of the subdomain that contains @c g
  //////////////////////////////////////////////////////////////////////
  index_type find(index_type g) const
  {
    stapl_assert(m_upper_bound.upper_bound(g) != m_upper_bound.end(),
      "partition index out of bounds");

    return m_upper_bound.upper_bound(g)->second;
  }
};

} // stapl namespace

#endif /* STAPL_CONTAINERS_NORMAL_PARTITION_HPP */


/* ----------------------------------------------------------------------------

* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)

* See LICENSE for the license information

* -------------------------------------------------------------------------- */

/**
* @file   testSymmetricBlockMatrix.cpp
* @brief  Unit tests for SymmetricBlockMatrix class
* @author Richard Roberts
**/

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <boost/assign/list_of.hpp>

using namespace std;
using namespace gtsam;
using boost::assign::list_of;

static SymmetricBlockMatrix testBlockMatrix(
  list_of(3)(2)(1),
  (Matrix(6, 6) <<
  1, 2, 3, 4, 5, 6,
  2, 8, 9, 10, 11, 12,
  3, 9, 15, 16, 17, 18,
  4, 10, 16, 22, 23, 24,
  5, 11, 17, 23, 29, 30,
  6, 12, 18, 24, 30, 36).finished());

/* ************************************************************************* */
TEST(SymmetricBlockMatrix, ReadBlocks)
{
  // On the diagonal
  Matrix expected1 = (Matrix(2, 2) <<
    22, 23,
    23, 29).finished();
  Matrix actual1 = testBlockMatrix.diagonalBlock(1);
  EXPECT(assert_equal(expected1, actual1));

  // Above the diagonal
  Matrix expected2 = (Matrix(3, 2) <<
    4, 5,
    10, 11,
    16, 17).finished();
  Matrix actual2 = testBlockMatrix.aboveDiagonalBlock(0, 1);
  EXPECT(assert_equal(expected2, actual2));
}

/* ************************************************************************* */
TEST(SymmetricBlockMatrix, WriteBlocks)
{
  // On the diagonal
  Matrix expected1 = testBlockMatrix.diagonalBlock(1);
  SymmetricBlockMatrix bm1 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);

  bm1.setDiagonalBlock(1, expected1);
  Matrix actual1 = bm1.diagonalBlock(1);
  EXPECT(assert_equal(expected1, actual1));

  // Above the diagonal
  Matrix expected2 = testBlockMatrix.aboveDiagonalBlock(0, 1);
  SymmetricBlockMatrix bm2 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);
  bm2.setOffDiagonalBlock(0, 1, expected2);
  Matrix actual2 = bm2.aboveDiagonalBlock(0, 1);
  EXPECT(assert_equal(expected2, actual2));

  // Below the diagonal
  Matrix expected3 = testBlockMatrix.aboveDiagonalBlock(0, 1).transpose();
  SymmetricBlockMatrix bm3 = SymmetricBlockMatrix::LikeActiveViewOf(testBlockMatrix);
  bm3.setOffDiagonalBlock(1, 0, expected3);
  Matrix actual3 = bm3.aboveDiagonalBlock(0, 1).transpose();
  EXPECT(assert_equal(expected3, actual3));
}

/* ************************************************************************* */
TEST(SymmetricBlockMatrix, Ranges)
{
  // On the diagonal
  Matrix expected1 = (Matrix(3, 3) <<
    22, 23, 24,
    23, 29, 30,
    24, 30, 36).finished();
  Matrix actual1 = testBlockMatrix.selfadjointView(1, 3);
  EXPECT(assert_equal(expected1, actual1));

  // Above the diagonal
  Matrix expected2 = (Matrix(3, 3) <<
    4, 5, 6,
    10, 11, 12,
    16, 17, 18).finished();
  Matrix actual2 = testBlockMatrix.aboveDiagonalRange(0, 1, 1, 3);
  EXPECT(assert_equal(expected2, actual2));
}

/* ************************************************************************* */
TEST(SymmetricBlockMatrix, expressions)
{
  SymmetricBlockMatrix expected1(list_of(2)(3)(1), (Matrix(6, 6) <<
    0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0,
    0, 0, 4, 6, 8, 0,
    0, 0, 0, 9, 12, 0,
    0, 0, 0, 0, 16, 0,
    0, 0, 0, 0, 0, 0).finished());

  SymmetricBlockMatrix expected2(list_of(2)(3)(1), (Matrix(6, 6) <<
    0, 0, 10, 15, 20, 0,
    0, 0, 12, 18, 24, 0,
    0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0).finished());

  Matrix a = (Matrix(1, 3) << 2, 3, 4).finished();
  Matrix b = (Matrix(1, 2) << 5, 6).finished();

  SymmetricBlockMatrix bm1(list_of(2)(3)(1));
  bm1.setZero();
  bm1.diagonalBlock(1).rankUpdate(a.transpose());
  EXPECT(assert_equal(Matrix(expected1.selfadjointView()), bm1.selfadjointView()));

  SymmetricBlockMatrix bm2(list_of(2)(3)(1));
  bm2.setZero();
  bm2.updateOffDiagonalBlock(0, 1, b.transpose() * a);
  EXPECT(assert_equal(Matrix(expected2.selfadjointView()), bm2.selfadjointView()));

  SymmetricBlockMatrix bm3(list_of(2)(3)(1));
  bm3.setZero();
  bm3.updateOffDiagonalBlock(1, 0, a.transpose() * b);
  EXPECT(assert_equal(Matrix(expected2.selfadjointView()), bm3.selfadjointView()));

  SymmetricBlockMatrix bm4(list_of(2)(3)(1));
  bm4.setZero();
  bm4.updateDiagonalBlock(1, expected1.diagonalBlock(1));
  EXPECT(assert_equal(Matrix(expected1.selfadjointView()), bm4.selfadjointView()));

  SymmetricBlockMatrix bm5(list_of(2)(3)(1));
  bm5.setZero();
  bm5.updateOffDiagonalBlock(0, 1, expected2.aboveDiagonalBlock(0, 1));
  EXPECT(assert_equal(Matrix(expected2.selfadjointView()), bm5.selfadjointView()));

  SymmetricBlockMatrix bm6(list_of(2)(3)(1));
  bm6.setZero();
  bm6.updateOffDiagonalBlock(1, 0, expected2.aboveDiagonalBlock(0, 1).transpose());
  EXPECT(assert_equal(Matrix(expected2.selfadjointView()), bm6.selfadjointView()));
}

/* ************************************************************************* */
TEST(SymmetricBlockMatrix, inverseInPlace) {
  // generate an invertible matrix
  const Vector3 a(1.0, 0.2, 2.0), b(0.3, 0.8, -1.0), c(0.1, 0.2, 0.7);
  Matrix inputMatrix(3, 3);
  inputMatrix.setZero();
  inputMatrix += a * a.transpose();
  inputMatrix += b * b.transpose();
  inputMatrix += c * c.transpose();
  const Matrix expectedInverse = inputMatrix.inverse();

  SymmetricBlockMatrix symmMatrix(list_of(2)(1), inputMatrix);
  // invert in place
  symmMatrix.invertInPlace();
  EXPECT(assert_equal(expectedInverse, symmMatrix.selfadjointView()));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


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

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ABS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_ABS_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/detail/math.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::arithmetic_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T a) const BOOST_NOEXCEPT
    {
      using utype = dispatch::as_unsigned_t<T>;

      utype mask = a >> (sizeof(T)*8 - 1);
      return (a + mask) ^ mask;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::single_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T a) const BOOST_NOEXCEPT
    {
      #ifdef BOOST_SIMD_HAS_FABSF
       return ::fabsf(a);
      #else
       return (a > 0) ? a : -a;
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::double_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T a) const BOOST_NOEXCEPT
    {
      return ::fabs(a);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::unsigned_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T a) const BOOST_NOEXCEPT
    {
      return a;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::bool_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T a) const BOOST_NOEXCEPT
    {
      return a;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::floating_<T>>
                          , boost::simd::std_tag
                          )
  {
    BOOST_FORCEINLINE T operator()(T a, std_tag const&) const BOOST_NOEXCEPT
    {
      return std::fabs(a);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( abs_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::integer_<T>>
                          , boost::simd::std_tag
                          )
  {
    BOOST_FORCEINLINE T operator()(T a, std_tag const&) const BOOST_NOEXCEPT
    {
      return std::abs(a);
    }
  };
} } }

#endif


/******************************************************************************
 * Copyright (c) 2017 Sergey Alexandrov
 *
 * 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.
 ******************************************************************************/

#ifdef HAVE_CERES

#include <iostream>

#include <boost/assert.hpp>

#include <opencv2/imgproc/imgproc.hpp>

#include <ceres/ceres.h>

#include "utils/colors.h"
#include "utils/plot_radiometric_response.h"

#include "debevec_calibration.h"

struct Residual {
  double t_;
  unsigned char i_;

  Residual(double t, unsigned char i)
  : t_(t)
  , i_(i) {}

  template <typename T>
  bool operator()(const T* const x, const T* const a, T* residual) const {
    residual[0] = x[0] + T(t_) - a[i_];
    return true;
  }
};

struct RegularizationResidual {
  RegularizationResidual() = default;

  template <typename T>
  bool operator()(const T* const a, T* residual) const {
    for (int i = 1; i < 255; ++i)
      residual[i - 1] = a[i - 1] - a[i] * T(2) + a[i + 1];
    return true;
  }
};

struct CeresIterationCallback : public ceres::IterationCallback {
  DebevecCalibration* p;
  bool print_, visualize_;
  CeresIterationCallback(DebevecCalibration* parent, bool print, bool visualize)
  : p(parent)
  , print_(print)
  , visualize_(visualize) {}
  ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) override {
    if (print_)
      p->printIteration(summary.iteration + 1, summary.cost, summary.cost_change);
    if (visualize_)
      p->visualizeProgress();
    return ceres::SOLVER_CONTINUE;
  }
};

cv::Mat DebevecCalibration::calibrateChannel(const Dataset& dataset) {
  CeresIterationCallback callback{this, verbosity_ > 0, static_cast<bool>(imshow_)};
  dataset_ = &dataset;
  selectPixels();

  X_.resize(locations_.size(), 0);
  U_.create(256, 1);
  for (size_t i = 0; i < 256; ++i)
    U_(i) = std::log(0.5 + i / 256.0);

  auto U = reinterpret_cast<double*>(U_.data);

  printHeader();

  ceres::Problem problem;
  problem.AddParameterBlock(U, 256);
  std::vector<int> constant_255;
  constant_255.push_back(128);
  problem.SetParameterization(U, new ceres::SubsetParameterization(256, constant_255));

  auto loss = new ceres::HuberLoss(0.05);
  auto scaling = new ceres::ScaledLoss(nullptr, lambda_ * lambda_, ceres::DO_NOT_TAKE_OWNERSHIP);

  std::vector<unsigned int> c(locations_.size(), 0);

  // Create residual blocks and compute initial irradiance estimates
  for (const auto& t : dataset_->getExposureTimes()) {
    for (const auto& image : dataset_->getImages(t)) {
      for (size_t i = 0; i < locations_.size(); ++i) {
        auto lt = std::log(t);
        auto p = image.at<uint8_t>(locations_[i]);
        if (isPixelValid(p)) {
          problem.AddResidualBlock(new ceres::AutoDiffCostFunction<Residual, 1, 1, 256>(new Residual{lt, p}), loss,
                                   &X_[i], U);
          X_[i] += U_(p) - lt;
          c[i] += 1;
        }
      }
    }
  }

  problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<RegularizationResidual, 254, 256>(new RegularizationResidual), scaling, U);

  for (size_t i = 0; i < locations_.size(); ++i)
    X_[i] /= c[i];

  ceres::Solver::Options options;
  options.max_num_iterations = max_num_iterations_;
  options.linear_solver_type = ceres::SPARSE_SCHUR;
  options.logging_type = ceres::SILENT;
  options.minimizer_progress_to_stdout = false;
  options.update_state_every_iteration = true;

  if (verbosity_ || imshow_)
    options.callbacks.push_back(&callback);

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  printFooter();

  if (verbosity_ > 1)
    std::cout << summary.FullReport() << std::endl;

  cv::Mat response;
  U_.convertTo(response, CV_32F);
  cv::exp(response, response);

  return response;
}

void DebevecCalibration::selectPixels() {
  BOOST_ASSERT(dataset_ != nullptr);

  locations_.clear();
  auto times = dataset_->getExposureTimes();

  const int M = min_samples_;
  std::vector<int> hist(256, 0);

  for (size_t i = 0; i < times.size(); ++i) {
    auto t = times[i];
    auto image = dataset_->getImages(t)[0];

    cv::Mat indices;
    cv::Mat flat = image.reshape(1, 1);
    cv::sortIdx(flat, indices, cv::SORT_EVERY_ROW + cv::SORT_DESCENDING);

    int x = 0;
    unsigned char intensity = max_valid_;

    while (intensity >= min_valid_) {
      while (hist[intensity] < M && x < indices.size().area()) {
        auto index = indices.at<int>(x++);
        auto p = flat.at<uint8_t>(index);
        if (p > intensity)
          continue;
        else if (p < intensity) {
          --x;  // to repeat this iteration again
          break;
        }
        locations_.push_back(index);
        for (size_t j = i; j < times.size(); ++j) {
          auto t = times[j];
          auto p = dataset_->getImages(t)[0].at<uint8_t>(index);
          if (!isPixelValid(p))
            continue;
          ++hist[p];
        }
      }
      intensity--;
    }

    break;
  }
}

void DebevecCalibration::visualizeProgress() {
  cv::Mat response;
  U_.convertTo(response, CV_32F);
  cv::exp(response, response);

  cv::Mat canvas(512, 512, CV_8UC3);
  canvas.setTo(255);

  double min, max;
  cv::minMaxIdx(response, &min, &max);

  response -= min;
  response /= (max - min);

  float x_scale = static_cast<float>(canvas.size().width) / 256;
  float y_scale = static_cast<float>(canvas.size().height) / 1.0;

  for (const auto& t : dataset_->getExposureTimes()) {
    for (const auto& image : dataset_->getImages(t)) {
      for (size_t i = 0; i < locations_.size(); ++i) {
        auto p = image.at<uint8_t>(locations_[i]);
        if (isPixelValid(p)) {
          auto v = std::exp(X_[i]) * t;
          cv::circle(canvas, cv::Point(p * x_scale, canvas.size().height - (v - min) / (max - min) * y_scale),
                     std::ceil(x_scale), utils::colors::BGR_LIGHT[channel_], -1);
        }
      }
    }
  }

  utils::plotRadiometricResponse(response, canvas, utils::colors::BGR[channel_]);
  imshow_(canvas);
}

void DebevecCalibration::setMinSamplesPerIntensityLevel(unsigned int min_samples) {
  min_samples_ = min_samples;
}

void DebevecCalibration::setSmoothingLambda(double lambda) {
  lambda_ = lambda;
}

#endif


////////////////////////////////////////////////////////////////////////////////
// distribution::survival::response::right_truncated::meta_factory.hpp 		  //
//                                                                            //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                 //
//  Software License, Version 1.0. (See accompanying file                     //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)          //
////////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_RIGHT_TRUNCATED_META_MAKE_RESPONSE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RESPONSE_RIGHT_TRUNCATED_META_MAKE_RESPONSE_HPP_ER_2009
#include <boost/mpl/identity.hpp>
#include <boost/statistics/detail/distribution/survival/response/common/meta_factory.hpp>
#include <boost/statistics/detail/distribution/survival/response/types/right_truncated/event_factory.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace distribution{
namespace survival {
namespace response {
namespace meta{

    template<typename T,typename B>
    struct factory< event<T,B> > : boost::mpl::identity< event_factory<T,B> >{};

}// meta
}// response
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


#pragma once

#include <iostream>
#include <memory>
#include <Eigen/Dense>
#include <vector>

class AnkleKneeCommand
{
public:
    Eigen::Vector2d q;
    Eigen::Vector2d qdot;
    Eigen::Vector2d jtrq;
};

class AnkleKneeSensorData
{
public:
    Eigen::Vector2d q;
    Eigen::Vector2d qdot;
    Eigen::Vector2d jtrq;
    Eigen::Vector2d chirp;
    Eigen::Vector2d chirpInput;
    Eigen::Vector2d chirpOutput;
    Eigen::Vector2d busVoltage;
    Eigen::Vector2d currentMsr;
    Eigen::Vector2d coreTemp;
    unsigned int nanosecondKnee;
    unsigned int nanosecondAnkle;
};

class AnkleKneeInterface
{
public:
    AnkleKneeInterface ();
    virtual ~AnkleKneeInterface ();

    void getCommand(std::shared_ptr<AnkleKneeSensorData> data,
                    std::shared_ptr<AnkleKneeCommand> cmd);
private:
    double mTime;
    int mCount;
    double mInitTime;
    double mServoRate;
    int mNumJoint;
    Eigen::Vector2d mInitQ;

    //Chirp related
    double m_high_freq;
    double m_low_freq;
    double m_rate;
    double m_amp;
    double m_offset;
    double m_start_dur;
    double m_end_dur;
    double m_last_signal;
    double m_last_velocity;
    std::vector<double> m_save_input;
    std::vector<double> m_save_output;
    std::vector<double> m_save_time;
    std::vector<double> m_save_temp;
    std::vector<double> m_debug_knee_q;
    std::vector<double> m_debug_knee_qdot;
    std::vector<double> m_debug_knee_effort;
    std::vector<double> m_debug_bus_voltage;
    std::vector<double> m_debug_currentMsr;
    bool m_is_saved;
    bool m_is_saved_cmd;
    int mMode; // 0 : joint impedance, 1 : currrent, 2: motor pos
    int mLinearChirp;

    void _initialize(std::shared_ptr<AnkleKneeSensorData> data,
                     std::shared_ptr<AnkleKneeCommand> cmd);
    void _maintainInitialPosition(std::shared_ptr<AnkleKneeSensorData> data,
                                  std::shared_ptr<AnkleKneeCommand> cmd);
    void _sinusoidalPosition(std::shared_ptr<AnkleKneeSensorData> data,
                             std::shared_ptr<AnkleKneeCommand> cmd);
    void _chirpSignal(std::shared_ptr<AnkleKneeSensorData> data,
                      std::shared_ptr<AnkleKneeCommand> cmd);
    void _stepSignal(std::shared_ptr<AnkleKneeSensorData> data,
                     std::shared_ptr<AnkleKneeCommand> cmd);
    void _bangControl(std::shared_ptr<AnkleKneeSensorData> data,
                      std::shared_ptr<AnkleKneeCommand> cmd);
    void _massID(std::shared_ptr<AnkleKneeSensorData> data,
                 std::shared_ptr<AnkleKneeCommand> cmd);

    Eigen::Vector2d mJPosAct;
    Eigen::Vector2d mJVelAct;
    Eigen::Vector2d mJEffAct;
    Eigen::Vector2d mJPosDes;
    Eigen::Vector2d mJVelDes;
    Eigen::Vector2d mJEffDes;
    Eigen::MatrixXd mA;
    double mMass;
    double mGrav;
    double mRx;
    double mRy;

    //Sys ID
    std::vector<double> mThetaBin;
    std::vector<double> mCmdThetaBin;
    std::vector<double> mTrqBin;
    std::vector<double> mCmdTrqBin;
};


#include <boost/test/unit_test.hpp>

#include "logs.h"
#include "route.h"
#include "track.h"

using namespace GPS;

/*
Design:
Speed will always be positive,
and also less than an maximum,
theoretically the maximum speed is that of light at 300,000m/s however most gps systems are made to shut off at 1200mph or 536.5m/s
test 1: typcial speed for persone walking
test 2: typical speed for car

test 3: edge case before shut off
test 5: impossible data

Structure:
Tests are seperated into two groupes
The first for correct data - consisting of regular and edge cases
The second for incorrect data - consisting of inacceptable or no data

*/

BOOST_AUTO_TEST_SUITE( trackMaxSpeed )

const bool isFileName = true;

//tests for correct data
BOOST_AUTO_TEST_CASE (walkingMaxSpeed)
{
	Track track = Track(LogFiles::GPXTracksDir + "maxSpeedWalking.gpx", isFileName);
	BOOST_CHECK_EQUAL(track.maxSpeed(), 1.46);
}

BOOST_AUTO_TEST_CASE (carMaxSpeed)
{
	Track track = Track(LogFiles::GPXTracksDir + "maxSpeedCar.gpx", isFileName);
	BOOST_CHECK_EQUAL(track.maxSpeed(), 7.32);
}

//test for missing or incorrect or out of bounds data
//less than shut off point
BOOST_AUTO_TEST_CASE (edgeMaxSpeed)
{
	Track track = Track(LogFiles::GPXTracksDir + "maxSpeedEdge.gpx", isFileName);
	BOOST_CHECK_LT(track.maxSpeed(), 536.5);
}
//less than LIGHTSPEED
BOOST_AUTO_TEST_CASE (LightSpeed)
{
	Track track = Track(LogFiles::GPXTracksDir + "maxSpeedLightSpeed.gpx", isFileName);
	BOOST_CHECK_LT(track.maxSpeed(), 300000);
}

BOOST_AUTO_TEST_SUITE_END()


#include <vector>
#include <iostream>
#include <map>
#include <cstring>
#include <list>
#include <memory>
#include <chrono>
#include <thread>
#include <cassert>
#include <csignal>
#include <cmath>
#include <variant>
#include <random>
#include <algorithm>
#include <numeric>
#include <iterator>
#include <fstream>
#include <time.h>

#define RESOURCES_DATA_PATH (std::string) "resources/data"

#ifdef WITH_BOOST

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

namespace py = boost::python;
namespace np = boost::python::numpy;

#endif

#include "MUtil.h"
#include "DGen.hpp"
#include "NNet.hpp"
#include "CEng.hpp"
#include "tests.hpp"

int main() {

	#ifdef WITH_BOOST
	Py_Initialize();

	boost::python::numpy::initialize();
	#endif

	dgen_tests();

	std::cout << "LOAD MNIST" << std::endl;

	#define IN_T float
	#define HL_T float

	auto datain = DGen::LoadMnist<DEBUG0>("data");

	constexpr size_t INPUT_LAYER = datain.get_pattern_size();
	constexpr size_t OUTPUT_LAYER = datain.get_labels_size();
	constexpr size_t B = datain.get_train_batch_size();
	constexpr size_t mB = 1000;

	{
		TIME_START

		//auto xtrainBegin = mnist->begin<DGen::data_e::TRAIN_IMG>();
		//auto xtrainEnd = mnist->end<DGen::data_e::TRAIN_IMG>();

		//auto f = std::accumulate(xtrainBegin, xtrainEnd, 0.f, [&](MNIST_IN_t in, MNIST_IN_t &x) {return in + x;});

		//std::cout << "sum = " << f / double{256} << std::endl;

		TIME_CHECK
	}

	std::srand(0);

	auto layersMaker = NNet::LayersMaker<IN_T, HL_T, B, mB, INPUT_LAYER, 20, OUTPUT_LAYER>();

	auto layers = layersMaker.alloc<DEBUG1>();

	auto datagen = DGen::Datagen<HL_T, DGen::RANDOM_NORMAL>();

	auto engine = CEng::Engine();

	auto network = NNet::Network(layersMaker, layers, engine, datain, datagen);

	network.print();

	network.init<DEBUG1>();

	network.compute<DEBUG2>();

	TIME_START
	for (int i = 0; i < 10000000; i++) {
		network.compute<DEBUG0>();
	}
	TIME_CHECK

/*	auto start = clock();

	size_t o = 0;
	for (size_t i = 0; i < 1000; i++) {
		o += 1;
		compute<DEBUG0, Layers_t, I, shapeNN...>(1, layersWrap, enginePtr);
	}

	std::cout << "Took " << clock() - start << " ms" << std::endl;

	std::cout << o << std::endl;
*/

	//auto * lh = (LayerHidden<int, 100, 100, mB> * ) layersWrap.layersTraining[1];
	//lh->V;
	//layersWrap->print();

	std::cout << "Done." << std::endl;

	return 0;
}


// Copyright (c) 2020 Julian Bernhard, Klemens Esterle, Patrick Hart and
// Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MODULES_WORLD_OPENDRIVE_PLAN_VIEW_HPP_
#define MODULES_WORLD_OPENDRIVE_PLAN_VIEW_HPP_

#include <Eigen/Core>
#include "modules/geometry/geometry.hpp"
#include "modules/world/opendrive/lane.hpp"

namespace modules {
namespace world {
namespace opendrive {

using modules::geometry::Point2d;
using modules::geometry::Line;

class PlanView {
 public:
  PlanView() : length_(0.0) {}
  ~PlanView() {}

  //! setter functions
  bool AddLine(Point2d start_point, float heading, float length);

  bool AddSpiral(
    Point2d start_point,
    float heading,
    float length,
    float curvStart,
    float curvEnd,
    float s_inc = 2.0f);

  bool AddArc(
    Point2d start_point,
    float heading,
    float length,
    float curvature,
    float s_inc = 2.0f);

  void CalcArcPosition(
    const float s, float initial_heading, float curvature, float &dx, float &dy);

  //! getter functions
  Line GetReferenceLine() const { return reference_line_; }

  bool ApplyOffsetTransform(float x, float y, float hdg);

  float GetLength() const { return length_; }
  float GetDistance( const Point2d &p) const {
    return boost::geometry::distance(reference_line_.obj_, p);
  }

 private:
  Line reference_line_;  // sequential build up
  float length_;
};

using PlanViewPtr = std::shared_ptr<PlanView>;

}  // namespace opendrive
}  // namespace world
}  // namespace modules

#endif  // MODULES_WORLD_OPENDRIVE_PLAN_VIEW_HPP_


#include <algorithm>

#include <boost/graph/adjacency_list.hpp>

#include <cyber/solve.hpp>
#include <cyber/term.hpp>

namespace {

struct vertex_info {
    position_type position;
};

struct edge_info {
    std::size_t dimension;
};

struct search_info {
    bool skip;
    search_info(bool skip = false) : skip(skip) {}
};

using Graph = boost::adjacency_list<
    boost::vecS, boost::vecS,
    boost::directedS,
    boost::property<boost::vertex_index_t, vertex_info>,
    boost::property<boost::edge_index_t, edge_info>
>;
using Traits = boost::graph_traits<Graph>;

using vertex_index_type = boost::property_map<Graph, boost::vertex_index_t>::type;
using edge_index_type = boost::property_map<Graph, boost::edge_index_t>::type;
using search_index_type = boost::property_map<Graph, boost::vertex_index_t>::type;

using vertex_props_type = boost::vector_property_map<vertex_info, vertex_index_type>;
using edge_props_type = boost::vector_property_map<edge_info, edge_index_type>;
using search_props_type = boost::vector_property_map<search_info, vertex_index_type>;

void traverse(
    const Graph & graph,
    const matrix_type & matrix,
    const std::vector<std::vector<unsigned char>> & sequences,
    vertex_props_type & vertex_props,
    edge_props_type & edge_props,
    search_props_type & search_props,
    Traits::vertex_descriptor src_vertex_descriptor,
    edge_info src_edge_info,
    buffer_length_type distance,
    score_type & high_score,
    std::vector<Traits::vertex_descriptor> & path,
    std::vector<Traits::vertex_descriptor> & result
) {
    path.push_back(src_vertex_descriptor);
    put(search_props, src_vertex_descriptor, search_info{true});
    score_type score = 0;

    for (auto sequence = sequences.begin(); sequence != sequences.end(); ++sequence) {
        if (std::search(path.begin(), path.end(), sequence->begin(), sequence->end(),
            [&matrix,&vertex_props](Traits::vertex_descriptor vertex, unsigned char rhs){
                return matrix(get(vertex_props, vertex).position) == rhs;
            }
        ) != path.end()) {
            score += (1 << (std::distance(sequences.begin(), sequence)));
        }
    }
    if (score && (score > high_score || (score == high_score && path.size() < result.size()))) {
        high_score = score;
        result = path;
    }
    if (distance) {
        const auto edges = out_edges(src_vertex_descriptor, graph);
        for (auto edge = edges.first; edge != edges.second; ++edge) {
            const edge_info dst_edge_info = get(boost::edge_index_t(), graph, *edge);
            const Traits::vertex_descriptor dst_vertex_descriptor = target(*edge, graph);
            if (dst_edge_info.dimension != src_edge_info.dimension && !get(search_props, dst_vertex_descriptor).skip) {
                traverse(
                    graph, matrix, sequences,
                    vertex_props, edge_props, search_props,
                    dst_vertex_descriptor, dst_edge_info,
                    distance - 1,
                    high_score,
                    path, result
                );
            }
        }
    }
    put(search_props, src_vertex_descriptor, search_info{false});
    path.pop_back();
}

} // namespace

solved_type solve(
    const matrix_type & matrix,
    const sequences_type & sequences,
    const buffer_length_type buffer
) {
    Graph graph(matrix.shape()[0] * matrix.shape()[1]);

    vertex_index_type vertex_index(get(boost::vertex_index, graph));
    vertex_props_type vertex_props(num_vertices(graph), vertex_index);

    edge_index_type edge_index(get(boost::edge_index, graph));
    edge_props_type edge_props(num_edges(graph), edge_index);

    search_index_type search_index(get(boost::vertex_index, graph));
    search_props_type search_props(num_vertices(graph), search_index);

    // Store vertex values
    for (std::size_t r = 0; r < matrix.shape()[0]; ++r) {
        for (std::size_t c = 0; c < matrix.shape()[1]; ++c) {
            // NOTE: For whatever insane reason, multi_array::index uses SIGNED indices.
            vertex_info info{{static_cast<position_type::value_type>(r), static_cast<position_type::value_type>(c)}};
            put(vertex_props, Traits::vertex_descriptor{r * matrix.shape()[1] + c}, info);
        }
    }

    // Store horizontal edge directions
    for (std::size_t r = 0; r < matrix.shape()[0]; ++r) {
        for (std::size_t c_a = 0; c_a < matrix.shape()[1]; ++c_a) {
            for (std::size_t c_b = 0; c_b < matrix.shape()[1]; ++c_b) {
                if (c_a != c_b) {
                    add_edge(
                        Traits::vertex_descriptor{r * matrix.shape()[1] + c_a},
                        Traits::vertex_descriptor{r * matrix.shape()[1] + c_b},
                        boost::property<boost::edge_index_t, edge_info>(edge_info{0}),
                        graph
                    );
                }
            }
        }
    }

    // Store vertical edge directions
    for (std::size_t c = 0; c < matrix.shape()[1]; ++c) {
        for (std::size_t r_a = 0; r_a < matrix.shape()[0]; ++r_a) {
            for (std::size_t r_b = 0; r_b < matrix.shape()[0]; ++r_b) {
                if (r_a != r_b) {
                    add_edge(
                        Traits::vertex_descriptor{r_a * matrix.shape()[1] + c},
                        Traits::vertex_descriptor{r_b * matrix.shape()[1] + c},
                        boost::property<boost::edge_index_t, edge_info>(edge_info{1}),
                        graph
                    );
                }
            }
        }
    }

    std::vector<Traits::vertex_descriptor> result;
    std::vector<Traits::vertex_descriptor> path;
    score_type high_score = 0;
    for (std::size_t c = 0; c < matrix.shape()[1]; ++c) {
        const Traits::vertex_descriptor vertex{c};
        traverse(
            graph, matrix, sequences,
            vertex_props, edge_props, search_props,
            vertex, edge_info{0},
            buffer - 1,
            high_score,
            path, result
        );
    }

    return solved_type {
        [&matrix,&vertex_props,&result](){
            path_type path;
            std::transform(result.begin(), result.end(), std::back_insert_iterator(path), [&matrix,&vertex_props](const Traits::vertex_descriptor & vertex){
                return get(vertex_props, vertex).position;
            });
            return path;
        }(),
        high_score
    };
}


#include "apriltag_ros/apriltag_pose_estimator.h"
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <boost/thread/lock_guard.hpp>
#include <geometry_msgs/TransformStamped.h>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <XmlRpcException.h>

namespace apriltag_ros {

namespace am = apriltag_msgs;
namespace ig = image_geometry;

cv::Mat QuatFromRvec(const cv::Mat &r) {
  // theta = norm(r)
  // q = [cos(theta/2), sin(theta/2) * r / theta]

  cv::Mat q = cv::Mat::zeros(cv::Size(1, 4), CV_64F);
  auto *pq = q.ptr<double>();
  const auto *pr = r.ptr<double>();
  const auto rx = pr[0], ry = pr[1], rz = pr[2];
  const double theta = std::sqrt(rx * rx + ry * ry + rz * rz);
  if (theta < std::numeric_limits<double>::epsilon() * 10.0) {
    pq[0] = 1.0;
    return q;
  }

  const double half_theta = theta * 0.5;
  const double half_sin = std::sin(half_theta);
  const double half_cos = std::cos(half_theta);
  const double k = half_sin / theta;
  pq[0] = half_cos;
  pq[1] = k * rx;
  pq[2] = k * ry;
  pq[3] = k * rz;

  return q;
}

ApriltagPoseEstimator::ApriltagPoseEstimator(const ros::NodeHandle &pnh)
    : pnh_(pnh) {

  pnh_.param("broadcast_tf", broadcast_tf_, false);
  bool auto_disconnect = true;
  pnh_.param("auto_disconnect", auto_disconnect, true);

  sub_cinfo_ = pnh_.subscribe("camera_info", 1, &ApriltagPoseEstimator::CinfoCb, this);

  if(auto_disconnect){
    auto connect_cb = boost::bind(&ApriltagPoseEstimator::ConnectCb, this);
    boost::lock_guard<boost::mutex> lock(connect_mutex_);
    pub_poses_ = pnh_.advertise<apriltag_msgs::ApriltagPoseStamped>("apriltag_poses", 1, connect_cb, connect_cb);
  }
  else{
    pub_poses_ = pnh_.advertise<apriltag_msgs::ApriltagPoseStamped>("apriltag_poses", 1);
    ROS_WARN("%s: Subscribing", pnh_.getNamespace().c_str());
    sub_apriltags_ = pnh_.subscribe("apriltags", 1,
                                    &ApriltagPoseEstimator::ApriltagsCb, this);
  }

  InitApriltagMap();
}

void ApriltagPoseEstimator::ConnectCb() {
  boost::lock_guard<boost::mutex> lock(connect_mutex_);
  if (pub_poses_.getNumSubscribers() == 0) {
    ROS_WARN("%s: No subscribers, shutting down", pnh_.getNamespace().c_str());
    sub_apriltags_.shutdown();
  } else if (!sub_apriltags_) {
    ROS_WARN("%s: Resubscribing", pnh_.getNamespace().c_str());
    sub_apriltags_ = pnh_.subscribe("apriltags", 1,
                                    &ApriltagPoseEstimator::ApriltagsCb, this);
  }
}

void ApriltagPoseEstimator::ApriltagsCb(const am::ApriltagArrayStampedConstPtr &apriltags_msg) {
  if (!cam_model_.initialized() || map_.empty()) return;

  am::ApriltagPoseStamped apriltag_poses;

  for (const am::Apriltag &apriltag : apriltags_msg->apriltags) {
    auto search = map_.find(apriltag.id);
    if (search != map_.end()) {

      const auto des_pair = search->second;
      const am::Apriltag &map_tag = des_pair.first;

      std::vector<cv::Point2d> img_pts;
      // Add points to img_pts
      for (const auto &c : apriltag.corners) {
        img_pts.emplace_back(c.x, c.y);
      }

      std::vector<cv::Point3d> obj_pts;
      // Add points to obj_pts
      for (const auto &c : map_tag.corners) {
        obj_pts.emplace_back(c.x, c.y, c.z);
      }

      // Check if empty
      if (img_pts.empty()) continue;

      cv::Mat rvec, tvec;
      sensor_msgs::CameraInfo cinfo = cam_model_.cameraInfo();
      if(cinfo.distortion_model == "fisheye") {

        std::vector<cv::Point2d> undistorted_pts;
        cv::fisheye::undistortPoints(img_pts, undistorted_pts, cam_model_.fullIntrinsicMatrix(), cam_model_.distortionCoeffs());

        cv::Mat normalized_cam_mat = cv::Mat::eye(3,3,cv::DataType<double>::type);
        cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<double>::type);
        auto good = cv::solvePnP(obj_pts, undistorted_pts, normalized_cam_mat, dist_coeffs, rvec, tvec);
        if (!good) {
          ROS_WARN("%s: Pose solver failed.", pnh_.getNamespace().c_str());
          continue;
        }
      }
      else {
        // Solve for pose
        // The estiamted r and t brings points from tag frame to camera frame
        // r = c_r_w, t = c_t_w
        auto good = cv::solvePnP(obj_pts, img_pts, cam_model_.fullIntrinsicMatrix(),
                                 cam_model_.distortionCoeffs(), rvec, tvec);
        if (!good) {
          ROS_WARN("%s: Pose solver failed.", pnh_.getNamespace().c_str());
          continue;
        }
      }

      cv::Matx33d r;
      cv::Rodrigues(rvec, r);

      tf2::Matrix3x3 wRo(r(0,0), r(0,1), r(0,2), r(1,0), r(1,1), r(1,2), r(2,0), r(2,1), r(2,2));
      tf2::Quaternion tf_quat;
      wRo.getRotation(tf_quat);

      geometry_msgs::Pose pose_msg;
      pose_msg.position.x = tvec.at<double>(0);
      pose_msg.position.y = tvec.at<double>(1);
      pose_msg.position.z = tvec.at<double>(2);
      pose_msg.orientation.x = tf_quat.x();
      pose_msg.orientation.y = tf_quat.y();
      pose_msg.orientation.z = tf_quat.z();
      pose_msg.orientation.w = tf_quat.w();

      if(broadcast_tf_) {

        AprilTagDescription description = des_pair.second;
        std::string child_frame_id = description.frame_name();
        if(child_frame_id.empty())
          child_frame_id = "tag_" + std::to_string(apriltag.id);

        geometry_msgs::TransformStamped transformStamped;
        transformStamped.header = apriltags_msg->header;
        transformStamped.child_frame_id = child_frame_id;

        transformStamped.transform.translation.x = tvec.at<double>(0);
        transformStamped.transform.translation.y = tvec.at<double>(1);
        transformStamped.transform.translation.z = tvec.at<double>(2);

        transformStamped.transform.rotation.x = tf_quat.x();
        transformStamped.transform.rotation.y = tf_quat.y();
        transformStamped.transform.rotation.z = tf_quat.z();
        transformStamped.transform.rotation.w = tf_quat.w();

        tf2_br_.sendTransform(transformStamped);
      }

      apriltag_poses.apriltags.push_back(apriltag);
      apriltag_poses.posearray.poses.push_back(pose_msg);
    }
  }

  // The poses are apriltags expressed in camera frame
  apriltag_poses.header = apriltags_msg->header;
  apriltag_poses.posearray.header = apriltag_poses.header;
  if(apriltag_poses.apriltags.size()>0)
    pub_poses_.publish(apriltag_poses);
}

void ApriltagPoseEstimator::InitApriltagMap() {

  /*
  // Construct a map of strings
  std::map<std::string,double> map_tags;
  pnh_.getParam("tag_descriptions", map_tags);

  //Return if no tags are specified on parameter server.
  if(map_tags.empty())
    return;
  */

  std::map<int, AprilTagDescription> descriptions;

  XmlRpc::XmlRpcValue april_tag_descriptions;
  if(!pnh_.getParam("tag_descriptions", april_tag_descriptions)){
    ROS_WARN("No april tags specified");
    return;
  }
  else{
    try{
      descriptions = parse_tag_descriptions(april_tag_descriptions);
    } catch(XmlRpc::XmlRpcException e){
      ROS_ERROR_STREAM("Error loading tag descriptions: "<<e.getMessage());
    }
  }

  std::map<int,AprilTagDescription>::iterator description_itr;
  for(description_itr = descriptions.begin(); description_itr != descriptions.end(); description_itr++) {

    AprilTagDescription description = description_itr->second;
    double tag_size = description.size();

    am::Apriltag tag;
    tag.family = "mit"; //TODO make these params? Or get from detections?
    tag.border = 1;
    tag.bits = 6;

    tag.id = description_itr->first;
    tag.center.x = 0.0;
    tag.center.y = 0.0;
    tag.center.z = 0.0;

    double s = tag_size/2.0;
    geometry_msgs::Point pt;
    pt.x = -s; pt.y = -s; pt.z = 0.0; tag.corners[0] = pt;
    pt.x =  s; pt.y = -s; pt.z = 0.0; tag.corners[1] = pt;
    pt.x =  s; pt.y =  s; pt.z = 0.0; tag.corners[2] = pt;
    pt.x = -s; pt.y =  s; pt.z = 0.0; tag.corners[3] = pt;

    //TODO Only save the frame_id from description as string?
    auto des = std::make_pair(tag, description);
    map_.insert(std::pair<int, std::pair<am::Apriltag, AprilTagDescription> >(tag.id, des) );

    ROS_INFO_STREAM("tag is: " << tag);
  }

  ROS_INFO("%s: apritlag map initialized", pnh_.getNamespace().c_str());
}

std::map<int, AprilTagDescription> ApriltagPoseEstimator::parse_tag_descriptions(XmlRpc::XmlRpcValue& tag_descriptions){
  std::map<int, AprilTagDescription> descriptions;
  ROS_ASSERT(tag_descriptions.getType() == XmlRpc::XmlRpcValue::TypeArray);
  for (int32_t i = 0; i < tag_descriptions.size(); ++i) {
    XmlRpc::XmlRpcValue& tag_description = tag_descriptions[i];
    ROS_ASSERT(tag_description.getType() == XmlRpc::XmlRpcValue::TypeStruct);
    ROS_ASSERT(tag_description["id"].getType() == XmlRpc::XmlRpcValue::TypeInt);
    ROS_ASSERT(tag_description["size"].getType() == XmlRpc::XmlRpcValue::TypeDouble);

    int id = (int)tag_description["id"];
    double size = (double)tag_description["size"];

    std::string frame_name;
    if(tag_description.hasMember("frame_id")){
      ROS_ASSERT(tag_description["frame_id"].getType() == XmlRpc::XmlRpcValue::TypeString);
      frame_name = (std::string)tag_description["frame_id"];
    }
    else{
      std::stringstream frame_name_stream;
      frame_name_stream << "tag_" << id;
      frame_name = frame_name_stream.str();
    }
    AprilTagDescription description(id, size, frame_name);
    ROS_INFO_STREAM("Loaded tag config: "<<id<<", size: "<<size<<", frame_name: "<<frame_name);
    descriptions.insert(std::make_pair(id, description));
  }
  return descriptions;
}

void ApriltagPoseEstimator::CinfoCb(
    const sensor_msgs::CameraInfoConstPtr &cinfo_msg) {
  if (!cam_model_.initialized()) {
    cam_model_.fromCameraInfo(cinfo_msg);
    ROS_INFO("%s: camera model initialized", pnh_.getNamespace().c_str());
    sub_cinfo_.shutdown();
  }
}

}  // namespace arpiltag_ros

int main(int argc, char **argv) {
  ros::init(argc, argv, "apriltag_pose_estimator");
  ros::NodeHandle pnh("~");

  try {
    apriltag_ros::ApriltagPoseEstimator node(pnh);
    ros::spin();
  } catch (const std::exception &e) {
    ROS_ERROR("%s: %s", pnh.getNamespace().c_str(), e.what());
  }
}


#include <armadillo>
#include <hr_time.h>
#include <cuda.h>
#include <cuda_runtime.h>
#include <cublas_v2.h>
#include <stdio.h>
#include "machine_info.h"
#include "model_wpam_3d_trajectory/model_wpam_3d.h"
#include "model_wpam_gpu/model_wpam_gpu.h"

using namespace arma;

void callCartToPolarTransformation(float* x_dev, int dim, int num, float* result_dev);
void callPolarToCartTransformation(float* x_dev, int dim, int num, float* result_dev);
int runMultTest(int dim, int num);
int runTransformationTest(int num);
int runWPAMTest(int num);

void printCUBLASError(cublasStatus_t error){
	if (error == CUBLAS_STATUS_NOT_INITIALIZED) printf("CUBLAS_STATUS_NOT_INITIALIZED\n");
	if (error == CUBLAS_STATUS_ALLOC_FAILED) printf("CUBLAS_STATUS_ALLOC_FAILED\n");
	if (error == CUBLAS_STATUS_INVALID_VALUE) printf("CUBLAS_STATUS_INVALID_VALUE\n");
	if (error == CUBLAS_STATUS_ARCH_MISMATCH) printf("CUBLAS_STATUS_ARCH_MISMATCH\n");
	if (error == CUBLAS_STATUS_MAPPING_ERROR) printf("CUBLAS_STATUS_MAPPING_ERROR\n");
	if (error == CUBLAS_STATUS_EXECUTION_FAILED) printf("CUBLAS_STATUS_EXECUTION_FAILED\n");
	if (error == CUBLAS_STATUS_INTERNAL_ERROR) printf("CUBLAS_STATUS_INTERNAL_ERROR\n");
}

int main (int argc, char* argv[])
{
	int dim=1;
	int num=1;
	int error = 0;

	MachineInformation informations;
	informations.printMachineInformation();

	if (argc != 3)
	{
        printf("MULTIPLICATION TESTS\n");
        runMultTest(3,100);
        runMultTest(3,500);
        runMultTest(3,2000);
        runMultTest(3,10000);

        runMultTest(9,100);
        runMultTest(9,500);
        runMultTest(9,2000);
        runMultTest(9,10000);

        runMultTest(15,100);
        runMultTest(15,500);
        runMultTest(15,2000);
        runMultTest(15,10000);

        printf("TRANSFORMATION TESTS\n");
        runTransformationTest(100);
        runTransformationTest(500);
        runTransformationTest(2000);
        runTransformationTest(10000);

        printf("WPAM TESTS\n");
		runWPAMTest(100);
		runWPAMTest(500);
		runWPAMTest(2000);
		runWPAMTest(10000);
	}
	else {
		dim=atoi(argv[1]);
		num=atoi(argv[2]);
        runMultTest(dim, num);
        runTransformationTest(num);
        runWPAMTest(num);
	}

    // run sorting test

    // run model with coordinate transformation

	return error;
}

int runMultTest(int dim, int num){
    CStopWatch timer;
    int devicecount = 0;
    int device = 0;
    printf("\n\nNumber of dimensions: %i\n",dim);
    printf("Number of particles: %i\n",num);
    unsigned int runs = 400;
    fmat x = randu<fmat>(dim,num);
    fmat y = randu<fmat>(num,dim);
    fmat result = zeros<fmat>(dim,num);

    // CPU test
    timer.startTimer();

    for (unsigned int i=0;i<runs;++i)
    {
        result = x * y;
    }

    timer.stopTimer();

    printf("CPU matrix-matrix multiplication done in %e seconds\n", (double)timer.getElapsedTime()/runs);
    //result.print();

    cudaGetDeviceCount(&devicecount);
    for (device = 0; device < devicecount; ++device)
    {
        cudaSetDevice(device);
        printf("GPU device id %i (no. %i of %i)\n", device, device+1, devicecount );

        // GPU test only one initialization
        //reset result
        result = zeros<fmat>(x.n_rows,y.n_cols);
        timer.startTimer();

        float* devX;
        float* devY;
        float* devResult;
        const float alpha = 1.0f;
        const float beta = 0.0f;
        cublasHandle_t  handle;
        cublasStatus_t stat;
        cudaError_t cudaStat;

        cudaStat = cudaMalloc((void**)&devX, x.n_rows* x.n_cols * sizeof(float));
        if ( cudaStat != cudaSuccess )
        {
            printf ( "device memory allocation failed\n" ) ;
            return -1;
        }
        cudaStat = cudaMalloc((void**)&devY, y.n_elem * sizeof(float));
        if ( cudaStat != cudaSuccess )
        {
            printf ( "device memory allocation failed\n" ) ;
            return -1;
        }
        cudaStat = cudaMalloc((void**)&devResult, x.n_rows*y.n_cols * sizeof(float));
        if ( cudaStat != cudaSuccess )
        {
            printf ( "device memory allocation failed\n" ) ;
            return -1;
        }
        stat = cublasCreate(&handle);
        if ( stat != CUBLAS_STATUS_SUCCESS) {
            printf ( "CUBLAS initialization failed\n" ) ;
            printCUBLASError(stat);
            return -1 ;
        }

        for (unsigned int i=0;i<runs;++i)
        {
            stat = cublasSetMatrix(x.n_rows, x.n_cols, sizeof(float), x.memptr(), x.n_rows, devX, x.n_rows);
            if ( stat != CUBLAS_STATUS_SUCCESS) {
                printf ( "CUBLAS host to device transfer failed X\n" ) ;
                printCUBLASError(stat);
                return -1 ;
            }
            stat = cublasSetMatrix(y.n_rows, y.n_cols, sizeof(float), y.memptr(), y.n_rows, devY, y.n_rows);
            if ( stat != CUBLAS_STATUS_SUCCESS) {
                printf ( "CUBLAS host to device transfer failed Y\n" ) ;
                printCUBLASError(stat);
                return -1 ;
            }

            stat = cublasSgemm(handle, CUBLAS_OP_N, CUBLAS_OP_N, x.n_rows, y.n_cols, x.n_cols, &alpha, devX, x.n_rows, devY, y.n_rows, &beta, devResult, x.n_rows);
            if ( stat != CUBLAS_STATUS_SUCCESS) {
                printf ( "CUBLAS calculation failed\n" ) ;
                printCUBLASError(stat);
                return -1 ;
            }

            stat = cublasGetMatrix(x.n_rows, y.n_cols, sizeof(float), devResult, x.n_rows, result.memptr(), x.n_rows);
            if ( stat != CUBLAS_STATUS_SUCCESS) {
                printf ( "CUBLAS device to host transfer failed\n" ) ;
                printCUBLASError(stat);
                return -1 ;
            }
        }

        cudaFree(devX);
        cudaFree(devY);
        cudaFree(devResult);
        cublasDestroy(handle);

        timer.stopTimer();

        printf("GPU matrix-matrix multiplication done in %e seconds\n", (double)timer.getElapsedTime()/runs);
        //result.print();

        // GPU test everytime new initialization

        //reset result
        result = zeros<fmat>(x.n_rows,y.n_cols);
        timer.startTimer();

        for (unsigned int i=0;i<runs;++i)
        {
            cudaMalloc((void**)&devX, x.n_elem * sizeof(float));
            cudaMalloc((void**)&devY, y.n_elem * sizeof(float));
            cudaMalloc((void**)&devResult, x.n_rows*y.n_cols * sizeof(float));
            cublasCreate(&handle);

            stat = cublasSetMatrix(x.n_rows, x.n_cols, sizeof(float), x.memptr(), x.n_rows, devX, x.n_rows);
            if ( stat != CUBLAS_STATUS_SUCCESS) {
                printf ( "CUBLAS host to device transfer failed X\n" ) ;
                printCUBLASError(stat);
                return -1 ;
            }
            stat = cublasSetMatrix(y.n_rows, y.n_cols, sizeof(float), y.memptr(), y.n_rows, devY, y.n_rows);
            if ( stat != CUBLAS_STATUS_SUCCESS) {
                printf ( "CUBLAS host to device transfer failed Y\n" ) ;
                printCUBLASError(stat);
                return -1 ;
            }

            stat = cublasSgemm(handle, CUBLAS_OP_N, CUBLAS_OP_N, x.n_rows, y.n_cols, x.n_cols, &alpha, devX, x.n_rows, devY, y.n_rows, &beta, devResult, x.n_rows);
            if ( stat != CUBLAS_STATUS_SUCCESS) {
                printf ( "CUBLAS calculation failed Y\n" ) ;
                printCUBLASError(stat);
                return -1 ;
            }

            stat = cublasGetMatrix(x.n_rows, y.n_cols, sizeof(float), devResult, x.n_rows, result.memptr(), x.n_rows);
            if ( stat != CUBLAS_STATUS_SUCCESS) {
                printf ( "CUBLAS device to host transfer failed\n" ) ;
                printCUBLASError(stat);
                return -1 ;
            }

            cudaFree(devX);
            cudaFree(devY);
            cudaFree(devResult);
            cublasDestroy(handle);
        }

        timer.stopTimer();

        printf("pessimistic matrix-matrix GPU multiplication done in %e seconds\n", (double)timer.getElapsedTime()/runs);

    }

    device = 0;
    devicecount = 0;
    return 0;
}

int runTransformationTest(int num){
    int dim = 2;
    CStopWatch timer;
    int devicecount = 0;
    int device = 0;
    printf("\n\nNumber of dimensions: %i\n",dim);
    printf("Number of particles: %i\n",num);
    unsigned int runs = 400;
    fmat x = randu<fmat>(dim,num);
    fmat result = zeros<fmat>(dim,num);
    fmat temp;

    // CPU test
    //x.print();
    timer.startTimer();

    for (unsigned int i=0;i<runs;++i)
    {
        for (unsigned j = 0; j < result.n_cols; ++j)
        {
            result(0,j) = sqrt(x(0,j)*x(0,j) + x(1,j)*x(1,j));
            result(1,j) = atan2(x(1,j), x(0,j));

            temp = result;

            result(0,j) = temp(0,j)* cos( temp(1,j) );
            result(1,j) = temp(0,j)* sin( temp(1,j) );

        }
    }

    timer.stopTimer();

    printf("CPU cartesian to polar coord. transformation done in %e seconds\n", (double)timer.getElapsedTime()/runs);
    //result.print();

    cudaGetDeviceCount(&devicecount);
    for (device = 0; device < devicecount; ++device)
    {
        cudaSetDevice(device);
        printf("GPU device id %i (no. %i of %i)\n", device, device+1, devicecount );

        // GPU test only one initialization
        //reset result
        result = zeros<fmat>(dim, num);
        timer.startTimer();

        float* devX;
        float* devResult;
        cudaError_t cudaStat;

        cudaStat = cudaMalloc((void**)&devX, dim* num * sizeof(float));
        if ( cudaStat != cudaSuccess )
        {
            printf ( "device memory allocation failed\n" ) ;
            return -1;
        }

        cudaStat = cudaMalloc((void**)&devResult, dim*num * sizeof(float));
        if ( cudaStat != cudaSuccess )
        {
            printf ( "device memory allocation failed\n" ) ;
            return -1;
        }

        float* temp;
        for (unsigned int i=0;i<runs;++i)
        {
            cudaMemcpy(devX,x.memptr(),(size_t) x.n_elem * sizeof(float), cudaMemcpyHostToDevice);
            callCartToPolarTransformation(devX, dim, num, devResult);
            temp = devX;
            devX = devResult;
            devResult = temp;
            callPolarToCartTransformation(devX, dim, num, devResult);
            cudaMemcpy(result.memptr(),devResult,result.n_elem * sizeof(float), cudaMemcpyDeviceToHost);
        }

        cudaFree(devX);
        cudaFree(devResult);

        timer.stopTimer();

        //result.print();

        printf("GPU cartesian to polar coord. transformation done in %e seconds with %s\n",
               (double)timer.getElapsedTime()/runs, cudaGetErrorString(cudaGetLastError()));
        //result.print();

    }

    device = 0;
    devicecount = 0;
    return 0;
}

int runWPAMTest(int num){
    int dim = 9;
    CStopWatch timer;
    int devicecount = 0;
    int device = 0;
    printf("\n\nNumber of dimensions: %i\n",dim);
    printf("Number of particles: %i\n",num);
    unsigned int runs = 400;
    fmat x = randu<fmat>(dim,num);
    fmat result = zeros<fmat>(dim,num);
	frowvec evaluation = zeros<frowvec>(num);
    fmat temp;

    // CPU test
    //x.print();
    ModelWPAM cpuModel;
    cpuModel.initialize();
    timer.startTimer();
    for (unsigned int i=0;i<runs;++i)
    {
        result = cpuModel.ffun(&x);
        result = cpuModel.hfun(&result);
		evaluation = cpuModel.eval(&result);
    }

    timer.stopTimer();

    printf("CPU WPAM calculation done in %e seconds\n", (double)timer.getElapsedTime()/runs);
    //result.print();

    cudaGetDeviceCount(&devicecount);
    for (device = 0; device < devicecount; ++device)
    {
        cudaSetDevice(device);
        printf("GPU device id %i (no. %i of %i)\n", device, device+1, devicecount );

        // GPU test only one initialization
        //reset result
        result = zeros<fmat>(dim, num);
        ModelWPAMGPU gpgpuModel;
        gpgpuModel.initialize();
        timer.startTimer();

        for (unsigned int i=0;i<runs;++i)
        {
            result = gpgpuModel.ffun(&x);

            result = gpgpuModel.hfun(&result);

			evaluation = gpgpuModel.eval(&result);
        }

        timer.stopTimer();

        //result.print();

        printf("GPU WPAM calculation done in %e seconds with %s\n",
               (double)timer.getElapsedTime()/runs, cudaGetErrorString(cudaGetLastError()));
        //result.print();

    }

	cudaGetDeviceCount(&devicecount);
    for (device = 0; device < devicecount; ++device)
    {
        cudaSetDevice(device);
        printf("GPU device id %i (no. %i of %i)\n", device, device+1, devicecount );

        // GPU test only one initialization
        //reset result
        result = zeros<fmat>(dim, num);
        ModelWPAMGPU gpgpuModel;
        gpgpuModel.initialize();
        timer.startTimer();

        for (unsigned int i=0;i<runs;++i)
        {
			float* temp;
            temp = gpgpuModel.ffun_gpu(&x);

			temp = gpgpuModel.hfun_gpu(temp,(int)result.n_cols, (int)result.n_rows);

			evaluation = gpgpuModel.eval_gpu(temp, (int)result.n_cols);

			cudaFree(temp);
        }

        timer.stopTimer();

        //result.print();

        printf("GPU pointer WPAM calculation done in %e seconds with %s\n",
               (double)timer.getElapsedTime()/runs, cudaGetErrorString(cudaGetLastError()));
        //result.print();

    }

    device = 0;
    devicecount = 0;
    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_IMPL_LOGS_D_LOG_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SIMD_COMMON_IMPL_LOGS_D_LOG_HPP_INCLUDED

#include <nt2/include/functions/simd/minusone.hpp>
#include <nt2/include/functions/simd/tofloat.hpp>
#include <nt2/include/functions/simd/sqr.hpp>
#include <nt2/include/functions/simd/unary_minus.hpp>
#include <nt2/include/functions/simd/is_greater.hpp>
#include <nt2/include/functions/simd/is_ltz.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#include <nt2/include/functions/simd/fast_frexp.hpp>
#include <nt2/include/functions/simd/seladd.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/bitwise_and.hpp>
#include <nt2/include/functions/simd/logical_or.hpp>
#include <nt2/include/functions/simd/if_allbits_else.hpp>
#include <nt2/include/functions/simd/if_else_zero.hpp>
#include <nt2/include/functions/simd/is_eqz.hpp>
#include <nt2/polynomials/functions/scalar/impl/horner.hpp>
#include <nt2/include/constants/invlog_2.hpp>
#include <nt2/include/constants/invlog_10.hpp>
#include <nt2/include/constants/log_2olog_10.hpp>
#include <nt2/include/constants/log_2hi.hpp>
#include <nt2/include/constants/log_2lo.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/sqrt_2o_2.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/minf.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>

#ifndef BOOST_SIMD_NO_NANS
#include <nt2/include/functions/simd/is_nan.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#endif

namespace nt2 { namespace details
{
  //////////////////////////////////////////////////////////////////////////////
  // math log functions
  //////////////////////////////////////////////////////////////////////////////

  template < class A0 >
  struct logarithm< A0, tag::simd_type, double>
  {
    static inline void kernel_log(const A0& a0,
                                  A0& dk,
                                  A0& hfsq,
                                  A0& s,
                                  A0& R,
                                  A0& f)
    {
      typedef typename meta::as_integer<A0, signed>::type int_type;
      typedef typename meta::scalar_of<A0>::type               sA0;
      int_type k;
      A0 x(nt2::fast_frexp(a0, k));
      const int_type x_lt_sqrthf = nt2::if_else_zero(nt2::gt(nt2::Sqrt_2o_2<A0>(), x),
                                                     nt2::Mone<int_type>());
      k = k+x_lt_sqrthf;
      f = nt2::minusone(x+nt2::bitwise_and(x, x_lt_sqrthf));
      dk = nt2::tofloat(k);
      s = f/nt2::add(Two<A0>(),f);
      A0 z = nt2::sqr(s);
      A0 w = nt2::sqr(z);
      A0 t1= w*nt2::horner<NT2_HORNER_COEFF_T(sA0, 3,
                                         (0x3fc39a09d078c69fll,
                                          0x3fcc71c51d8e78afll,
                                          0x3fd999999997fa04ll)
                                        )> (w);
      A0 t2= z*nt2::horner<NT2_HORNER_COEFF_T(sA0, 4,
                                         (0x3fc2f112df3e5244ll,
                                          0x3fc7466496cb03dell,
                                          0x3fd2492494229359ll,
                                          0x3fe5555555555593ll)
                                        )> (w);
      R = t2+t1;
      hfsq = nt2::mul(nt2::Half<A0>(), nt2::sqr(f));
    }

    static inline A0 log(const A0& a0)
    {
      A0 dk, hfsq, s, R, f;
      kernel_log(a0, dk, hfsq, s, R, f);
      A0 y =  nt2::mul(dk, Log_2hi<A0>())-
        ((hfsq-(s*(hfsq+R)+nt2::mul(dk,Log_2lo<A0>())))-f);
      return finalize(a0, y);
    }

    static inline A0 log2(const A0& a0)
    {
      A0 dk, hfsq, s, R, f;
      kernel_log(a0, dk, hfsq, s, R, f);
      A0 y = -(hfsq-(s*(hfsq+R))-f)*nt2::Invlog_2<A0>()+dk;
      return finalize(a0, y);
    }

    static inline A0 log10(const A0& a0)
    {
      A0 dk, hfsq, s, R, f;
      kernel_log(a0, dk, hfsq, s, R, f);
      A0 y = -(hfsq-(s*(hfsq+R))-f)*nt2::Invlog_10<A0>()+dk*nt2::Log_2olog_10<A0>();
      return finalize(a0, y);
    }
  private:
    static inline A0 finalize(const A0& a0, const A0& y)
    {
      typedef typename meta::as_logical<A0>::type              lA0;
    #ifdef BOOST_SIMD_NO_NANS
      lA0 test =  nt2::is_ltz(a0);
    #else
      lA0 test =  nt2::logical_or(nt2::is_ltz(a0), nt2::is_nan(a0));
    #endif
      A0 y1 = nt2::if_nan_else(test, y);
    #ifndef BOOST_SIMD_NO_INFINITIES
      y1 = if_else(nt2::eq(a0, nt2::Inf<A0>()), a0, y1);
    #endif
      return if_else(is_eqz(a0), nt2::Minf<A0>(), y1);
    }
  };

} }

#endif