/*!
  \file auto_axes.hpp
  \brief Scalable Vector Graphic (SVG) autoscaling of axes.
  \details Inspect container or data values to find minimum and maximum,
    avoiding values that are NaN and/or 'at limit'.
    Scale axis using max and min values (calculated or user provided),
    optionally to include the orgin, and to set the ticks.
    Provide fine control over any overlap at the edges of the axes to avoid a tiny
    amount over the limit resulting in an ugly extra major tick.
    Also allow optional forcing of the ticks to be multiples of 1, 2, 5, 10.
  \author
*/

// Copyright Paul A. Bristow 2006 - 2013, 2021.

// Use, modification and distribution are subject to 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_SVG_AUTO_AXES_HPP
#define BOOST_SVG_AUTO_AXES_HPP

#include <boost/svg_plot/detail/numeric_limits_handling.hpp>
#include <boost/svg_plot/detail/fp_compare.hpp> // is_small & is_close
#include <boost/quan/meas.hpp> // for boost::quan::value_of.

//#include <boost/math/special_functions/fpclassify.hpp>
// for template <class FPT> bool boost::math::isfinite(FPT t);
#include <boost/algorithm/minmax_element.hpp>
 using boost::minmax_element;
 // minmax_element finds both min and max elements more efficiently than separately.

#include <cmath> // using std::fabs, std::pow, std::ceil, std::log10
#include <limits> // using std::numeric_limits;
#include <stdexcept> // using std::domain_error; sdtd::runtime_error;
#include <iterator> // using std::iterator_traits;
#include <utility> // using std::pair; using std::make_pair;

// Autoscaling algorithm derived from:
// Michael P.D. Bramley. CUJ July 2000, p 20 - 26.
// Antonio Gomiz Bas, CUJ march 2000, p 42 - 45
// J. A. Nelder and W. Douglas Stirling, FORTRAN program SCALE.
//  // Algorithm AS 168: Scale Selection and Formatting  W.Douglas Stirling
// Journal of the Royal Statistical Society.Series C(Applied Statistics)
// Vol. 30, No. 3 (1981), pp. 339-344 (6 pages)
// Published By: Wiley
// Journal of the Royal Statistical Society.Series C(Applied Statistics)
// https://doi.org/10.2307/2346366
// https://www.jstor.org/stable/2346366
// Algorithm AS 96 https://doi.org/10.2307/2346537 J. A. Nelder, Simple Algorithm for Scaling Graphs
// http://lib.stat.cmu.edu/apstat/96   FORTRAN code

namespace boost
{
namespace svg
{

// Forward Declarations:

// Show and show_all to display size and contents of STL containers.
// range_mx and range_all to find the min and max of STL containers.
// _all versions deal with a container of containers.

// Round value up to nearest multiple of 10. Round up and round down to 2, 4, 6, 8, 10, or 5, 10 or 2, 5, 10 systems:
//
double roundup10(double value);
 // Round value down to nearest multiple of 10.
double rounddown10(double value);
// Round value up to nearest multiple of 5.
double roundup5(double value);
 // Round value down to nearest multiple of 5.
double rounddown5(double value);
 // Round value up to nearest multiple of 2.
double roundup2(double value);
// Round value down to nearest multiple of 2.
double rounddown2(double value);

// Four versions of scale_axis with different parameter combinations.

// Scale axis and update min and max axis values, and tick increment and number of ticks.
void scale_axis(
   double min_value, //!< Scale axis from explicit input minimum.
   double max_value, //!< Scale axis from explicit input maximum.
   double* axis_min_value, //!< Computed minimum value for the axis, updated by scale_axis.
   double* axis_max_value, //!<  Computed maximum value for the axis, updated by scale_axis.
   double* axis_tick_increment, //!<  Computed tick increment for the axis, updated by scale_axis.
   int* auto_ticks, //!< Computed number of ticks, updated by scale_axis.
   //  NO check_limits parameter.
   bool origin = false, //!< Do not include the origin unless the range min_value <= 0 <= max_value.
   double tight = 0., //!< Tightness - fraction of overrun allowed before another tick used. For visual effect up to about 0.001 might suit a 1000 pixel wide image, allowing values just 1 pixel over the tick to be shown.
   int min_ticks = 6, //!< Minimum number of major ticks.
   int steps = 0 //!< Round up and down to 2, 4, 6, 8, 10, or 5, 10 or 2, 5, 10 systems.
);

/* Scale axis function to define axis marker ticks based on min & max parameters values (handling uncertainty).

  \param min_value Scale axis from explicit input minimum.
  \param max_value Scale axis from explicit input maximum.

  \param axis_min_value Computed minimum value for the axis, updated by scale_axis.
  \param axis_max_value Computed maximum value for the axis, updated by scale_axis.
  \param axis_tick_increment Computed tick increment for the axis, updated by scale_axis.
  \param auto_ticks  Computed number of ticks, updated by scale_axis.
  \param check_limits  If true then check all values for infinity, NaN etc.
  \param autoscale_plusminus Multiplier of uncertainty or standard deviations to allow for confidence ellipses.
  \param origin If true, ensures that zero is a tick value.
  \param tight  Allows user to avoid a small fraction over a tick using another tick.
  \param min_ticks  Minimum number of major ticks.
  \param steps Round up and down to 2, 4, 6, 8, 10, or 5, 10 or 2, 5, 10 systems.
 */
void scale_axis(
  double min_value, // Scale axis from explicit input minimum.
  double max_value, // Scale axis from explicit input maximum.
  double* axis_min_value, // Minimum value for the axis, updated by scale_axis.
  double* axis_max_value, //  Maximum value for the axis, updated by scale_axis.
  double* axis_tick_increment, //  Tick increment for the axis, updated by scale_axis.
  int* auto_ticks,  // Updated with number of ticks.
  bool check_limits = true, // Whether to check all values for infinity, NaN etc.
  double autoscale_plusminus = 2., // Multiplier of uncertainty or standard deviations to allow for confidence ellipses.
  bool origin = false, // If true, ensures that zero is a tick value.
  double tight = 0., // Allows user to avoid a small fraction over a tick using another tick.
  int min_ticks = 6, // Minimum number of ticks.
  int steps = 0 // Round up and down to 2, 4, 6, 8, 10, or 5, 10, or 2, 5, 10 major ticks multiples.
);

/* Scale axis using an iterator into an STL container.

 \details Scale axis from data-series values (usually to then plot), perhaps using only part of container.

 \tparam iter Iterator into an STL container: array, vector, set ...
 \param begin Iterator into @c begin in STL container.
 \param end iterators into @c end in STL container.
 \param axis_min_value Computed minimum value for the axis, updated by scale_axis.
 \param axis_max_value Computed maximum value for the axis, updated by scale_axis.
 \param axis_tick_increment Computed tick increment for the axis, updated by scale_axis.
 \param auto_ticks  Computed number of ticks, updated by scale_axis.
 \param check_limits  If true then check all values for infinity, NaN etc.
 \param autoscale_plusminus Mutiplier of uncertainty or standard deviations to allow for confidence ellipses.
 \param origin If true, ensures that zero is a tick value.
 \param tight  Allows user to avoid a small fraction over a tick using another tick.
 \param min_ticks  Minimum number of major ticks.
 \param steps Round up and down to 2, 4, 6, 8, 10, or 5, 10 or 2, 5, 10 systems.
*/

template <typename Iter>
void scale_axis(
  Iter begin, // Iterator into begin in STL container.
  Iter end, // Iterator into end in STL container.
  // (not necessarily ordered by size, so will find min and max).
  double* axis_min_value, // Computed minimum value for the axis, updated by scale_axis.
  double* axis_max_value, //  Computed maximum value for the axis, updated by scale_axis.
  double* axis_tick_increment, //  Computed tick increment for the axis, updated by scale_axis.
  int* auto_ticks, // Computed number of ticks, updated by scale_axis.
  bool check_limits, // Whether to check all values for infinity, NaN etc.
  double autoscale_plusminus, // Mutiplier of uncertainty or standard deviations to allow for confidence ellipses.
  bool origin = false, // Do not include the origin unless the range min_value <= 0 <= max_value.
  double tight = 0., // tightest - fraction of 'overrun' allowed before another tick used.
  // for visual effect up to about 0.001 might suit a 1000 pixel wide image,
  // allowing values just 1 pixel over the tick to be shown.
  int min_ticks = 6, // Minimum number of major ticks.
  int steps = 0 // 0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
);

/* Scale axis using all the 1D data in an STL container.
  Scale axis using an \b entire Container data-series, usually to plot.
  (not necessarily ordered, so will find min and max).
  \tparam C an STL container: array, vector ...
  \param container STL container, usually of a data-series.
  \param axis_min_value Computed minimum value for the X-axis, updated by scale_axis.
  \param axis_max_value Computed minimum value for the X-axis, updated by scale_axis.
  \param axis_tick_increment Computed tick increment for the axis, updated by scale_axis.
  \param auto_ticks Computed number of ticks, updated by scale_axis.
  \param check_limits Whether to check all values for infinity, NaN etc.
  \param autoscale_plusminus Multiplier of uncertainty or standard deviations to allow for confidence ellipses.
  \param origin Do not include the origin unless the range min_value <= 0 <= max_value.
  \param tight fraction of 'overrun' allowed before another tick used. For visual effect up to about 0.001 might suit a 1000 pixel wide image, allowing values just 1 pixel over the tick to be shown.
  \param min_ticks Minimum number of major ticks.
  \param steps  0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
*/
 template <typename C>
 void scale_axis(
   const C& container, // STL container, usually of a data-series.
   double* axis_min_value, // Computed minimum value for the axis, updated by @c scale_axis.
   double* axis_max_value,  // Computed maximum value for the axis, updated by @c scale_axis.
   double* axis_tick_increment, //  Computed tick increment for the axis, updated by @c scale_axis.
   int* auto_ticks, // Computed number of ticks, updated by @c scale_axis.
   bool check_limits, // Whether to check all values for infinity, NaN etc.
   double autoscale_plusminus = 3., // Multiplier of uncertainty or standard deviations to allow for confidence ellipses.
   bool origin = false, // do not include the origin unless the range min_value <= 0 <= max_value.
   double tight = 0., // tightest - fraction of 'overrun' allowed before another tick used.
   //! for visual effect up to about 0.001 might suit a 1000 pixel wide image,
   //! allowing values just 1 pixel over the tick to be shown.
   int min_ticks = 6, // Minimum number of major ticks.
   int steps = 0 // 0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
);

/* Scale X and Y axis using T a 2D STL container: array of pairs, vector of pairs, list of pairs, map ...
  \tparam C STL container of 2D pairs of X and Y.

  \param container data-series to plot - entire 2D container (not necessarily ordered, so will find min and max)..
  \param x_axis_min_value Computed minimum value for the X-axis, updated by @c scale_axis.
  \param x_axis_max_value Computed minimum value for the X-axis, updated by @c scale_axis.
  \param x_axis_tick_increment Computed tick increment for the axis, updated @c by scale_axis.
  \param x_auto_ticks Computed number of ticks, updated by scale_axis.
  \param y_axis_min_value  Computed minimum value for the Y-axis, updated by @c scale_axis.
  \param y_axis_max_value  Computed maximum value for the Y-axis, updated by @c scale_axis.
  \param y_axis_tick_increment Updated with Y axis tick increment.
  \param y_auto_ticks Computed number of Y-axis ticks, updated by scale_axis.
  \param check_limits Whether to check all values for infinity, NaN etc.
  \param autoscale_plusminus Multiplier of uncertainty or standard deviations to allow for confidence ellipses.
  \param x_origin Do not include the origin unless the range min_value <= 0 <= max_value.
  \param x_tight fraction of 'overrun' allowed before another tick used. For visual effect up to about 0.001 might suit a 1000 pixel wide image, allowing values just 1 pixel over the tick to be shown.
  \param x_min_ticks Minimum number of X-axis major ticks.
  \param x_steps  0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
  \param y_origin  Do not include the origin unless the range min_value <= 0 <= max_value.
  \param y_tight fraction of 'overrun' allowed before another tick used. For visual effect up to about 0.001 might suit a 1000 pixel wide image, allowing values just 1 pixel over the tick to be shown.
  \param y_steps  0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
  \param y_min_ticks Minimum number of Y axis major ticks.
*/
template <typename C>
void scale_axis(
  const C& container, // data-series to plot - \b entire 2D container (not necessarily ordered, so will find min and max).
  double* x_axis_min_value, //  \param x_axis_min_value Computed minimum value for the X-axis, updated by scale_axis.
  double* x_axis_max_value,  //   \param x_axis_max_value Computed minimum value for the X-axis, updated by scale_axis.
  double* x_axis_tick_increment, // Updated with X axis tick increment.
  int* x_auto_ticks,   // Computed number of X axis ticks, updated by @c scale_axis.
  double* y_axis_min_value, // Computed minimum value for the Y-axis, updated by @c scale_axis.
  double* y_axis_max_value,  // Computed maximum value for the Y-axis, updated by @c scale_axis.
  double* y_axis_tick_increment, // Computed Y axis tick increment. updated by @c scale_axis.
  int* y_auto_ticks,  // Computed number of Y-axis ticks, updated by @c scale_axis.
  bool check_limits = true, // Whether to check all values for infinity, NaN etc.
  double autoscale_plusminus = 3., // Mutiplier of uncertainty or standard deviations to allow fo confidence ellipses.
  bool x_origin = false, // do not include the origin unless the range min_value <= 0 <= max_value.
  double x_tight = 0., // tightest - fraction of 'overrun' allowed before another tick used.
  //! for visual effect up to about 0.001 might suit a 1000 pixel wide image,
  //! allowing values just 1 pixel over the tick to be shown.
  int x_min_ticks = 6, // Minimum number of major ticks.
  int x_steps = 0, // 0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
  bool y_origin = false, // do not include the origin unless the range min_value <= 0 <= max_value.
  double y_tight = 0., // tightest - fraction of 'overrun' allowed before another tick used.
  // for visual effect up to about 0.001 might suit a 1000 pixel wide image,
  // allowing values just 1 pixel over the tick to be shown.
  int y_min_ticks = 6, // Minimum number of major ticks.
  int y_steps = 0 // 0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
);

//  End forward scale_axis and roundup and round down declarations.

// All use:
namespace detail
{ // Declaration of implementation with default parameters.
//* \cond DETAIL
void scale_axis_impl(double min_value, double max_value, // Scale axis from input range min & max.
               double* axis_min_value,  double* axis_max_value, double* axis_tick_increment,
               int* auto_ticks, // All 4 updated.
               // NO check_limits parameter in this version.
               bool origin = false, // Do not include the origin unless the range min_value <= 0 <= max_value.
               double tight = 0., // tightest - fraction of 'overrun' allowed before another tick used.
               // for visual effect up to about 0.001 might suit a 1000 pixel wide image,
               // allowing values just 1 pixel over the tick to be shown.
               int min_ticks = 6, // Minimum number of major ticks.
               int steps = 0); // 0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
//! \endcond // DETAIL
} // namespace detail

// End of declarations.

// Definitions of scale_axis and roundup and round down.

 /*! \brief Inspect values to find min and max.
     \details Inspect all values between begin and (one before) end to work out and update min and max.
       Similar to boost::minmax_element, but ignoring at 'limit': non-finite, +-infinity, max & min, & NaN).
       If can't find a max and a min, then throw a runtime_error exception.
    \tparam iter InputIterator into STL container.
    \param begin Iterator to chosen first item in container.
    \param end Iterator to chosen last item in container.
    \param min Updated with Minimum value found (not 'at limit').
    \param max Updated with Maximum value found (not 'at limit').
    \return number of normal values (not 'at limit' neither too big, NaN nor infinite).
  */
template <typename Iter>
int mnmx(
  Iter begin, // iterator to chosen first item in container.
  Iter end,  // iterator to chosen last item in container.
  double* min, // Updated with Minimum value found (not 'at limit').
  double* max) // Updated with Maximum value found (not 'at limit').
{
  *max = std::numeric_limits<double>::quiet_NaN();
  *min = std::numeric_limits<double>::quiet_NaN();
  using boost::svg::detail::is_limit; // Either x and/or y not a proper data value.
  int goods = 0; // Count of values within limits.
  int limits = 0;
  Iter pos = begin;
  using boost::quan::value_of;
  while(pos != end && is_limit(value_of(*pos)))
  { // Count any limits before the first 'good' (FP normal) value.
    limits++;
    pos++;
  }
  if (pos == end)
  { // ALL values are at limit!
      throw std::runtime_error("Autoscale could not find any useful values to scale axis!");
    // std::cout << "all values at limit (NaN or infinity)!" << std::endl;
    // min and max are both == NaN
  }
  else
  {
    using boost::quan::value_of;
    double x = value_of(*pos);
    *max = x;
    *min = x;
    //std::cout << "Initial min & max " << x << std::endl;
    pos++;
    goods++;
    while(pos != end)
    {
      if (!is_limit(value_of(*pos)))
      { // x is finite.
        x = value_of(*pos);
        if (x > *max)
        {
          *max = x;
        }
        if (x < *min)
        {
          *min = x;
        }
        goods++;
        //std::cout << goods << " goods, " << x << std::endl;
     } // if finite
      else
      { // If x not finite, then the y value won't be plotted.
        std::cout << "limit value: " << *pos  << std::endl;
        limits++;
      }
      ++pos;
    } // while
    //std::cout << "min " << *min << ", max " << *max << std::endl; //
    //std::cout << "limits " << limits << std::endl;
  }
  if (goods < 2)
  {
    throw std::runtime_error("Autoscale could not find useful min & max to scale axis!");
  }
  return goods; // If goods < 2,
} // template <typename iter>int mnmx((iter begin, iter end, double* min, double* max)

/*! Scale axis and update min and max axis values, and tick increment and number of ticks.

   \param min_value Scale axis from explicit input range minimum.
   \param max_value Scale axis from explicit input range maximum.

   \param axis_min_value Computed minimum value for the axis, updated by scale_axis.
   \param axis_max_value Computed maximum value for the axis, updated by scale_axis.
   \param axis_tick_increment  Computed tick increment for the axis, updated by scale_axis.
   \param auto_ticks Computed number of ticks, updated by scale_axis.
   \param origin If false, do not include the origin unless the range @c min_value to @c max_value includes zero.
   \param tight Fraction of overrun allowed before another tick used. For a good visual effect, up to about 0.001 might suit a 1000 pixel wide image, allowing values just 1 pixel over the tick to be shown.
   \param min_ticks Minimum number of major ticks.
   \param steps 0, or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
*/
void scale_axis(
   double min_value, // Scale axis from explicit input range minimum.
   double max_value, // Scale axis from input range maximum.
   double* axis_min_value, // Computed minimum value for the axis, updated by @c scale_axis.
   double* axis_max_value, //  Computed maximum value for the axis, updated by @c scale_axis.
   double* axis_tick_increment, //  Computed tick increment for the axis, updated by scale_axis.
   int* auto_ticks, // Computed number of ticks, updated by scale_axis.
   //  NO check_limits parameter.
   bool origin, // = false, // Do not include the origin unless the range min_value <= 0 <= max_value.
   double tight, // = 0., // Tightness - fraction of 'overrun' allowed before another tick used.
   // for visual effect up to about 0.001 might suit a 1000 pixel wide image,
   // allowing values just 1 pixel over the tick to be shown.
   int min_ticks, //= 6, // Minimum number of major ticks.
   int steps // = 0 // Round up and down to 2, 4, 6, 8, 10, or 5, 10 or 2, 5, 10 systems.
)
{
  detail::scale_axis_impl(min_value, max_value,
    axis_min_value, axis_max_value, axis_tick_increment, auto_ticks, // All 4 updated.
    origin, tight, min_ticks, steps); // Display range.
} //

/*! Scale axis function to define axis marker ticks based on min & max parameters values (handling uncertainty).

  \param min_value Scale axis from explicit input minimum.
  \param max_value Scale axis from explicit input maximum.

  \param axis_min_value Computed minimum value for the axis, updated by scale_axis.
  \param axis_max_value Computed maximum value for the axis, updated by scale_axis.
  \param axis_tick_increment Computed tick increment for the axis, updated by scale_axis.
  \param auto_ticks  Computed number of ticks, updated by scale_axis.
  \param check_limits  If true then check all values for infinity, NaN etc.
  \param autoscale_plusminus Multiplier of uncertainty or standard deviations to allow for confidence ellipses.
  \param origin If true, ensures that zero is a tick value.
  \param tight  Allows user to avoid a small fraction over a tick using another tick.
  \param min_ticks  Minimum number of major ticks.
  \param steps Round up and down to 2, 4, 6, 8, 10, or 5, 10 or 2, 5, 10 systems.
 */
void scale_axis(
   double min_value, // Updated with Minimum value found.
   double max_value, // Updated with Maximum value found.
   double* axis_min_value, // Minimum value for the axis, updated by scale_axis.
   double* axis_max_value, //  Maximum value for the axis, updated by scale_axis.
   double* axis_tick_increment, //  Tick increment for the axis, updated by scale_axis.
   int* auto_ticks,  // Updated with number of ticks.
   bool check_limits, // Whether to check all values for infinity, NaN etc.
   double autoscale_plusminus, // Multiplier of uncertainty or standard deviations to allow for confidence ellipses.
   bool origin, // If true, ensures that zero is a tick value.
   double tight, // Allows user to avoid a small fraction over a tick using another tick.
   int min_ticks, // Minimum number of ticks.
   int steps) // Round up and down to 2, 4, 6, 8, 10, or 5, 10, or 2, 5, 10 major ticks multiples.
{
  // Must assume max and min are OK (can't ignore limit values).
  // If either at limit then will be caught and exception thrown later by x_range.
  // So deliberately ignore check_limits parameter & and autoscale_plusminus & suppress any warnings.
  detail::scale_axis_impl(min_value, max_value,
    axis_min_value, axis_max_value, axis_tick_increment, auto_ticks, // All 4 updated.
    origin, tight, min_ticks, steps); // Display range.
  check_limits = false;
  autoscale_plusminus = 1.96;
}

 /*! Scale axis from data-series (usually to plot), perhaps only part of container.

   \tparam Iter Type of interator into STL container type: @c array, @c vector ...

   \param begin First item in container to use to calculate autoscale mimimum or maximum.
   \param end Last item in container to use to calculate autoscale mimimum or maximum.
   \param axis_min_value Computed minimum value for the axis, updated by scale_axis.
   \param axis_max_value Computed maximum value for the axis, updated by scale_axis.
   \param axis_tick_increment  Computed tick increment for the axis, updated by scale_axis.
   \param auto_ticks Computed number of ticks, updated by scale_axis.
   \param check_limits Whether to check all values for infinity, NaN etc.
   \param autoscale_plusminus Multiplier of uncertainty or standard deviations to allow for confidence ellipses.
   \param origin If false, do not include the origin unless the range min_value <= 0 <= max_value.
   \param tight fraction of 'overrun' allowed before another tick used. For visual effect up to about 0.001 might suit a 1000 pixel wide image, allowing values just 1 pixel over the tick to be shown.
   \param min_ticks Minimum number of major ticks.
   \param steps 0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
 */
template <typename Iter>
void scale_axis(
   Iter begin, // iterators into begin in STL container.
   Iter end, // iterators into end in STL container.
   // (not necessarily ordered by size, so will find min and max).
   double* axis_min_value, // Computed minimum value for the axis, updated by scale_axis.
   double* axis_max_value, //  Computed maximum value for the axis, updated by scale_axis.
   double* axis_tick_increment, //  Computed tick increment for the axis, updated by scale_axis.
   int* auto_ticks, // Computed number of ticks, updated by scale_axis.
   bool check_limits, // Whether to check all values for infinity, NaN etc.
   double autoscale_plusminus, // Mutiplier of uncertainty or standard deviations to allow fo confidence ellipses.
   bool origin, // = false, // Do not include the origin unless the range min_value <= 0 <= max_value.
   double tight, // = 0., // tightest - fraction of 'overrun' allowed before another tick used.
   // for visual effect up to about 0.001 might suit a 1000 pixel wide image,
   // allowing values just 1 pixel over the tick to be shown.
   int min_ticks, // = 6, // Minimum number of major ticks.
   int steps) // = 0) // 0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
{
  double x_min;
  double x_max;
  if (!check_limits)
  { // minmax_element is efficient for maps because can use knowledge of being sorted,
    // BUT only if it can be assumed that no values are 'at limits',
    // infinity, NaN, max_value, min_value, denorm_min.
    // Otherwise it is necessary to inspect all values individually.
    std::pair<Iter, Iter> result = boost::minmax_element(begin, end); // min & max
    // scale_axis (not check_limits version) forward declaration to ensure compiler finds right version.
     x_min = *(result.first);
     x_max = *(result.second);
  }
  else
  { // Must check limits.
    int good = mnmx(begin, end, &x_min, &x_max);
    if (good < 2)
    {
      throw std::runtime_error("Autoscale could not find useful min & max to scale axis!");
    }
    detail::scale_axis_impl(x_min, x_max,
    axis_min_value, axis_max_value, axis_tick_increment, auto_ticks, // All 4 updated.
      origin, tight, min_ticks, steps); // Display range.
  }
  autoscale_plusminus = 1.96;
} // template <typename iter> void scale_axis(iter begin, iter end, ...

/*!
  \brief Scale axis using an \b entire Container of a data-series, usually to plot (not necessarily ordered, so will find minimum and maximum).

  \tparam C STL container type: @c array, @c vector ...

  \param container STL container, usually of a data-series.
  \param axis_min_value Computed minimum value for the axis, updated by scale_axis.
  \param axis_max_value Computed maximum value for the axis, updated by scale_axis.
  \param axis_tick_increment  Computed tick increment for the axis, updated by scale_axis.
  \param auto_ticks Computed number of ticks, updated by scale_axis.
  \param check_limits Whether to check all values for infinity, NaN etc.
  \param autoscale_plusminus Multiplier of uncertainty or standard deviations to allow for confidence ellipses.
  \param origin If false, do not include the origin unless the range min_value \<= 0 \<= max_value.
  \param tight fraction of overrun allowed before another tick used. For visual effect up to about 0.001 might suit a 1000 pixel wide image, allowing values just 1 pixel over the tick to be shown.
  \param min_ticks Minimum number of major ticks.
  \param steps 0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
*/
template <class C>
void scale_axis(
  const C& container, // STL container, usually of a data-series.
  double* axis_min_value, // Computed minimum value for the axis, updated by scale_axis.
  double* axis_max_value,  // Computed maximum value for the axis, updated by scale_axis.
  double* axis_tick_increment, //  Computed tick increment for the axis, updated by scale_axis.
  int* auto_ticks, // Computed number of ticks, updated by scale_axis.
  bool check_limits, // Whether to check all values for infinity, NaN etc.
  double autoscale_plusminus, // = 3., Multiplier of uncertainty or standard deviations to allow for confidence ellipses.
  bool origin, // = false, // do not include the origin unless the range min_value to max_value includes zero.
  double tight, // = 0., // tightest - fraction of overrun allowed before another tick used.
  // For a good visual effect, up to about 0.001 might suit a 1000 pixel wide image,
  // allowing values just 1 pixel over the tick to be shown.
  int min_ticks, // = 6, // Minimum number of major ticks.
  int steps) // = 0) // 0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
{ // s
  double x_min;
  double x_max;
  if (!check_limits)
  {
    //std::pair<T::iterator, T::iterator> result = boost::minmax_element(container.begin(), container.end());
    std::pair<typename C::const_iterator, typename C::const_iterator> result = boost::minmax_element(container.begin(), container.end());
    // minmax_element is efficient because can use knowledge of being sorted,
    // BUT only if it can be assumed that no values are 'at limits',
    // infinity, NaN, max_value, min_value, denorm_min.
    using boost::quan::value_of;
    x_min = value_of(*(result.first));
    x_max = value_of(*(result.second));
  }
  else
  { // It is necessary to inspect all values individually.
    // std::cout << container.size() << " values." << std::endl;
    // Work out min and max, ignoring non-finite, +-infinity, max & min, & NaN).
    // If can't find a max and a min, then will throw exception.
    int good = mnmx(container.begin(), container.end(), &x_min, &x_max);
    if (good < 2)
    {
      throw std::runtime_error("Autoscale could not find useful min & max values to scale the X axis!");
    }
    // std::cout << "x_min " << x_min << ", x_max " << x_max << std::endl; //
  }

  detail::scale_axis_impl(x_min, x_max,
    axis_min_value, axis_max_value, axis_tick_increment, auto_ticks,
    origin, tight, min_ticks, steps);
  autoscale_plusminus = 3.;

} // template <class C> int scale_axis  C an STL container: array, vector ...

/*! Scale X and Y axis using a 2D STL container: @c std::array of @c std::pairs, @c std::vector of @c std::pairs, ...

  \tparam C STL container holding 2D pairs of X and Y.

  \param container data-series to plot - entire 2D container.
  \param x_axis_min_value Computed minimum value for the X-axis, updated by scale_axis.
  \param x_axis_max_value Computed minimum value for the X-axis, updated by scale_axis.
  \param x_axis_tick_increment Computed tick increment for the axis, updated by scale_axis.
  \param x_auto_ticks Computed number of ticks, updated by scale_axis.
  \param y_axis_min_value  Computed minimum value for the Y-axis, updated by scale_axis.
  \param y_axis_max_value  Computed maximum value for the Y-axis, updated by scale_axis.
  \param y_axis_tick_increment Updated with Y-axis tick increment.
  \param y_auto_ticks Computed number of Y-axis ticks, updated by scale_axis.
  \param check_limits Whether to check all values for infinity, NaN etc.
  \param autoscale_plusminus Multiplier of uncertainty or standard deviations to allow for confidence ellipses.
  \param x_origin Do not include the origin unless the range min_value <= 0 <= max_value.
  \param x_tight Fraction of 'overrun' allowed before another tick used. For visual effect up to about 0.001 might suit a 1000 pixel wide image, allowing values just 1 pixel over the tick to be shown.
  \param x_min_ticks Minimum number of X-axis major ticks.
  \param x_steps  0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
  \param y_origin Do not include the origin unless the range min_value to max_value contains zero.
  \param y_tight Fraction of 'overrun' allowed before another tick used. For visual effect up to about 0.001 might suit a 1000 pixel wide image, allowing values just 1 pixel over the tick to be shown.
  \param y_min_ticks Minimum number of Y axis major ticks.
  \param y_steps 0, or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
*/
template <class C>
void scale_axis(
  const C& container, // data-series to plot - \b entire 2D container (not necessarily ordered, so will find min and max).
  double* x_axis_min_value, // Computed minimum value for the X-axis, updated by scale_axis.
  double* x_axis_max_value,  // Computed minimum value for the X-axis, updated by scale_axis.
  double* x_axis_tick_increment, // Updated with X axis tick increment.
  int* x_auto_ticks,   // Computed number of X axis ticks, updated by scale_axis.
  double* y_axis_min_value, // Computed minimum value for the Y-axis, updated by scale_axis.
  double* y_axis_max_value,  // Computed maximum value for the Y-axis, updated by scale_axis.
  double* y_axis_tick_increment, // Updated with Y axis tick increment.
  int* y_auto_ticks,  // Computed number of Y-axis ticks, updated by scale_axis.
  bool check_limits, // = true, // Whether to check all values for infinity, NaN etc.
  double autoscale_plusminus, // = 3., // Mutiplier of uncertainty or standard deviations to allow of confidence ellipses.
  bool x_origin, // = false, // do not include the origin unless the range min_value <= 0 <= max_value.
  double x_tight, // = 0., // tightest - fraction of 'overrun' allowed before another tick used.
  // for visual effect up to about 0.001 might suit a 1000 pixel wide image,
  // allowing values just 1 pixel over the tick to be shown.
  int x_min_ticks, // = 6, // Minimum number of major ticks.
  int x_steps, // = 0, // 0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
  bool y_origin, // = false, // do not include the origin unless the range min_value <= 0 <= max_value.
  double y_tight, // = 0., // tightest - fraction of 'overrun' allowed before another tick used.
  // for visual effect up to about 0.001 might suit a 1000 pixel wide image,
  // allowing values just 1 pixel over the tick to be shown.
  int y_min_ticks, // = 6, // Minimum number of major ticks.
  int y_steps) // = 0) // 0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
{ /* Scale X and Y axis using T a 2D STL container: array of pairs, vector of pairs, list of pairs, map ...
      container data-series to plot - entire 2D container.
    */
  double x_max = std::numeric_limits<double>::quiet_NaN();
  double x_min = std::numeric_limits<double>::quiet_NaN();
  double y_max = std::numeric_limits<double>::quiet_NaN();
  double y_min = std::numeric_limits<double>::quiet_NaN();

  if (!check_limits)  // TODO my_plot.autoscale_check_limits(false);
  { // BUT only if it can be assumed that no values are 'at limits',
    // infinity, NaN, max_value, min_value, denorm_min.
    // minmax_element is efficient for maps because it can use knowledge of all maps being sorted,
    // And also sadly it doesn't work right - Y minimum is wrong!  // TODO ??

    std::pair<typename C::const_iterator, typename C::const_iterator> result
      = boost::minmax_element(container.begin(), container.end());
    using boost::quan::values_of;
    std::pair<double, double> px = values_of(*result.first); // X min & X max.
    std::pair<double, double> py = values_of(*result.second); // Y min & Y max.
    x_min = px.first;
    x_max = py.first;
    // x are OK, but Y are only those corresponding to those X, not min and max.,
    // so still need to iterate through the Y to find y min and y max.
    //y_min = px.second;
    //y_max = py.second;
    typename C::const_iterator pos = container.begin();
    if (pos == container.end())
    {
      throw std::runtime_error("SVG_plot Autoscale could not find any values to scale axes!");
    }
    using boost::quan::value_of;
    double y = value_of(pos->second);
    using boost::quan::unc_of;
    double yu = unc_of(pos->second) * autoscale_plusminus;
    y_max = y + yu;
    y_min = y - yu;
    while(pos != container.end())
    {
      y = value_of(pos->second);
      yu = unc_of(pos->second) * autoscale_plusminus;
      if (y + yu > y_max)
      {
        y_max = y + yu;
      }
      if (y - yu < y_min)
      {
        y_min = y - yu;
      }
      pos++;
    }
    std::cout << "SVG_plot Autoscale warning (No limits checks):"
                 "x_min = " << x_min
            << ", x_max = " << x_max
            << ", y_min = " << y_min
            << ", y_max = " << y_max << std::endl;
  }
  else
  { // Otherwise it is necessary to inspect all values individually.
    // It seems that X and Y need to be examined in pairs, so sadly, we can't use:
    // int good_x = mnmx(container.begin(), container.end(), &x_min, &x_max);
    // or
    // int good_y = mnmx(container.begin(), container.end(), &y_min, &y_max);

    // Work out min and max, ignoring non-finite (+-infinity & NaNs).
    using boost::svg::detail::pair_is_limit; // Either x and/or y is not a proper data value.
    int goods = 0; // Count of values where both X and Y are normal (within limits).
    int limits = 0;// Count of values where both X and Y are at limits (not normal).
    typename C::const_iterator pos = container.begin();
    while(pos != container.end() && pair_is_limit(*pos))
    { // Count any limits before the first good.
      limits++;
      pos++;
    }
    if (pos == container.end())
    { // ALL values are at limit!
      //std::cout << "all values at limit" << std::endl;
      throw std::runtime_error("Autoscale could not find any useful values to scale axes!");
    }
    else
    {
      using boost::quan::value_of;
      double x = value_of(pos->first);
      using boost::quan::unc_of;
      double xu = unc_of(pos->first) * autoscale_plusminus;
      x_max = x + xu;
      x_min = x - xu;
      double y = value_of(pos->second);
      double yu = unc_of(pos->second) * autoscale_plusminus;
      y_max = y + yu;
      y_min = y - yu;
      //std::cout << "Initial min & max " << x << "+-" << xu << " = " << x_min << " to " << x_max << ", " << y << "+-" << yu << "=" <<y_min << " to " << y_max << std::endl;
      pos++;
      goods++;
      while(pos != container.end())
      {
        if (!pair_is_limit(*pos))
        { // Either x and/or y are finite.
          x = value_of(pos->first);
          xu = unc_of(pos->first) * autoscale_plusminus;
          if (x + xu > x_max)
          {
            x_max = x + xu;
          }
          if (x - xu < x_min)
          {
            x_min = x - xu;
          }
          y = value_of(pos->second);
          yu = unc_of(pos->second) * autoscale_plusminus;
          if (y + yu > y_max)
          {
            y_max = y + yu;
          }
          if (y - yu < y_min)
          {
            y_min = y - yu;
          }
          //std::cout << "min & max " << x << "+-" << xu << " = " << x_min << " to " << x_max << ", " << y << "+-" << yu << "=" <<y_min << " to " << y_max << std::endl;
          goods++;
          // std::cout << goods << " goods, " << x << ' ' << y << std::endl;
       } // if finite
        else
        { // If either are not finite, then neither useful for autoscaling.
          // If x not finite, then the y value won't be plotted.
          // If y value not finite, then it will be 'off limits'.
          // std::cout << "limit value: " << pos->first << ' ' << pos->second  << std::endl;
          limits++;
        }
        ++pos;
      } // while
      std::cout << "Checked: x_min " << x_min << ", x_max " << x_max << ", y_min " << y_min << ", y_max " << y_max << ", " << goods << " 'good' values, " << limits << " values at limits."<< std::endl;
    }
  }
  detail::scale_axis_impl(x_min, x_max,
    x_axis_min_value, x_axis_max_value, x_axis_tick_increment, x_auto_ticks,
    x_origin, x_tight, x_min_ticks, x_steps);

  detail::scale_axis_impl(y_min, y_max,
    y_axis_min_value, y_axis_max_value, y_axis_tick_increment, y_auto_ticks,
    y_origin, y_tight, y_min_ticks, y_steps);

} // template <class T> int scale_axis  T an STL container: array, vector ...

// Above versions all use the scale_axis_impl implementation below that does the real scaling work.
namespace detail
{ // Definition of implementation with default parameters.
//! @cond DETAIL

void scale_axis_impl(
  double min_value,
  double max_value, // Scale axis from Input range min & max.
  double* axis_min_value,
  double* axis_max_value,
  double* axis_tick_increment,
  int* auto_ticks, // All 4 updated.
               // NO check_limits parameter in this version.
  bool origin, // = false, // Do not include the origin unless the range min_value <= 0 <= max_value.
  double tight, // = 0., // tightest - fraction of 'overrun' allowed before another tick used.
               // for visual effect up to about 0.001 might suit a 1000 pixel wide image,
               // allowing values just 1 pixel over the tick to be shown.
  int min_ticks, // = 6, // Minimum number of major ticks.
  int steps) // = 0) // 0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
{
  int ticks = -1; // Negative to warn of 'bad' value.
  double test_max;
  double test_min;
  double test_increment;
  switch (steps)
  { // Optionally expand the range by rounding actual max and min values up and down.
  case 0 :
    break; // No steps.
  case 10 :
    max_value = roundup10(max_value);
    min_value = rounddown10(min_value);
    break;
  case 5 :
    max_value = roundup5(max_value);
    min_value = rounddown5(min_value);
    break;
  case 2 :
    max_value = roundup2(max_value);
    min_value = rounddown2(min_value);
    break;
  default:
    throw std::domain_error("Unimplemented steps!");
  } // switch
  double range = max_value - min_value;  // range of data.

  smallest<> is_small(1000. * (std::numeric_limits<double>::min)()); // 1000 * min value
  close_to<> is_near_100eps(100. * (std::numeric_limits<double>::epsilon)()); // 100 * epsilon

  if ((tight < 0.) || tight > 1.)
  { // Tight can't be negative and > 1 is very likely a mistake, 0.01 = 1% more reasonable.
    throw std::domain_error("tight not in range 0 to 1 !");
  }

  using std::isfinite;
  if(!(isfinite)(min_value))
  {
    throw std::domain_error("min_value not finite!");
  }
  if(!(isfinite)(max_value))
  {
    throw std::domain_error("max_value not finite!");
  }
  if (origin == true)
  { // Ensure the axis includes zero.
    if (min_value > 0.)
    { // All positive case.
      min_value = 0.;
    }
    else if(max_value < 0.)
    { // All negative case.
      max_value = 0.;
    }
  } // origin

  if (min_value > max_value)
  { // max and min are transposed!
    throw std::domain_error("min > max!");
  }
  else if (is_small(range)
    // range <= 1000. * std::numeric_limits<double>::min()) // Absolute range > ~1e-308 * 1000 ~= 1e-305
    // Range has already been checked to be > 0.
    // This checks for range too near to zero to be useful.
    // is_small is similar to Boost.Test check_is_small

    || is_near_100eps(max_value, min_value)

    //|| (range <= 100. * std::numeric_limits<double>::epsilon() * abs(min_value))
    // This checks the relative range is not too near to epsilon to be useful.
    // Knuth Vol II, avoiding over and underflow, as used in Boost.Test close_at_tolerance.
    )
  { // Factor of 1000 is to ensure range is more than a few epsilon relative wide.
    // Special cases of max ~== min values *and* exactly max == min (including == 0).
    // This could be two or more duplicate (repeat) measurements on x or y axis,
    // or only a modest number of epsilons apart even,
    // so it not necessarily an error, but some special handling is required.
    // At what point does range become big enough to be plausible to provide axis max & min tick values?
    // A few numeric_limits<double>::epsilon() only covers smallish compute errors.
    // at least 1000 epsilon absolute seems more plausible, and relative to biggest?
    // Return 3 ticks: -1, max==min==mid, and +1 ticks.
    // But uncertain if this is best solution?

    double mean = (min_value + max_value) /2;
    test_increment = 1;
    test_min = mean - test_increment;
    test_max = mean + test_increment;

    ticks = 3; // ticks - OK, but ticks == 3 warns that max_value ~== min_value.
  }
  else
  { // Range is reasonably large, so
    // compute candidate for increment - must be smaller than range, so divide by 10.
    test_increment = std::pow(10., std::ceil(std::log10(std::abs<double>(range)/10.)));
    // Must be a decimal multiple or decimal fraction,
    // but is not necessarily exactly representable in floating-point format.
    // Establish maximum axis scale value, using this increment.

    test_max = (static_cast<long>(max_value / test_increment)) * test_increment;

    if(test_max < max_value)
    {
      test_max += test_increment;
    }
    ticks = 1; // Must be 1 'extra' tick at the end.
    // Establish minimum axis tick value by decrementing from test_max.
    test_min = test_max;
    do
    {
      ticks++;
      test_min -= test_increment;
    }
    while (test_min > min_value); // min_value);

    // Subtracting small values can screw up the scale limits,
    // eg: if scale_axis is called with (min, max)=(0.01, 0.1),
    // then the calculated scale is 1.0408E17 TO 0.05 BY 0.01,
    // rather than 0, 0.05, 0.01.
    // I suspect 1.e-10 is bigger than necessary?  related to std::numeric_limits<>::epsilon?
    if(std::abs(test_min) < 1.E-14)
    { // test_min is very near zero,
      test_min = 0.; // so treat as exact zero to avoid risk of a switch to e format.
    }
    while(ticks < min_ticks)
    {  // Adjust for too few tick marks by
      test_increment /= 2.; // halving the increment.
      // (divide by two should not cause trouble by being inexact).
      ticks = static_cast<int>((test_max - test_min) / test_increment) +1;
      if (steps == 0)
      { // Remove any superfluous ticks above max and below min.
        while((test_min + test_increment) <= min_value)
        { // min_value is > 2nd from bottom tick,
          test_min += test_increment;
          ticks--; // so we can scrap the 1st bottom tick.
        }
        while((test_max - test_increment) >= max_value)
        { // max_value is > top_but_one tick,
          ticks--; // so ditch the top tick.
          test_max -= test_increment;
        }
      }
    } // while

    if (tight > 0.)
    { // Check that can't use a tick less at top or bottom.
      double max_plus_margin = test_max - test_increment + test_increment * tight;
      if (max_value < max_plus_margin)
      { // max is too big, so remove top tick.
        ticks -= 1;
        test_max -= test_increment;
      }

      double min_plus_margin = test_min + test_increment - test_increment * tight;
      if (min_value > min_plus_margin)
      { // min is too small, so remove the bottom tick.
        ticks -= 1;
        test_min += test_increment;
      }
      // Check again to make quite sure can't reduce again.
      max_plus_margin = test_max - test_increment + test_increment * tight;
      if (max_value < max_plus_margin)
      { // max is too big
        ticks -= 1;
        test_max -= test_increment;
      }
      min_plus_margin = test_min + test_increment - test_increment * tight;
      if (min_value > min_plus_margin)
      { // min is too small, so remove the bottom tick.
        ticks -= 1;
        test_min += test_increment;
      }
    } // if (tight != 0.)
  } // range reasonable

  // Pass computed min & max axis tick values back to caller.
  *axis_min_value = test_min; //
  *axis_max_value = test_max;
  *axis_tick_increment = test_increment; // major_tick_interval.
  *auto_ticks = ticks; // major ticks.
} // scale_axis_impl

//! @endcond // DETAIL

} // namespace detail
// Utility functions to display containers
// and to find min and max values in containers.

template <typename T> // T an STL container: array, vector ...
size_t show(const T& container)
{ //! Utility functions to display STL containers.
  std::cout << container.size() << " values in container: ";
  for (typename T::const_iterator it = container.begin(); it != container.end(); it++)
  {
    std::cout << *it << ' ';
  }
  std::cout << std::endl;
  return container.size();
}// Container data-series to plot.

// Pointer version is not needed - iterator version is used instead.

template <typename iter> // T an STL container: array, vector ...
size_t show(iter begin, iter end) // Iterators
{ //! Utility function to display STL containers.
  size_t count = 0;
  while (begin != end)
  {
    count++;
    std::cout << *begin << ' ';
    ++begin;
  }
  std::cout << ": " << count << " values used.";
  std::cout << std::endl;
  return count;
}// Container data-series to plot.

template <typename T>
size_t show_all(const T& containers)
{ //! Show all the containers values.
  // \tparam T an STL container: container of containers.
  for (typename T::const_iterator it = containers.begin(); it != containers.end(); it++)
  {
    show(*it);
  }
  return containers.size();
} // Container data-series to plot.

  /*! Calculate minimum and maximum from data in a container.
    \param container Container data-series.
    \return minimum and maximum of an STL container as a @c std::pair.
    \tparam  T an STL container: array, vector, set, map ...
  */
template <class T>
std::pair<double, double> range_mx(const T& container)
{
  std::pair<typename T::const_iterator, typename T::const_iterator> result
    = boost::minmax_element(container.begin(), container.end());
  std::pair<double, double> minmax;
  minmax.first = *result.first;
  minmax.second = *result.second;
  return minmax;
} // template <class T> range_mx

template <typename T> // T an STL container: array, vector, set, map ...
std::pair<double, double> range_all(const T& containers) // Container of STL containers of data-series.
{ /*! \return minimum and maximum of a container containing STL containers.
      \tparam T an STL container: array, vector, set, map ...
  */
  std::pair<double, double> minmax(
    (std::numeric_limits<double>::max)(), (std::numeric_limits<double>::min)()
  );
  for (typename T::const_iterator it = containers.begin(); it != containers.end(); it++)
  {
    std::pair<double, double> mm = range_mx(*it); // Scale of this container.
    minmax.first = (std::min)(mm.first, minmax.first); //
    minmax.second = (std::max)(mm.second, minmax.second);
  }
  return minmax;
} // template <class T> scale_all

/*! Round up to nearest multiple of 10.
  Decimal scaling steps, so value is 0.1, 0.2, 0.5, 1., 2., 5. or 1., 10., 20., 100. ...
  \return Rounded up value.
*/
double roundup10(double value)
{
  BOOST_MATH_STD_USING
  smallest<> is_small(100. * (std::numeric_limits<double>::min)()); // 100 * min value.
  if (is_small(value) )
  { // Value very close to zero.
    return 0.; // Just return zero.
  }
  bool is_neg = (value >= 0) ? false : true;
  value = std::abs(value);

  int order = int(floor(log10(value))); // 0 to 9.999, gives 0, 10 to 99.9 gives 2 ...
  double scaled_value = value * pow(10., -order); // 0 to 9.99 is unchanged, 10 to 9.99 scaled down to 1. to 9.99
  double pow10order = is_neg ? -pow(10., order) : pow(10., order); //  power of ten, signed.
  if(scaled_value > 5.)
  {
    return 10. * pow10order;
  }
  else if(scaled_value > 2.)
  {
    return 5. * pow10order;
  }
  else if(scaled_value > 1.)
  {
    return  2. * pow10order;
  }
  else
  {
    return  1. * pow10order;
  }
} // double roundup10(double value)

/*! Round down to nearest multiple of 10.
  Decimal scaling steps, so value is 0.1, 0.2, 0.5, 1., 2., 5. or 1., 10., 20., 100. ...
  \return Rounded down value.
*/
double rounddown10(double value)
{
  BOOST_MATH_STD_USING

  smallest<> is_small(100. * (std::numeric_limits<double>::min)()); // 100 * min value
  if (is_small(value))
  { // Value very close to zero.
    return 0.; // Just return zero.
  }
  bool is_neg = (value >= 0) ? false : true;
  value = std::abs(value);
  int order = int(floor(log10(value))); // 0 to 9.999, gives 0, 10 to 99.9 gives 2 ...
  double scaled_value = value * pow(10., -order); // 0 to 9.99 is unchanged, 10 to 9.99 scaled down to 1. to 9.99
  double pow10order = is_neg ? -pow(10., order) : pow(10., order); //  power of ten, signed.

  if(scaled_value <= 2.)
  {
    return 1. * pow10order;
  }
  else if(scaled_value <= 5.)
  {
    return 2. * pow10order;
  }
  else if(scaled_value <= 10.)
  {
    return 5. * pow10order;
  }
  else
  {
    return 10. * pow10order;
  }
} // double rounddown10(double value)

/*! Semi-decimal scaling, so return 0.1, 0.5, 1, 5, 10, 50, 100 ...
  \return rounded up value.
*/
double roundup5(double value)
{
  BOOST_MATH_STD_USING

  smallest<> is_small(100. * (std::numeric_limits<double>::min)()); // 100 * min value.
  if (is_small(value) )
  { // Value very close to zero.
    return 0.; // Just return zero.
  }
  bool is_neg = (value >= 0) ? false : true;
  value = std::abs(value);
  int order = int(floor(log10(value))); // 0 to 9.999, gives 0, 10 to 99.9 gives 2 ...
  double scaled_value = value * pow(10., -order); // 0 to 9.99 is unchanged, 10 to 9.99 scaled down to 1. to 9.99
  double pow10order = is_neg ? -pow(10., order) : pow(10., order); //  power of ten, signed.

  if(scaled_value > 5.)
  { // Scale down to 1.
    return 10. * pow10order;
  }
  else if(scaled_value > 1.)
  { //
    return 5. * pow10order;
  }
  else
  { // is < 1
  }
  return 1. * pow10order;
} // double roundup2(double value)

/*! Semi-decimal scaling, so return 0.1, 0.5, 1, 5, 10, 50, 100 ...
  \return Rounded down value.
*/
double rounddown5(double value)
{
  BOOST_MATH_STD_USING

  smallest<> is_small(100. * (std::numeric_limits<double>::min)()); // 100 * min value
  if (is_small(value))
  { // Value very close to zero.
    return 0.; // Just return zero.
  }
  bool is_neg = (value >= 0) ? false : true;
  value = std::abs(value);
  int order = int(floor(log10(value))); // 0 to 9.999, gives 0, 10 to 99.9 gives 2 ...
  double scaled_value = value * pow(10., -order); // 0 to 9.99 is unchanged, 10 to 9.99 scaled down to 1. to 9.99
  double pow10order = is_neg ? -pow(10., order) : pow(10., order); //  power of ten, signed.

  if(scaled_value < 2.)
  { //
    return 1. * pow10order;
  }
  else if(scaled_value < 10.)
  { //
    return 5. * pow10order;
  }
  else
  { //
    return 10. * pow10order;
  }
} // double rounddow5(double value)

/*! Binary scaling steps, so return 0.1, 0.2, 0.4, 0.6, 0.8, 1.0, 2, 4, 6, 8, 10, 20, 40 60, 80, 100..
  \return Rounded up value.
*/
double roundup2(double value)
{
  BOOST_MATH_STD_USING

  smallest<> is_small(100. * (std::numeric_limits<double>::min)()); // 100 * min value.
  if (is_small(value) )
  { // Value very close to zero.
    return 0.; // Just return zero.
  }
  bool is_neg = (value >= 0) ? false : true;
  value = std::abs(value);
  int order = int(floor(log10(value))); // 0 to 9.999, gives 0, 10 to 99.9 gives 2 ...
  double scaled_value = value * pow(10., -order); // 0 to 9.99 is unchanged, 10 to 9.99 scaled down to 1. to 9.99
  double pow10order = is_neg ? -pow(10., order) : pow(10., order); //  power of ten, signed.

  if(scaled_value > 8.)
  {
    return 10. * pow10order;
  }
  else if(scaled_value > 6.)
  {
    return 8. * pow10order;
  }
  else if(scaled_value > 4.)
  {
    return 6. * pow10order;
  }
  else if(scaled_value > 2.)
  {
    return 4. * pow10order;
  }
  else
  {
    return 2. * pow10order;
  }
} // double roundup2(double value)

/*! Binary scaling steps, so return 0.1, 0.2, 0.4, 0.6, 0.8, 1.0, 2, 4, 6, 8, 10, 20, 40 60, 80, 100..
  \return Rounded down value.
*/
double rounddown2(double value)
{
  BOOST_MATH_STD_USING
  smallest<> is_small(100. * (std::numeric_limits<double>::min)()); // 100 * min value
  if (is_small(value))
  { // Value very close to zero.
    return 0.; // Just return zero.
  }
  bool is_neg = (value >= 0) ? false : true;
  value = std::abs(value);
  int order = int(floor(log10(value))); // 0 to 9.999, gives 0, 10 to 99.9 gives 2 ...
  double scaled_value = value * pow(10., -order); // 0 to 9.99 is unchanged, 10 to 9.99 scaled down to 1. to 9.99
  double pow10order = is_neg ? -pow(10., order) : pow(10., order); //  power of ten, signed.

  if(scaled_value < 2.)
  { // Not scaled.
    return 1. * pow10order;
  }
  else if(scaled_value < 4.)
  {
    return 2. * pow10order;
  }
  else if(scaled_value < 6.)
  {
    return 4. * pow10order;
  }
  else if(scaled_value < 8.)
  {
    return 6. * pow10order;
  }
  else
  { // > 8
    return 8. * pow10order;
  }
} // double rounddown2(double value)

} // namespace svg
} // namespace boost

#endif // BOOST_SVG_AUTO_AXES_HPP


#ifndef STAN_MATH_PRIM_MAT_FUN_QUAD_FORM_HPP
#define STAN_MATH_PRIM_MAT_FUN_QUAD_FORM_HPP

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/prim/mat/err/check_symmetric.hpp>
#include <stan/math/prim/mat/fun/dot_product.hpp>
#include <stan/math/prim/mat/fun/multiply.hpp>
#include <stan/math/prim/mat/fun/transpose.hpp>

namespace stan {
  namespace math {
    /**
     * Compute B^T A B
     **/
    template<int RA, int CA, int RB, int CB, typename T>
    inline Eigen::Matrix<T, CB, CB>
    quad_form(const Eigen::Matrix<T, RA, CA>& A,
              const Eigen::Matrix<T, RB, CB>& B) {
      check_square("quad_form", "A", A);
      check_multiplicable("quad_form", "A", A, "B", B);
      return multiply(transpose(B), multiply(A, B));
    }

    template<int RA, int CA, int RB, typename T>
    inline T
    quad_form(const Eigen::Matrix<T, RA, CA>& A,
              const Eigen::Matrix<T, RB, 1>& B) {
      check_square("quad_form", "A", A);
      check_multiplicable("quad_form", "A", A, "B", B);
      return dot_product(B, multiply(A, B));
    }

  }
}

#endif


#define CATCH_CONFIG_MAIN
#include <Eigen/Dense>
#include <catch.hpp>
#include <memory>
#include <random>

#include "EDP/ConstructSparseMat.hpp"
#include "EDP/LocalHamiltonian.hpp"

#include "yavque/operators.hpp"
#include "yavque/utils.hpp"

#include "common.hpp"

TEST_CASE("test random ZZ", "[random-zz]")
{
	const uint32_t N = 14;
	const uint32_t n_terms = 10;

	using namespace yavque;

	std::random_device rd;
	std::default_random_engine re{rd()};
	std::normal_distribution<double> ndist;

	std::vector<uint32_t> sites;

	for(uint32_t k = 0; k < N; ++k)
		sites.push_back(k);

	for(uint32_t k = 0; k < 100; ++k) // instance
	{
		std::vector<std::pair<uint32_t, uint32_t>> interactions;
		for(uint32_t iter_term = 0; iter_term < n_terms; ++iter_term)
		{
			std::shuffle(sites.begin(), sites.end(), re);
			interactions.emplace_back(sites[0], sites[1]);
		}

		// construct diagonal
		Eigen::VectorXd ham_diag(1u << N);
		for(uint32_t n = 0; n < (1u << N); ++n)
		{
			int elt = 0;
			for(auto [i, j] : interactions)
			{
				int z0 = 1 - 2 * ((n >> i) & 1);
				int z1 = 1 - 2 * ((n >> j) & 1);
				elt += z0 * z1;
			}
			ham_diag(n) = elt;
		}
		auto diag_ham = DiagonalOperator(ham_diag);
		auto diag_ham_evol = DiagonalHamEvol(diag_ham);

		std::vector<std::map<uint32_t, Pauli>> pauli_strings;
		for(auto [i, j] : interactions)
		{
			std::map<uint32_t, Pauli> m;
			m[i] = Pauli('Z');
			m[j] = Pauli('Z');
			pauli_strings.emplace_back(std::move(m));
		}

		auto sum_pauli = SumPauliString(N, pauli_strings);
		auto sum_pauli_evol = SumPauliStringHamEvol(sum_pauli);

		Eigen::VectorXcd ini = Eigen::VectorXcd::Random(1u << N);
		ini.normalize();

		double t = ndist(re);

		diag_ham_evol.set_variable_value(t);
		sum_pauli_evol.set_variable_value(t);

		auto res1 = diag_ham_evol.apply_right(ini);
		auto res2 = sum_pauli_evol.apply_right(ini);

		REQUIRE((res1 - res2).norm() < 1e-6);
	}
}

Eigen::SparseMatrix<yavque::cx_double>
single_pauli(const uint32_t N, const uint32_t idx,
             const Eigen::SparseMatrix<yavque::cx_double>& m)
{
	edp::LocalHamiltonian<yavque::cx_double> lh(N, 2);
	lh.addOneSiteTerm(idx, m);
	return edp::constructSparseMat<yavque::cx_double>(1 << N, lh);
}
Eigen::SparseMatrix<yavque::cx_double> identity(const uint32_t N)
{
	std::vector<Eigen::Triplet<yavque::cx_double>> triplets;
	for(uint32_t n = 0; n < (1u << N); ++n)
	{
		triplets.emplace_back(n, n, 1.0);
	}
	Eigen::SparseMatrix<yavque::cx_double> m(1 << N, 1 << N);
	m.setFromTriplets(triplets.begin(), triplets.end());
	return m;
}

TEST_CASE("test ZXZ", "[zxz]")
{
	const uint32_t N = 10;

	using namespace yavque;

	std::random_device rd;
	std::default_random_engine re{rd()};
	std::normal_distribution<> ndist;

	std::vector<std::map<uint32_t, Pauli>> pauli_strings;

	for(uint32_t k = 0; k < N; k++)
	{
		std::map<uint32_t, Pauli> m;
		m[k] = Pauli('Z');
		m[(k + 1) % N] = Pauli('X');
		m[(k + 2) % N] = Pauli('Z');

		pauli_strings.emplace_back(std::move(m));
	}

	auto sum_pauli = SumPauliString(N, pauli_strings);
	auto sum_pauli_evol = SumPauliStringHamEvol(sum_pauli);

	Eigen::SparseMatrix<cx_double> ham(1 << N, 1 << N);
	for(uint32_t k = 0; k < N; k++)
	{
		Eigen::SparseMatrix<cx_double> term = identity(N);
		term = term * single_pauli(N, k, pauli_z().cast<cx_double>());
		term = term * single_pauli(N, (k + 1) % N, pauli_x().cast<cx_double>());
		term = term * single_pauli(N, (k + 2) % N, pauli_z().cast<cx_double>());

		ham += term;
	}

	auto ham_full = Hamiltonian(ham);
	auto ham_full_evol = HamEvol(ham_full);

	double t = ndist(re);

	ham_full_evol.set_variable_value(t);
	sum_pauli_evol.set_variable_value(t);

	for(uint32_t k = 0; k < 100; ++k) // instance
	{
		Eigen::VectorXcd ini = Eigen::VectorXcd::Random(1u << N);
		ini.normalize();

		auto res1 = ham_full_evol.apply_right(ini);
		auto res2 = sum_pauli_evol.apply_right(ini);

		REQUIRE((res1 - res2).norm() < 1e-6);
	}
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <complex>
#include <vector>

#include <boost/numeric/mtl/mtl.hpp>

const unsigned sz= 5;

inline float f(float x) { return x; }
inline double f(double x) { return x; }

inline std::complex<double> f(std::complex<double> x)
{
    return std::complex<double>(real(x), real(x)+1.0);
}

template <typename Vector, typename T>
void test(Vector& v, const T&, const char* name)
{
    using std::abs; using std::cout; using mtl::size;
    using mtl::orth; using mtl::orthogonalize_factors;

    std::cout << "\n" << name << "\n";
    for (unsigned i= 0, c= 1; i < size(v); ++i)
	for (unsigned j= 0; j < size(v[i]); ++j, c++)
	    v[i][j]= f(T((i + j) % sz));

    cout << "w initially\n";
    Vector w(v);
    for (unsigned i= 0; i < size(w); ++i)
	std::cout << w[i] << "\n";
    std::cout << "\n";

    orth(w);

    for (unsigned i= 0; i < size(w); ++i)
	std::cout << w[i] << "\n";
    std::cout << "\n";

    for (unsigned i= 0, c= 1; i < size(w); ++i) {
	for (unsigned j= 0; j < size(w); ++j, ++c)
	    std::cout << dot(w[i], w[j]) << " ";
	std::cout << "\n";
    }

    MTL_THROW_IF(abs(dot(w[3], w[4])) > 0.00001, mtl::runtime_error("Vectors 3 and 4 are not orthogonal!"));
    MTL_THROW_IF(abs(dot(w[4], w[4]) - T(1)) > 0.00001, mtl::runtime_error("Vector 4 is not normal!"));

    cout << "\nv initially\n";
    for (unsigned i= 0; i < size(v); ++i)
	std::cout << v[i] << "\n";
    std::cout << "\n";

    std::cout << "The according factors are: \n" << orthogonalize_factors(v) << '\n';

    for (unsigned i= 0; i < size(v); ++i)
	std::cout << v[i] << "\n";
    std::cout << "\n";

    for (unsigned i= 0, c= 1; i < size(v); ++i) {
	for (unsigned j= 0; j < size(v); ++j, ++c)
	    std::cout << dot(v[i], v[j]) << " ";
	std::cout << "\n";
    }

    MTL_THROW_IF(abs(dot(v[3], v[4])) > 0.00001, mtl::runtime_error("Vectors 3 and 4 are not orthogonal!"));
    MTL_THROW_IF(abs(dot(v[4], v[4])) < 0.00001, mtl::runtime_error("Vector 4 should be non-zero!"));

}

int main(int, char**)
{
    using namespace mtl;
    dense_vector<float>                                                 cf(sz, 1.0);
    dense_vector<double>                                                cd(sz, 1.0);
    dense_vector<std::complex<double> >                                 cc(sz, 1.0);
    dense_vector<float, mtl::vec::parameters<row_major> >                 rf(sz, 1.0);

    std::vector<dense_vector<float> >                                   scf(sz, cf);
    std::vector<dense_vector<double> >                                  scd(sz, cd);
    std::vector<dense_vector<std::complex<double> > >                   scc(sz, cc);
    std::vector<dense_vector<float, mtl::vec::parameters<row_major> > >   srf(sz, rf);

    dense_vector<dense_vector<float> >                                  ccf(sz, cf);
    dense_vector<dense_vector<float>, mtl::vec::parameters<row_major> >   rcf(sz, cf);

    test(scf, cf[0], "std::vector<dense_vector<float> >");
    test(scd, cd[0], "std::vector<dense_vector<double> >");
    test(scc, cc[0], "std::vector<dense_vector<std::complex<double> > >");
    test(srf, rf[0], "std::vector<dense_vector<float, parameters<row_major> > >");

    test(ccf, cf[0], "dense_vector<dense_vector<float> >");
    test(rcf, cf[0], "dense_vector<dense_vector<float>, parameters<row_major> >");

    return 0;
}


#include <vector>

#include <boost/mpi.hpp>
#include <boost/test/minimal.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

//#include "../../sketch/CT.hpp"
//#include "../../sketch/CWT.hpp"
#include "../../sketch/sketch.hpp"

// TODO: Are these includes really needed?
#include <elemental.hpp>
#include <skylark.hpp>
#include "../../sketch/CT.hpp"
#include "../../sketch/CWT.hpp"

#include "../../base/context.hpp"

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

    //////////////////////////////////////////////////////////////////////////
    //[> Parameters <]
    const size_t n   = 10;
    const size_t m   = 5;
    const size_t n_s = 6;
    const size_t m_s = 3;

    const int seed = static_cast<int>(rand() * 100);

    typedef FullyDistVec<size_t, double> mpi_vector_t;
    typedef SpDCCols<size_t, double> col_t;
    typedef SpParMat<size_t, double, col_t> DistMatrixType;

    namespace mpi = boost::mpi;
    mpi::environment env(argc, argv);
    mpi::communicator world;
    const size_t rank = world.rank();
    skylark::base::context_t context (seed);

    double count = 1.0;

    const size_t matrix_full = n * m;
    mpi_vector_t colsf(matrix_full);
    mpi_vector_t rowsf(matrix_full);
    mpi_vector_t valsf(matrix_full);

    for(size_t i = 0; i < matrix_full; ++i) {
        colsf.SetElement(i, i % m);
        rowsf.SetElement(i, i / m);
        valsf.SetElement(i, count);
        count++;
    }

    DistMatrixType A(n, m, rowsf, colsf, valsf);

    //////////////////////////////////////////////////////////////////////////
    //[> Setup test <]

    //[> 1. Create the sketching matrix and dump JSON <]
    skylark::sketch::CWT_t<DistMatrixType, DistMatrixType>
        Sparse(n, n_s, context);

    // dump to property tree
    boost::property_tree::ptree pt = Sparse.get_data()->to_ptree();

    //[> 2. Dump the JSON string to file <]
    std::ofstream out("sketch.json");
    write_json(out, pt);
    out.close();

    //[> 3. Create a sketch from the JSON file. <]
    std::ifstream file;
    std::stringstream json;
    file.open("sketch.json", std::ios::in);

    boost::property_tree::ptree json_tree;
    boost::property_tree::read_json(file, json_tree);

    skylark::sketch::CWT_t<DistMatrixType, DistMatrixType> tmp(json_tree);

    //[> 4. Both sketches should compute the same result. <]
    mpi_vector_t zero;
    DistMatrixType sketch_A(n_s, m, zero, zero, zero);
    DistMatrixType sketch_Atmp(n_s, m, zero, zero, zero);

    Sparse.apply(A, sketch_A, skylark::sketch::columnwise_tag());
    tmp.apply(A, sketch_Atmp, skylark::sketch::columnwise_tag());

    if (!static_cast<bool>(sketch_A == sketch_Atmp))
        BOOST_FAIL("Applied sketch did not result in same result");

    return 0;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  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)

//! \file
//!\brief Test sqrt() for negatable in a tiny digit region.

#include <cmath>

#define BOOST_TEST_MODULE test_negatable_func_sqrt_tiny
#define BOOST_LIB_DIAGNOSTIC

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

namespace local
{
  template<typename FixedPointType>
  const FixedPointType& tolerance_maker(const int fuzzy_bits)
  {
    static const FixedPointType the_tolerance = ldexp(FixedPointType(1), FixedPointType::resolution + fuzzy_bits);

    return the_tolerance;
  }

  template<typename FixedPointType,
           typename FloatPointType = typename FixedPointType::float_type>
  void test_sqrt(const int fuzzy_bits)
  {
    // Use at least 8 resolution bits.
    // Use at least 4 range bits.

    BOOST_STATIC_ASSERT(-FixedPointType::resolution >= 8);
    BOOST_STATIC_ASSERT( FixedPointType::range      >= 4);

    using std::sqrt;

    const FixedPointType a1 (FixedPointType(1) /  2);                       const FloatPointType b1(FloatPointType(1) /  2);
    const FixedPointType a2 (FixedPointType(6) / 10);                       const FloatPointType b2(FloatPointType(6) / 10);
    const FixedPointType a3 (FixedPointType(3) /  2);                       const FloatPointType b3(FloatPointType(3) /  2);
    const FixedPointType a4 (2L);                                           const FloatPointType b4(2L);
    const FixedPointType a5 (FixedPointType(5) /  2);                       const FloatPointType b5(FloatPointType(5) /  2);
    const FixedPointType a6 (3L);                                           const FloatPointType b6(3L);
    const FixedPointType a7 (FixedPointType(1) /  3);                       const FloatPointType b7(FloatPointType(1) /  3);
    const FixedPointType a8 (boost::math::constants::pi<FixedPointType>()); const FloatPointType b8(boost::math::constants::pi<FloatPointType>());
    const FixedPointType a9 (11L);                                          const FloatPointType b9(11L);

    BOOST_CHECK_CLOSE_FRACTION(sqrt(a1), FixedPointType(sqrt(b1)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a2), FixedPointType(sqrt(b2)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a3), FixedPointType(sqrt(b3)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a4), FixedPointType(sqrt(b4)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a5), FixedPointType(sqrt(b5)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a6), FixedPointType(sqrt(b6)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a7), FixedPointType(sqrt(b7)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a8), FixedPointType(sqrt(b8)), tolerance_maker<FixedPointType>(fuzzy_bits));
    BOOST_CHECK_CLOSE_FRACTION(sqrt(a9), FixedPointType(sqrt(b9)), tolerance_maker<FixedPointType>(fuzzy_bits));
  }
}

BOOST_AUTO_TEST_CASE(test_negatable_func_sqrt_tiny)
{
  { typedef boost::fixed_point::negatable<7,  -8> fixed_point_type; local::test_sqrt<fixed_point_type>(1); }
  { typedef boost::fixed_point::negatable<4, -11> fixed_point_type; local::test_sqrt<fixed_point_type>(1); }
}


/*******************************************************************************
 * Copyright (c) 2014, 2015  IBM Corporation and others
 *
 * 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.
 *******************************************************************************/

#include <boost/math/distributions/chi_squared.hpp>
#include "MathUtils.hpp"
#include "LocException.hpp"

double MathUtils::quantileChiSquaredDistribution(int degreeOfFreedom, double cumulativeDensity){
    boost::math::chi_squared chi_sq(degreeOfFreedom);
    double x = boost::math::quantile(chi_sq, cumulativeDensity);
    return x;
}

DirectionalStatistics MathUtils::computeDirectionalStatistics(std::vector<double> orientations){
    size_t n = orientations.size();
    if(n==0){
        BOOST_THROW_EXCEPTION(LocException("The size of input orientation vector is zero."));
    }
    double x = 0, y = 0;
    for(auto ori : orientations){
        x += std::cos(ori);
        y += std::sin(ori);
    }
    x /= n;
    y /= n;
    double meanOri = std::atan2(y,x);
    double R = std::sqrt(x*x + y*y);
    double v = 1.0 - R;
    DirectionalStatistics oristat(meanOri, v);
    return oristat;
}

WrappedNormalParameter MathUtils::computeWrappedNormalParameters(const std::vector<double>& orientations){

    size_t n = orientations.size();

    DirectionalStatistics dstats = MathUtils::computeDirectionalStatistics(orientations);

    double mu = dstats.circularMean();
    double R = 1.0 - dstats.circularVariance();

    double R2 = R*R>1.0/n? R*R : 1.0/n;
    double Re2 = (double)n/(n-1)*(R2 - 1.0/n);

    double sigma2 = std::log(1.0/Re2);
    double sigma =  sigma2>0? std::sqrt(sigma2) : 0.0;

    WrappedNormalParameter param(mu, sigma);
    return param;
}


#include <tagloc/tracking.h>
#include <Eigen/Dense>
#define USE_MATH_DEFINES
#include <math.h>
#include <iostream>

using namespace std;

int main(int argc, char** argv){
	Eigen::MatrixXd t = Eigen::MatrixXd::Zero(2,1);
	Eigen::MatrixXd r = Eigen::MatrixXd::Zero(2,1);
	r<<10,10;
	double th = M_PI/4;
	Eigen::MatrixXd R = Eigen::MatrixXd::Zero(2,2);
	R<<cos(th),-sin(th),sin(th),cos(th);
	Eigen::MatrixXd P = Eigen::MatrixXd::Zero(2,2);
	P<<4,0,0,1;
	P = R*P*R.transpose();
	cout<<P<<endl;

	Eigen::MatrixXd cp;

	cout<<"---"<<endl;
	cp = RSN::closest_pt_ellipse(t,P,r,1);
	cout<<"Expect something along [1,1]"<<endl;
	cout<<cp<<endl;

	cout<<"---"<<endl;
	r<<-10,-10;
	cp = RSN::closest_pt_ellipse(t,P,r,1);
	cout<<"Expect reflection of above"<<endl;
	cout<<cp<<endl;

	cout<<"---"<<endl;
	r<<-10,-10;
	cout<<"x:"<<endl;
	cout<<r<<endl;
	th = 0;
	R<<cos(th),-sin(th),sin(th),cos(th);
	P<<4,0,0,1;
	P = R*P*R.transpose();
	cout<<P<<endl;
	cout<<"cp:"<<endl;
	cp = RSN::closest_pt_ellipse(t,P,r,1);
	cout<<cp<<endl;

	cout<<"---"<<endl;
	cout<<"Same as above, but with target shifted to [20,0]"<<endl;
	t<<20,0;
	r<<-10,-10;
	cout<<"x:"<<endl;
	cout<<r<<endl;
	th = 0;
	R<<cos(th),-sin(th),sin(th),cos(th);
	P<<4,0,0,1;
	P = R*P*R.transpose();
	cout<<P<<endl;
	cout<<"cp:"<<endl;
	cp = RSN::closest_pt_ellipse(t,P,r,1);
	cout<<cp<<endl;
	return 0;
}


//
//  Copyright Toon Knapen and Kresimir Fresl
//
// 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_NUMERIC_BINDINGS_BLAS_BLAS1_HPP
#define BOOST_NUMERIC_BINDINGS_BLAS_BLAS1_HPP

#include <boost/numeric/bindings/blas/blas1_overloads.hpp>
#include <boost/numeric/bindings/traits/vector_traits.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <cassert>

namespace boost { namespace numeric { namespace bindings { namespace blas {

  // x <- y
  template < typename vector_x_type, typename vector_y_type >
  void copy(const vector_x_type &x, vector_y_type &y )
  {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    BOOST_STATIC_ASSERT( ( boost::is_same< typename traits::vector_traits<vector_x_type>::value_type, typename traits::vector_traits<vector_y_type>::value_type >::value ) ) ;
#else
    BOOST_STATIC_ASSERT( ( boost::is_same< typename vector_x_type::value_type, typename vector_y_type::value_type >::value ) ) ;
#endif

    const int n =  traits::vector_size( x ) ;
    assert( n==traits::vector_size( y ) ) ;
    const int stride_x = traits::vector_stride( x ) ;
    const int stride_y = traits::vector_stride( y ) ;
    typename traits::vector_traits<vector_x_type>::value_type const *x_ptr = traits::vector_storage( x ) ;
    typename traits::vector_traits<vector_y_type>::value_type *y_ptr = traits::vector_storage( y ) ;

    detail::copy( n, x_ptr, stride_x, y_ptr, stride_y ) ;
  }

  // x <- alpha * x
  template < typename value_type, typename vector_type >
  void scal(const value_type &alpha, vector_type &x )
  {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename traits::vector_traits<vector_type>::value_type >::value ) ) ;
#else
    BOOST_STATIC_ASSERT( ( boost::is_same< value_type, typename vector_type::value_type >::value ) ) ;
#endif

    const int n =  traits::vector_size( x ) ;
    const int stride = traits::vector_stride( x ) ;
    value_type *x_ptr = traits::vector_storage( x ) ;

    detail::scal( n, alpha, x_ptr, stride ) ;
  }

  // y <- alpha * x + y
  template < typename value_type, typename vector_type_x, typename vector_type_y >
  void axpy(const value_type& alpha, const vector_type_x &x, vector_type_y &y )
  {
#ifdef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    BOOST_STATIC_ASSERT( ( is_same< value_type, typename vector_type_x::value_type >::value ) ) ;
    BOOST_STATIC_ASSERT( ( is_same< value_type, typename vector_type_y::value_type >::value ) ) ;
#else
    BOOST_STATIC_ASSERT( ( is_same< value_type, typename traits::vector_traits< vector_type_x >::value_type >::value ) ) ;
    BOOST_STATIC_ASSERT( ( is_same< value_type, typename traits::vector_traits< vector_type_y >::value_type >::value ) ) ;
#endif
    assert( traits::vector_size( x ) == traits::vector_size( y ) ) ;

    const int n = traits::vector_size( x ) ;
    const int stride_x = traits::vector_stride( x ) ;
    const int stride_y = traits::vector_stride( y ) ;
    const value_type *x_ptr = traits::vector_storage( x ) ;
    value_type *y_ptr = traits::vector_storage( y ) ;

    detail::axpy( n, alpha, x_ptr, stride_x, y_ptr, stride_y ) ;
  }

  // dot <- x^T * y  (real vectors)
  template < typename vector_type_x, typename vector_type_y >
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
  typename traits::vector_traits< vector_type_x >::value_type
#else
  typename vector_type_x::value_type
#endif
  dot(const vector_type_x &x, const vector_type_y &y)
  {
#ifdef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    BOOST_STATIC_ASSERT( ( is_same< typename vector_type_y::value_type, typename vector_type_x::value_type >::value ) ) ;
#else
    BOOST_STATIC_ASSERT( ( is_same< typename traits::vector_traits< vector_type_y >::value_type, typename traits::vector_traits< vector_type_x >::value_type >::value ) ) ;
#endif

    assert( traits::vector_size( x ) == traits::vector_size( y ) ) ;

    typedef
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    typename traits::vector_traits< vector_type_x >::value_type
#else
    typename vector_type_x::value_type
#endif
    value_type ;

    const int n = traits::vector_size( x ) ;
    const int stride_x = traits::vector_stride( x ) ;
    const int stride_y = traits::vector_stride( y ) ;

    const value_type *x_ptr = traits::vector_storage( x ) ;
    const value_type *y_ptr = traits::vector_storage( y ) ;

    return detail::dot( n, x_ptr, stride_x, y_ptr, stride_y ) ;
  }

  // dotu <- x^T * y  (complex vectors)
  template < typename vector_type_x, typename vector_type_y >
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
  typename traits::vector_traits< vector_type_x >::value_type
#else
  typename vector_type_x::value_type
#endif
  dotu(const vector_type_x &x, const vector_type_y &y)
  {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    BOOST_STATIC_ASSERT( ( is_same< typename traits::vector_traits< vector_type_y >::value_type, typename traits::vector_traits< vector_type_x >::value_type >::value ) ) ;
#else
    BOOST_STATIC_ASSERT( ( is_same< typename vector_type_y::value_type, typename vector_type_x::value_type >::value ) ) ;
#endif
    assert( traits::vector_size( x ) == traits::vector_size( y ) ) ;

    typedef
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    typename traits::vector_traits< vector_type_x >::value_type
#else
    typename vector_type_x::value_type
#endif
    value_type ;

    const int n = traits::vector_size( x ) ;
    const int stride_x = traits::vector_stride( x ) ;
    const int stride_y = traits::vector_stride( y ) ;
    const value_type *x_ptr = traits::vector_storage( x ) ;
    const value_type *y_ptr = traits::vector_storage( y ) ;

    value_type ret ;
    detail::dotu( ret, n, x_ptr, stride_x, y_ptr, stride_y ) ;
    return ret;
  }

  // dotc <- x^H * y  (complex vectors)
  template < typename vector_type_x, typename vector_type_y >
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
  typename traits::vector_traits< vector_type_x >::value_type
#else
  typename vector_type_x::value_type
#endif
  dotc(const vector_type_x &x, const vector_type_y &y)
  {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    BOOST_STATIC_ASSERT( ( is_same< typename traits::vector_traits< vector_type_y >::value_type, typename traits::vector_traits< vector_type_x >::value_type >::value ) ) ;
#else
    BOOST_STATIC_ASSERT( ( is_same< typename vector_type_y::value_type, typename vector_type_x::value_type >::value ) ) ;
#endif
    assert( traits::vector_size( x ) == traits::vector_size( y ) ) ;

    typedef
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    typename traits::vector_traits< vector_type_x >::value_type
#else
    typename vector_type_x::value_type
#endif
    value_type ;

    const int n = traits::vector_size( x ) ;
    const int stride_x = traits::vector_stride( x ) ;
    const int stride_y = traits::vector_stride( y ) ;
    const value_type *x_ptr = traits::vector_storage( x ) ;
    const value_type *y_ptr = traits::vector_storage( y ) ;

    value_type ret ;
    detail::dotc( ret, n, x_ptr, stride_x, y_ptr, stride_y ) ;
    return ret;
  }

  // nrm2 <- ||x||_2
  template < typename vector_type >
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
  typename traits::type_traits< typename traits::vector_traits< vector_type >::value_type >::real_type
#else
  typename traits::type_traits< typename vector_type::value_type >::real_type
#endif
  nrm2(const vector_type &x)
  {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    typedef typename traits::vector_traits< vector_type >::value_type value_type;
#else
    typedef vector_type::value_type value_type ;
#endif
    const int n = traits::vector_size( x ) ;
    const int stride_x = traits::vector_stride( x ) ;
    const value_type *x_ptr = traits::vector_storage( x ) ;

    return detail::nrm2( n, x_ptr, stride_x ) ;
  }

  // asum <- ||x||_1
  // .. for now works only with real vectors
  template < typename vector_type >
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
  typename traits::type_traits< typename traits::vector_traits< vector_type >::value_type >::real_type
#else
  typename traits::type_traits< typename vector_type::value_type >::real_type
#endif
  asum(const vector_type &x)
  {
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
    typedef typename traits::vector_traits< vector_type >::value_type value_type;
#else
    typedef vector_type::value_type value_type ;
#endif

    const int n = traits::vector_size( x ) ;
    const int stride_x = traits::vector_stride( x ) ;
    const value_type *x_ptr = traits::vector_storage( x ) ;

    return detail::asum( n, x_ptr, stride_x ) ;
  }

}}}}

#endif // BOOST_NUMERIC_BINDINGS_BLAS_BLAS1_HPP


#include "forward_dynamics.h"

#include <Eigen/Cholesky>
#include <iit/rbd/robcogen_commons.h>

using namespace iit::rbd;

// Initialization of static-const data
const ur5::rcg::ForwardDynamics::ExtForces
    ur5::rcg::ForwardDynamics::zeroExtForces(Force::Zero());

ur5::rcg::ForwardDynamics::ForwardDynamics(InertiaProperties& inertia, MotionTransforms& transforms) :
    inertiaProps( & inertia ),
    motionTransforms( & transforms )
{
    shoulder_v.setZero();
    shoulder_c.setZero();
    upper_arm_v.setZero();
    upper_arm_c.setZero();
    forearm_v.setZero();
    forearm_c.setZero();
    wrist_1_v.setZero();
    wrist_1_c.setZero();
    wrist_2_v.setZero();
    wrist_2_c.setZero();
    wrist_3_v.setZero();
    wrist_3_c.setZero();

    vcross.setZero();
    Ia_r.setZero();

}

void ur5::rcg::ForwardDynamics::fd(
    JointState& qdd,
    const JointState& qd,
    const JointState& tau,
    const ExtForces& fext/* = zeroExtForces */)
{

    shoulder_AI = inertiaProps->getTensor_shoulder();
    shoulder_p = - fext[SHOULDER];
    upper_arm_AI = inertiaProps->getTensor_upper_arm();
    upper_arm_p = - fext[UPPER_ARM];
    forearm_AI = inertiaProps->getTensor_forearm();
    forearm_p = - fext[FOREARM];
    wrist_1_AI = inertiaProps->getTensor_wrist_1();
    wrist_1_p = - fext[WRIST_1];
    wrist_2_AI = inertiaProps->getTensor_wrist_2();
    wrist_2_p = - fext[WRIST_2];
    wrist_3_AI = inertiaProps->getTensor_wrist_3();
    wrist_3_p = - fext[WRIST_3];
    // ---------------------- FIRST PASS ---------------------- //
    // Note that, during the first pass, the articulated inertias are really
    //  just the spatial inertia of the links (see assignments above).
    //  Afterwards things change, and articulated inertias shall not be used
    //  in functions which work specifically with spatial inertias.

    // + Link shoulder
    //  - The spatial velocity:
    shoulder_v(AZ) = qd(SHOULDER_PAN);

    //  - The bias force term:
    shoulder_p += vxIv(qd(SHOULDER_PAN), shoulder_AI);

    // + Link upper_arm
    //  - The spatial velocity:
    upper_arm_v = (motionTransforms-> fr_upper_arm_X_fr_shoulder) * shoulder_v;
    upper_arm_v(AZ) += qd(SHOULDER_LIFT);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(upper_arm_v, vcross);
    upper_arm_c = vcross.col(AZ) * qd(SHOULDER_LIFT);

    //  - The bias force term:
    upper_arm_p += vxIv(upper_arm_v, upper_arm_AI);

    // + Link forearm
    //  - The spatial velocity:
    forearm_v = (motionTransforms-> fr_forearm_X_fr_upper_arm) * upper_arm_v;
    forearm_v(AZ) += qd(ELBOW);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(forearm_v, vcross);
    forearm_c = vcross.col(AZ) * qd(ELBOW);

    //  - The bias force term:
    forearm_p += vxIv(forearm_v, forearm_AI);

    // + Link wrist_1
    //  - The spatial velocity:
    wrist_1_v = (motionTransforms-> fr_wrist_1_X_fr_forearm) * forearm_v;
    wrist_1_v(AZ) += qd(WR1);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(wrist_1_v, vcross);
    wrist_1_c = vcross.col(AZ) * qd(WR1);

    //  - The bias force term:
    wrist_1_p += vxIv(wrist_1_v, wrist_1_AI);

    // + Link wrist_2
    //  - The spatial velocity:
    wrist_2_v = (motionTransforms-> fr_wrist_2_X_fr_wrist_1) * wrist_1_v;
    wrist_2_v(AZ) += qd(WR2);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(wrist_2_v, vcross);
    wrist_2_c = vcross.col(AZ) * qd(WR2);

    //  - The bias force term:
    wrist_2_p += vxIv(wrist_2_v, wrist_2_AI);

    // + Link wrist_3
    //  - The spatial velocity:
    wrist_3_v = (motionTransforms-> fr_wrist_3_X_fr_wrist_2) * wrist_2_v;
    wrist_3_v(AZ) += qd(WR3);

    //  - The velocity-product acceleration term:
    motionCrossProductMx<Scalar>(wrist_3_v, vcross);
    wrist_3_c = vcross.col(AZ) * qd(WR3);

    //  - The bias force term:
    wrist_3_p += vxIv(wrist_3_v, wrist_3_AI);

    // ---------------------- SECOND PASS ---------------------- //
    Matrix66 IaB;
    Force pa;

    // + Link wrist_3
    wrist_3_u = tau(WR3) - wrist_3_p(AZ);
    wrist_3_U = wrist_3_AI.col(AZ);
    wrist_3_D = wrist_3_U(AZ);

    compute_Ia_revolute(wrist_3_AI, wrist_3_U, wrist_3_D, Ia_r);  // same as: Ia_r = wrist_3_AI - wrist_3_U/wrist_3_D * wrist_3_U.transpose();
    pa = wrist_3_p + Ia_r * wrist_3_c + wrist_3_U * wrist_3_u/wrist_3_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_wrist_3_X_fr_wrist_2, IaB);
    wrist_2_AI += IaB;
    wrist_2_p += (motionTransforms-> fr_wrist_3_X_fr_wrist_2).transpose() * pa;

    // + Link wrist_2
    wrist_2_u = tau(WR2) - wrist_2_p(AZ);
    wrist_2_U = wrist_2_AI.col(AZ);
    wrist_2_D = wrist_2_U(AZ);

    compute_Ia_revolute(wrist_2_AI, wrist_2_U, wrist_2_D, Ia_r);  // same as: Ia_r = wrist_2_AI - wrist_2_U/wrist_2_D * wrist_2_U.transpose();
    pa = wrist_2_p + Ia_r * wrist_2_c + wrist_2_U * wrist_2_u/wrist_2_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_wrist_2_X_fr_wrist_1, IaB);
    wrist_1_AI += IaB;
    wrist_1_p += (motionTransforms-> fr_wrist_2_X_fr_wrist_1).transpose() * pa;

    // + Link wrist_1
    wrist_1_u = tau(WR1) - wrist_1_p(AZ);
    wrist_1_U = wrist_1_AI.col(AZ);
    wrist_1_D = wrist_1_U(AZ);

    compute_Ia_revolute(wrist_1_AI, wrist_1_U, wrist_1_D, Ia_r);  // same as: Ia_r = wrist_1_AI - wrist_1_U/wrist_1_D * wrist_1_U.transpose();
    pa = wrist_1_p + Ia_r * wrist_1_c + wrist_1_U * wrist_1_u/wrist_1_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_wrist_1_X_fr_forearm, IaB);
    forearm_AI += IaB;
    forearm_p += (motionTransforms-> fr_wrist_1_X_fr_forearm).transpose() * pa;

    // + Link forearm
    forearm_u = tau(ELBOW) - forearm_p(AZ);
    forearm_U = forearm_AI.col(AZ);
    forearm_D = forearm_U(AZ);

    compute_Ia_revolute(forearm_AI, forearm_U, forearm_D, Ia_r);  // same as: Ia_r = forearm_AI - forearm_U/forearm_D * forearm_U.transpose();
    pa = forearm_p + Ia_r * forearm_c + forearm_U * forearm_u/forearm_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_forearm_X_fr_upper_arm, IaB);
    upper_arm_AI += IaB;
    upper_arm_p += (motionTransforms-> fr_forearm_X_fr_upper_arm).transpose() * pa;

    // + Link upper_arm
    upper_arm_u = tau(SHOULDER_LIFT) - upper_arm_p(AZ);
    upper_arm_U = upper_arm_AI.col(AZ);
    upper_arm_D = upper_arm_U(AZ);

    compute_Ia_revolute(upper_arm_AI, upper_arm_U, upper_arm_D, Ia_r);  // same as: Ia_r = upper_arm_AI - upper_arm_U/upper_arm_D * upper_arm_U.transpose();
    pa = upper_arm_p + Ia_r * upper_arm_c + upper_arm_U * upper_arm_u/upper_arm_D;
    ctransform_Ia_revolute(Ia_r, motionTransforms-> fr_upper_arm_X_fr_shoulder, IaB);
    shoulder_AI += IaB;
    shoulder_p += (motionTransforms-> fr_upper_arm_X_fr_shoulder).transpose() * pa;

    // + Link shoulder
    shoulder_u = tau(SHOULDER_PAN) - shoulder_p(AZ);
    shoulder_U = shoulder_AI.col(AZ);
    shoulder_D = shoulder_U(AZ);

    // ---------------------- THIRD PASS ---------------------- //
    shoulder_a = (motionTransforms-> fr_shoulder_X_fr_base).col(LZ) * (ur5::rcg::g);
    qdd(SHOULDER_PAN) = (shoulder_u - shoulder_U.dot(shoulder_a)) / shoulder_D;
    shoulder_a(AZ) += qdd(SHOULDER_PAN);

    upper_arm_a = (motionTransforms-> fr_upper_arm_X_fr_shoulder) * shoulder_a + upper_arm_c;
    qdd(SHOULDER_LIFT) = (upper_arm_u - upper_arm_U.dot(upper_arm_a)) / upper_arm_D;
    upper_arm_a(AZ) += qdd(SHOULDER_LIFT);

    forearm_a = (motionTransforms-> fr_forearm_X_fr_upper_arm) * upper_arm_a + forearm_c;
    qdd(ELBOW) = (forearm_u - forearm_U.dot(forearm_a)) / forearm_D;
    forearm_a(AZ) += qdd(ELBOW);

    wrist_1_a = (motionTransforms-> fr_wrist_1_X_fr_forearm) * forearm_a + wrist_1_c;
    qdd(WR1) = (wrist_1_u - wrist_1_U.dot(wrist_1_a)) / wrist_1_D;
    wrist_1_a(AZ) += qdd(WR1);

    wrist_2_a = (motionTransforms-> fr_wrist_2_X_fr_wrist_1) * wrist_1_a + wrist_2_c;
    qdd(WR2) = (wrist_2_u - wrist_2_U.dot(wrist_2_a)) / wrist_2_D;
    wrist_2_a(AZ) += qdd(WR2);

    wrist_3_a = (motionTransforms-> fr_wrist_3_X_fr_wrist_2) * wrist_2_a + wrist_3_c;
    qdd(WR3) = (wrist_3_u - wrist_3_U.dot(wrist_3_a)) / wrist_3_D;
    wrist_3_a(AZ) += qdd(WR3);

}


#include <QTime>
#include <QApplication>
#include <QAction>
#include <QStringList>

#include "opengl_tools.h"
#include "Scene_polyhedron_item.h"
#include "Scene_points_with_normal_item.h"
#include "Scene_polylines_item.h"
#include "Scene_polyhedron_selection_item.h"
#include "Polyhedron_type.h"

#include <CGAL/Three/Polyhedron_demo_plugin_helper.h>
#include <CGAL/Three/Polyhedron_demo_plugin_interface.h>

#include <CGAL/convex_hull_3.h>
#include <boost/iterator/transform_iterator.hpp>
using namespace CGAL::Three;
class Polyhedron_demo_convex_hull_plugin :
  public QObject,
  public Polyhedron_demo_plugin_helper
{
  Q_OBJECT
  Q_INTERFACES(CGAL::Three::Polyhedron_demo_plugin_interface)
  Q_PLUGIN_METADATA(IID "com.geometryfactory.PolyhedronDemo.PluginInterface/1.0")
public:
    void init(QMainWindow* mainWindow,
              Scene_interface* scene_interface)
    {
        mw = mainWindow;
        scene = scene_interface;
        actions_map["actionConvexHull"] = getActionFromMainWindow(mw, "actionConvexHull");
        actions_map["actionConvexHull"]->setProperty("subMenuName",
                                                     "3D Convex Hulls");
        autoConnectActions();

    }

  // used by Polyhedron_demo_plugin_helper
  QStringList actionsNames() const {
    return QStringList() << "actionConvexHull";
  }

  bool applicable(QAction*) const {
    return
      qobject_cast<Scene_polyhedron_item*>(scene->item(scene->mainSelectionIndex())) ||
      qobject_cast<Scene_polylines_item*>(scene->item(scene->mainSelectionIndex())) ||
      qobject_cast<Scene_points_with_normal_item*>(scene->item(scene->mainSelectionIndex())) ||
      qobject_cast<Scene_polyhedron_selection_item*>(scene->item(scene->mainSelectionIndex()));
  }

public Q_SLOTS:
  void on_actionConvexHull_triggered();

}; // end Polyhedron_demo_convex_hull_plugin

// for transform iterator
struct Get_point {
  typedef const Polyhedron::Point_3& result_type;
  result_type operator()(const Polyhedron::Vertex_handle v) const
  { return v->point(); }
};

void Polyhedron_demo_convex_hull_plugin::on_actionConvexHull_triggered()
{
  const CGAL::Three::Scene_interface::Item_id index = scene->mainSelectionIndex();

  Scene_polyhedron_item* poly_item =
    qobject_cast<Scene_polyhedron_item*>(scene->item(index));

  Scene_points_with_normal_item* pts_item =
    qobject_cast<Scene_points_with_normal_item*>(scene->item(index));

  Scene_polylines_item* lines_item =
    qobject_cast<Scene_polylines_item*>(scene->item(index));

  Scene_polyhedron_selection_item* selection_item =
    qobject_cast<Scene_polyhedron_selection_item*>(scene->item(index));

  if(poly_item || pts_item || lines_item || selection_item)
  {
    // wait cursor
    QApplication::setOverrideCursor(Qt::WaitCursor);

    QTime time;
    time.start();
    std::cout << "Convex hull...";

    // add convex hull as new polyhedron
    Polyhedron *pConvex_hull = new Polyhedron;
    if(selection_item) {
      CGAL::convex_hull_3(
        boost::make_transform_iterator(selection_item->selected_vertices.begin(), Get_point()),
        boost::make_transform_iterator(selection_item->selected_vertices.end(), Get_point()),
        *pConvex_hull);
    }
    else if ( poly_item ){
      Polyhedron* pMesh = poly_item->polyhedron();
      CGAL::convex_hull_3(pMesh->points_begin(),pMesh->points_end(),*pConvex_hull);
    }
    else{
      if (pts_item)
        CGAL::convex_hull_3(pts_item->point_set()->begin(),pts_item->point_set()->end(),*pConvex_hull);
      else{
        std::size_t nb_points=0;
        for(std::list<std::vector<Kernel::Point_3> >::const_iterator it = lines_item->polylines.begin();
            it != lines_item->polylines.end();
            ++it)  nb_points+=it->size();

        std::vector<Kernel::Point_3> all_points;
        all_points.reserve( nb_points );

        for(std::list<std::vector<Kernel::Point_3> >::const_iterator it = lines_item->polylines.begin();
            it != lines_item->polylines.end();
            ++it)  std::copy(it->begin(), it->end(),std::back_inserter( all_points ) );

        CGAL::convex_hull_3(all_points.begin(),all_points.end(),*pConvex_hull);
      }
    }
    std::cout << "ok (" << time.elapsed() << " ms)" << std::endl;

    Scene_polyhedron_item* new_item = new Scene_polyhedron_item(pConvex_hull);
    new_item->setName(tr("%1 (convex hull)").arg(scene->item(index)->name()));
    new_item->setColor(Qt::magenta);
    new_item->setRenderingMode(FlatPlusEdges);
    scene->addItem(new_item);

    // default cursor
    QApplication::restoreOverrideCursor();
  }
}

#include "Convex_hull_plugin.moc"