/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/
#ifndef __itkOdfMaximaExtractionFilter_cpp
#define __itkOdfMaximaExtractionFilter_cpp

#include "itkOdfMaximaExtractionFilter.h"
#include <itkImageRegionConstIterator.h>
#include <itkImageRegionConstIteratorWithIndex.h>
#include <itkImageRegionIterator.h>
#include <vnl/vnl_vector.h>
#include <itkOrientationDistributionFunction.h>
#include <itkContinuousIndex.h>

#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkPolyLine.h>

#include <boost/math/special_functions.hpp>
#include <boost/progress.hpp>

using namespace boost::math;

namespace itk {

static bool CompareVectors(const vnl_vector_fixed< double, 3 >& v1, const vnl_vector_fixed< double, 3 >& v2)
{
  return (v1.magnitude()>v2.magnitude());
}

template< class PixelType, int ShOrder, int NrOdfDirections >
OdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::OdfMaximaExtractionFilter()
  : m_NormalizationMethod(MAX_VEC_NORM)
  , m_MaxNumPeaks(2)
  , m_RelativePeakThreshold(0.4)
  , m_AbsolutePeakThreshold(0)
  , m_AngularThreshold(0.9)
  , m_NumCoeffs((ShOrder*ShOrder + ShOrder + 2)/2 + ShOrder)
  , m_Toolkit(MRTRIX)
  , m_FlipX(false)
  , m_FlipY(false)
  , m_FlipZ(false)
  , m_ApplyDirectionMatrix(false)
  , m_ScaleByGfa(false)
  , m_Iterations(10)
{
  this->SetNumberOfRequiredInputs(1);
}

template< class PixelType, int ShOrder, int NrOdfDirections >
double OdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::FindCandidatePeaks(OdfType& odf, double thr, std::vector< DirectionType >& container)
{
  double gfa = 1.0;
  if (m_ScaleByGfa)
    gfa = odf.GetGeneralizedFractionalAnisotropy();

  //Find the peaks using a finite difference method
  bool flag = true;
  vnl_vector_fixed< bool, NrOdfDirections > used; used.fill(false);
  //Find the peaks
  for (int i=0; i<NrOdfDirections; i++)
  {
    if (used[i])
      continue;

    double val = odf.GetElement(i);
    if (val>thr*0.9 && gfa*val>m_AbsolutePeakThreshold*0.9)
    {
      flag = true;
      std::vector< int > neighbours = odf.GetNeighbors(i);
      for (unsigned int j=0; j<neighbours.size(); j++)
        if (val<odf.GetElement(neighbours.at(j)))
        {
          flag = false;
          break;
        }
      if (flag)   // point is a peak
      {
        container.push_back(odf.GetDirection(i).normalize());
        used[i] = true;
        for (unsigned int j=0; j<neighbours.size(); j++)
          used[neighbours.at(j)] = true;
      }
    }
  }
  return gfa;
}

template< class PixelType, int ShOrder, int NrOdfDirections >
void OdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::BeforeThreadedGenerateData()
{
  typename CoefficientImageType::Pointer ShCoeffImage = static_cast< CoefficientImageType* >( this->ProcessObject::GetInput(0) );
  itk::Vector<double,3> spacing = ShCoeffImage->GetSpacing();
  double minSpacing = spacing[0];
  if (spacing[1]<minSpacing)
    minSpacing = spacing[1];
  if (spacing[2]<minSpacing)
    minSpacing = spacing[2];

  mitk::Point3D origin = ShCoeffImage->GetOrigin();
  itk::Matrix<double, 3, 3> direction = ShCoeffImage->GetDirection();
  ImageRegion<3> imageRegion = ShCoeffImage->GetLargestPossibleRegion();

  if (m_MaskImage.IsNotNull())
  {
    origin = m_MaskImage->GetOrigin();
    direction = m_MaskImage->GetDirection();
    imageRegion = m_MaskImage->GetLargestPossibleRegion();
  }

  itk::Vector<double, 3> spacing3 = ShCoeffImage->GetSpacing();
  itk::Point<float, 3> origin3 = ShCoeffImage->GetOrigin();
  itk::Matrix<double, 3, 3> direction3 = ShCoeffImage->GetDirection();
  itk::ImageRegion<3> imageRegion3 = ShCoeffImage->GetLargestPossibleRegion();

  itk::Vector<double, 4> spacing4;
  itk::Point<float, 4> origin4;
  itk::Matrix<double, 4, 4> direction4;
  itk::ImageRegion<4> imageRegion4;

  spacing4[0] = spacing3[0]; spacing4[1] = spacing3[1]; spacing4[2] = spacing3[2]; spacing4[3] = 1;
  origin4[0] = origin3[0]; origin4[1] = origin3[1]; origin4[2] = origin3[2]; origin4[3] = 0;
  for (int r=0; r<3; r++)
    for (int c=0; c<3; c++)
      direction4[r][c] = direction3[r][c];
  direction4[3][3] = 1;
  imageRegion4.SetSize(0, imageRegion3.GetSize()[0]);
  imageRegion4.SetSize(1, imageRegion3.GetSize()[1]);
  imageRegion4.SetSize(2, imageRegion3.GetSize()[2]);
  imageRegion4.SetSize(3, m_MaxNumPeaks*3);

  m_PeakImage = PeakImageType::New();
  m_PeakImage->SetSpacing( spacing4 );
  m_PeakImage->SetOrigin( origin4 );
  m_PeakImage->SetDirection( direction4 );
  m_PeakImage->SetRegions( imageRegion4 );
  m_PeakImage->Allocate();
  m_PeakImage->FillBuffer(0.0);

  if (m_MaskImage.IsNull())
  {
    m_MaskImage = ItkUcharImgType::New();
    m_MaskImage->SetSpacing( spacing );
    m_MaskImage->SetOrigin( origin );
    m_MaskImage->SetDirection( direction );
    m_MaskImage->SetRegions( imageRegion );
    m_MaskImage->Allocate();
    m_MaskImage->FillBuffer(1);
  }
  m_NumDirectionsImage = ItkUcharImgType::New();
  m_NumDirectionsImage->SetSpacing( spacing );
  m_NumDirectionsImage->SetOrigin( origin );
  m_NumDirectionsImage->SetDirection( direction );
  m_NumDirectionsImage->SetRegions( imageRegion );
  m_NumDirectionsImage->Allocate();
  m_NumDirectionsImage->FillBuffer(0);

  // calculate SH basis
  OdfType odf;
  vnl_matrix< double > dir_matrix;
  dir_matrix.set_size(3, NrOdfDirections);
  for (int i=0; i<NrOdfDirections; i++)
  {
    dir_matrix(0, i) = odf.GetDirection(i)[0];
    dir_matrix(1, i) = odf.GetDirection(i)[1];
    dir_matrix(2, i) = odf.GetDirection(i)[2];
  }

  if (m_Toolkit==Toolkit::MRTRIX)
    m_ShBasis = mitk::sh::CalcShBasisForDirections(ShOrder, dir_matrix);
  else
    m_ShBasis = mitk::sh::CalcShBasisForDirections(ShOrder, dir_matrix, false);

  MITK_INFO << "Starting peak extraction";
  MITK_INFO << "SH order: " << ShOrder;
  MITK_INFO << "Maximum peaks: " << m_MaxNumPeaks;
  MITK_INFO << "Relative threshold: " << m_RelativePeakThreshold;
  MITK_INFO << "Absolute threshold: " << m_AbsolutePeakThreshold;
  MITK_INFO << "Angular threshold: " << m_AngularThreshold;
  this->SetNumberOfThreads(1);
}

template< class PixelType, int ShOrder, int NrOdfDirections >
void OdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::AfterThreadedGenerateData()
{

}

template< class PixelType, int ShOrder, int NrOdfDirections >
void OdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::ThreadedGenerateData( const OutputImageRegionType& outputRegionForThread, ThreadIdType threadID )
{
  typename CoefficientImageType::Pointer ShCoeffImage = static_cast< CoefficientImageType* >( this->ProcessObject::GetInput(0) );

  ImageRegionConstIterator< CoefficientImageType > cit(ShCoeffImage, outputRegionForThread );

  OdfType odf;
  while( !cit.IsAtEnd() )
  {
    typename CoefficientImageType::IndexType idx3 = cit.GetIndex();
    if (m_MaskImage->GetPixel(idx3)==0)
    {
      ++cit;
      continue;
    }

    CoefficientPixelType c = cit.Get();
    vnl_vector<float> coeffs; coeffs.set_size((ShOrder*ShOrder + ShOrder + 2)/2 + ShOrder);
    for (int j=0; j<m_NumCoeffs; j++)
      coeffs[j] = c[j];

    double max = 0;
    for (int i=0; i<NrOdfDirections; i++)
    {
      odf[i] = 0;
      for (int j=0; j<m_NumCoeffs; j++)
        odf[i] += c[j]*m_ShBasis(i,j);
      if (odf[i]>max)
        max = odf[i];
    }
    if (max<0.0001)
    {
      ++cit;
      continue;
    }

    std::vector< DirectionType > candidates, final_peaks;
    double scale = FindCandidatePeaks(odf, max*m_RelativePeakThreshold, candidates);       // find all local maxima

    max = 0;
    for (unsigned int i=0; i<candidates.size(); ++i)
    {
      double spherical[3];
      mitk::sh::Cart2Sph(candidates[i][0], candidates[i][1], candidates[i][2], spherical);
      vnl_vector<double> x;
      x.set_size(2);
      x[0] = spherical[1];  // theta
      x[1] = spherical[0];  // phi

      VnlCostFunction cost;
      if (m_Toolkit==Toolkit::MRTRIX)
        cost.SetProblem(coeffs, ShOrder, true, max);
      else
        cost.SetProblem(coeffs, ShOrder, false, max);

      vnl_lbfgsb minimizer(cost);
      minimizer.set_f_tolerance(1e-6);
//      minimizer.set_trace(true);

      vnl_vector<double> l; l.set_size(2); l[0] = 0; l[1] = -itk::Math::pi;
      vnl_vector<double> u; u.set_size(2); u[0] = itk::Math::pi; u[1] = itk::Math::pi;
      vnl_vector<long> bound_selection; bound_selection.set_size(2); bound_selection.fill(2);
      minimizer.set_bound_selection(bound_selection);
      minimizer.set_lower_bound(l);
      minimizer.set_upper_bound(u);
      if (m_Iterations>0)
        minimizer.set_max_function_evals(m_Iterations);
      minimizer.minimize(x);

      float v = -minimizer.get_end_error()*scale;
      candidates[i] = mitk::sh::Sph2Cart(x[0], x[1], v);
      if (v>max)
        max = v;
    }

    std::sort( candidates.begin(), candidates.end(), CompareVectors );  // sort peaks

    // kick out directions to close to a larger direction
    for (unsigned int i=0; i<candidates.size(); i++)
    {
      DirectionType v1 = candidates.at(i);
      double val = v1.magnitude();
      if (val<max*m_RelativePeakThreshold || val<m_AbsolutePeakThreshold)
        break;

      bool flag = true;
      for (unsigned int j=0; j<candidates.size(); j++)
        if (i!=j)
        {
          DirectionType v2 = candidates.at(j);
          double val2 = v2.magnitude();
          double angle = fabs(dot_product(v1,v2)/(val*val2));
          if (angle>m_AngularThreshold && val<val2)
          {
            flag = false;
            break;
          }
        }

      if (flag)
        final_peaks.push_back(v1);
    }

    itk::Index<4> idx4; idx4[0] = idx3[0]; idx4[1] = idx3[1]; idx4[2] = idx3[2];

    // fill output image
    unsigned int num = final_peaks.size();
    if ( num>m_MaxNumPeaks )
      num = m_MaxNumPeaks;
    for (unsigned int i=0; i<num; i++)
    {
      DirectionType dir = final_peaks.at(i);
      switch (m_NormalizationMethod)
      {
      case NO_NORM:
        break;
      case SINGLE_VEC_NORM:
        dir.normalize();
        break;
      case MAX_VEC_NORM:
        dir /= max;
        break;
      }

      if (m_ApplyDirectionMatrix)
        dir = m_MaskImage->GetDirection()*dir;

      if (m_FlipX)
        dir[0] = -dir[0];
      if (m_FlipY)
        dir[1] = -dir[1];
      if (m_FlipZ)
        dir[2] = -dir[2];

      for (unsigned int j = 0; j<3; j++)
      {
        idx4[3] = i*3 + j;
        m_PeakImage->SetPixel(idx4, dir[j]);
      }
    }
    m_NumDirectionsImage->SetPixel(idx3, num);
    ++cit;
  }
  MITK_INFO << "Thread " << threadID << " finished extraction";
}

}

#endif // __itkOdfMaximaExtractionFilter_cpp


//
// Copyright 2010 Scott McMurray.
// 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_HASH_BLOCK_CYPHERS_BASIC_SHACAL_HPP
#define BOOST_HASH_BLOCK_CYPHERS_BASIC_SHACAL_HPP

#include <boost/hash/block_cyphers/detail/shacal_policy.hpp>
#include <boost/hash/block_cyphers/detail/shacal1_policy.hpp>
#include <boost/static_assert.hpp>

#ifdef BOOST_HASH_SHOW_PROGRESS
#include <cstdio>
#endif

//
// Encrypt implemented directly from the SHA standard as found at
// http://csrc.nist.gov/publications/fips/fips180-2/fips180-2.pdf
//
// Decrypt is a straight-forward inverse
//
// In SHA terminology:
// - plaintext = H^(i-1)
// - cyphertext = H^(i)
// - key = M^(i)
// - schedule = W
//

namespace boost {
namespace hashes {
namespace block_cyphers {

//
// The algorithms for SHA(-0) and SHA-1 are identical apart from the
// key scheduling, so encapsulate that as a class that takes an
// already-prepared schedule.  (Constructor is protected to help keep
// people from accidentally giving it just a key in a schedule.)
//

class basic_shacal {
  public:
    typedef detail::shacal_policy policy_type;

    static unsigned const word_bits = policy_type::word_bits;
    typedef policy_type::word_type word_type;

    static unsigned const key_bits = policy_type::key_bits;
    static unsigned const key_words = policy_type::key_words;
    typedef policy_type::key_type key_type;

    static unsigned const block_bits = policy_type::block_bits;
    static unsigned const block_words = policy_type::block_words;
    typedef policy_type::block_type block_type;

    static unsigned const rounds = policy_type::rounds;
    typedef policy_type::schedule_type schedule_type;

  protected:
    basic_shacal(schedule_type const &s) : schedule(s) {}
  private:
    schedule_type const schedule;

  public:
    block_type
    encypher(block_type const &plaintext) {
        return encypher_block(plaintext);
    }
  private:
    block_type
    encypher_block(block_type const &plaintext) {
        return encypher_block(schedule, plaintext);
    }
    static block_type
    encypher_block(schedule_type const &schedule,
                   block_type const &plaintext) {

#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t < block_words; ++t) {
            std::printf(word_bits == 32 ?
                        "H[%d] = %.8x\n" :
                        "H[%d] = %.16lx\n",
                        t, plaintext[t]);
        }
#endif

        // Initialize working variables with block
        word_type a = plaintext[0], b = plaintext[1],
                  c = plaintext[2], d = plaintext[3],
                  e = plaintext[4];

        // Encypher block
#ifdef BOOST_HASH_NO_OPTIMIZATION

        for (unsigned t = 0; t < rounds; ++t) {
            word_type T = policy_type::ROTL<5>(a)
                        + policy_type::f(t,b,c,d)
                        + e
                        + policy_type::constant(t)
                        + schedule[t];

            e = d;
            d = c;
            c = policy_type::ROTL<30>(b);
            b = a;
            a = T;

#ifdef BOOST_HASH_SHOW_PROGRESS
            printf(word_bits == 32 ?
                   "t = %2d: %.8x %.8x %.8x %.8x %.8x\n" :
                   "t = %2d: %.16lx %.16lx %.16lx %.16lx %.16lx\n",
                   t, a, b, c, d, e);
#endif
        }

#else // BOOST_HASH_NO_OPTIMIZATION

#   ifdef BOOST_HASH_SHOW_PROGRESS
#       define BOOST_HASH_SHACAL1_TRANSFORM_PROGRESS \
            printf(word_bits == 32 ? \
                   "t = %2d: %.8x %.8x %.8x %.8x %.8x\n" : \
                   "t = %2d: %.16lx %.16lx %.16lx %.16lx %.16lx\n", \
                   t, a, b, c, d, e);
#   else
#       define BOOST_HASH_SHACAL1_TRANSFORM_PROGRESS
#   endif

#   define BOOST_HASH_SHACAL1_TRANSFORM \
            word_type T = policy_type::ROTL<5>(a) \
                        + policy_type::f(t,b,c,d) \
                        + e \
                        + policy_type::constant(t) \
                        + schedule[t]; \
            e = d; \
            d = c; \
            c = policy_type::ROTL<30>(b); \
            b = a; \
            a = T; \
            BOOST_HASH_SHACAL1_TRANSFORM_PROGRESS

        BOOST_STATIC_ASSERT(rounds == 80);
        BOOST_STATIC_ASSERT(rounds % block_words == 0);
        for (unsigned t =  0; t < 20; ) {
            for (int n = block_words; n--; ++t) {
                BOOST_HASH_SHACAL1_TRANSFORM
            }
        }
        for (unsigned t = 20; t < 40; ) {
            for (int n = block_words; n--; ++t) {
                BOOST_HASH_SHACAL1_TRANSFORM
            }
        }
        for (unsigned t = 40; t < 60; ) {
            for (int n = block_words; n--; ++t) {
                BOOST_HASH_SHACAL1_TRANSFORM
            }
        }
        for (unsigned t = 60; t < 80; ) {
            for (int n = block_words; n--; ++t) {
                BOOST_HASH_SHACAL1_TRANSFORM
            }
        }

#endif

        block_type cyphertext = {{a, b, c, d, e}};
        return cyphertext;
    }

  public:
    block_type
    decypher(block_type const &plaintext) {
        return decypher_block(plaintext);
    }
  private:
    block_type
    decypher_block(block_type const &plaintext) {
        return decypher_block(schedule, plaintext);
    }
    static block_type
    decypher_block(schedule_type const &schedule,
                   block_type const &cyphertext) {

#ifdef BOOST_HASH_SHOW_PROGRESS
        for (unsigned t = 0; t < block_words; ++t) {
            std::printf(word_bits == 32 ?
                        "H[%d] = %.8x\n" :
                        "H[%d] = %.16lx\n",
                        t, cyphertext[t]);
        }
#endif

        // Initialize working variables with block
        word_type a = cyphertext[0], b = cyphertext[1],
                  c = cyphertext[2], d = cyphertext[3],
                  e = cyphertext[4];

        // Decypher block
        for (unsigned t = rounds; t--; ) {
            word_type T = a;

            a = b;
            b = policy_type::ROTR<30>(c);
            c = d;
            d = e;
            e = T
              - policy_type::ROTL<5>(a)
              - policy_type::f(t, b, c, d)
              - policy_type::constant(t)
              - schedule[t];

#ifdef BOOST_HASH_SHOW_PROGRESS
            std::printf(word_bits == 32 ?
                        "t = %2d: %.8x %.8x %.8x %.8x %.8x\n" :
                        "t = %2d: %.16lx %.16lx %.16lx %.16lx %.16lx\n",
                        t, a, b, c, d, e);
#endif
        }

        block_type plaintext = {{a, b, c, d, e}};
        return plaintext;
    }

};

} // namespace block_cyphers
} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_BLOCK_CYPHERS_BASIC_SHACAL_HPP


/* boost random/uniform_int.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: uniform_int.hpp 47233 2008-07-08 16:22:46Z steven_watanabe $
 *
 * Revision history
 *  2001-04-08  added min<max assertion (N. Becker)
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_UNIFORM_INT_HPP
#define BOOST_RANDOM_UNIFORM_INT_HPP

#include <cassert>
#include <iostream>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/detail/workaround.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/detail/signed_unsigned_tools.hpp>
#include <boost/type_traits/make_unsigned.hpp>
#ifdef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#include <boost/type_traits/is_float.hpp>
#endif

namespace boost {

// uniform integer distribution on [min, max]
template<class IntType = int>
class uniform_int
{
public:
  typedef IntType input_type;
  typedef IntType result_type;
  typedef typename make_unsigned<result_type>::type range_type;

  explicit uniform_int(IntType min_arg = 0, IntType max_arg = 9)
    : _min(min_arg), _max(max_arg)
  {
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    // MSVC fails BOOST_STATIC_ASSERT with std::numeric_limits at class scope
    BOOST_STATIC_ASSERT(std::numeric_limits<IntType>::is_integer);
#endif
    assert(min_arg <= max_arg);
    init();
  }

  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return _min; }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return _max; }
  void reset() { }

  // can't have member function templates out-of-line due to MSVC bugs
  template<class Engine>
  result_type operator()(Engine& eng)
  {
    typedef typename Engine::result_type base_result;
    // ranges are always unsigned
    typedef typename make_unsigned<base_result>::type base_unsigned;
    const base_result bmin = (eng.min)();
    const base_unsigned brange =
      random::detail::subtract<base_result>()((eng.max)(), (eng.min)());

    if(_range == 0) {
      return _min;
    } else if(brange == _range) {
      // this will probably never happen in real life
      // basically nothing to do; just take care we don't overflow / underflow
      base_unsigned v = random::detail::subtract<base_result>()(eng(), bmin);
      return random::detail::add<base_unsigned, result_type>()(v, _min);
    } else if(brange < _range) {
      // use rejection method to handle things like 0..3 --> 0..4
      for(;;) {
        // concatenate several invocations of the base RNG
        // take extra care to avoid overflows
        range_type limit;
        if(_range == (std::numeric_limits<range_type>::max)()) {
          limit = _range/(range_type(brange)+1);
          if(_range % range_type(brange)+1 == range_type(brange))
            ++limit;
        } else {
          limit = (_range+1)/(range_type(brange)+1);
        }
        // We consider "result" as expressed to base (brange+1):
        // For every power of (brange+1), we determine a random factor
        range_type result = range_type(0);
        range_type mult = range_type(1);
        while(mult <= limit) {
          result += random::detail::subtract<base_result>()(eng(), bmin) * mult;
          mult *= range_type(brange)+range_type(1);
        }
        if(mult == limit)
          // _range+1 is an integer power of brange+1: no rejections required
          return result;
        // _range/mult < brange+1  -> no endless loop
        result += uniform_int<range_type>(0, _range/mult)(eng) * mult;
        if(result <= _range)
          return random::detail::add<range_type, result_type>()(result, _min);
      }
    } else {                   // brange > range
      if(brange / _range > 4 /* quantization_cutoff */ ) {
        // the new range is vastly smaller than the source range,
        // so quantization effects are not relevant
        return boost::uniform_smallint<result_type>(_min, _max)(eng);
      } else {
        // use rejection method to handle cases like 0..5 -> 0..4
        for(;;) {
          base_unsigned result =
	    random::detail::subtract<base_result>()(eng(), bmin);
          // result and range are non-negative, and result is possibly larger
          // than range, so the cast is safe
          if(result <= static_cast<base_unsigned>(_range))
            return random::detail::add<base_unsigned, result_type>()(result, _min);
        }
      }
    }
  }

#if !defined(BOOST_NO_OPERATORS_IN_NAMESPACE) && !defined(BOOST_NO_MEMBER_TEMPLATE_FRIENDS)
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const uniform_int& ud)
  {
    os << ud._min << " " << ud._max;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, uniform_int& ud)
  {
# if BOOST_WORKAROUND(_MSC_FULL_VER, BOOST_TESTED_AT(13102292)) && BOOST_MSVC == 1400
      return detail::extract_uniform_int(is, ud, ud.impl);
# else
   is >> std::ws >> ud._min >> std::ws >> ud._max;
    ud.init();
    return is;
# endif
  }
#endif

private:
  void init()
  {
    _range = random::detail::subtract<result_type>()(_max, _min);
  }

  // The result_type may be signed or unsigned, but the _range is always
  // unsigned.
  result_type _min, _max;
  range_type _range;
};

} // namespace boost

#endif // BOOST_RANDOM_UNIFORM_INT_HPP


//          Copyright Gavin Band 2008 - 2012.
// 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)

#include <iostream>
#include <iomanip>
#include <string>
#include <Eigen/Core>
#include "genfile/CohortIndividualSource.hpp"
#include "genfile/Error.hpp"
#include "statfile/BuiltInTypeStatSource.hpp"
#include "components/RelatednessComponent/UDUTDecompositionLoader.hpp"

namespace relatedness {
	UDUTDecompositionLoader::UDUTDecompositionLoader( genfile::CohortIndividualSource const& samples ):
	 	m_samples( samples )
	{}

	void UDUTDecompositionLoader::send_UDUT_to( ResultCallback callback ) {
		m_result_signal.connect( callback ) ;
	}

	void UDUTDecompositionLoader::load_matrix( std::string const& filename ) const {
		Eigen::MatrixXd matrix ;
		std::size_t number_of_snps = 0;
		load_matrix_impl( filename, &matrix, &number_of_snps ) ;
		m_result_signal( matrix, m_samples.get_number_of_individuals(), number_of_snps ) ;
	}

	void UDUTDecompositionLoader::load_matrix_impl( std::string const& filename, Eigen::MatrixXd* matrix, std::size_t* number_of_snps ) const {
		statfile::BuiltInTypeStatSource::UniquePtr source = statfile::BuiltInTypeStatSource::open( filename ) ;
		assert( matrix ) ;
		assert( number_of_snps ) ;

		using namespace genfile::string_utils ;
		// Read the metadata
		std::size_t number_of_samples = 0 ;
		std::vector< std::string > metadata = split( source->get_descriptive_text(), "\n" ) ;
		for( std::size_t i = 0; i < metadata.size(); ++i ) {
			std::vector< std::string > elts = split_and_strip( metadata[i], ":", " " ) ;
			if( elts.size() == 2 && elts[0] == "Number of SNPs" ) {
				*number_of_snps = to_repr< std::size_t >( elts[1] ) ;
			}
			else if( elts.size() == 2 && elts[0] == "Number of samples" ) {
				number_of_samples = to_repr< std::size_t >( elts[1] ) ;
			}
		}
		if( number_of_samples != m_samples.get_number_of_individuals() ) {
			throw genfile::MismatchError(
				"UDUTDecompositionLoader::load_matrix()",
				filename,
				"number of samples: " + to_string( number_of_samples ),
				"expected number: " + to_string( m_samples.get_number_of_individuals() )
			) ;
		}

		if( source->number_of_columns() != number_of_samples + 1 ) {
			throw genfile::MalformedInputError( source->get_source_spec(), 0, std::min( source->number_of_columns(), number_of_samples + 1 )) ;
		}
		if( source->number_of_rows() && *source->number_of_rows() != number_of_samples ) {
			throw genfile::MalformedInputError( source->get_source_spec(), 0, std::min( *source->number_of_rows(), number_of_samples )) ;
		}
		// Read the matrix, making sure the samples come in the same order as in the sample file.
		matrix->resize( number_of_samples, number_of_samples+1 ) ;
		for( std::size_t i = 0; i < number_of_samples; ++i ) {
			for( std::size_t j = 0; j < ( number_of_samples + 1 ); ++j ) {
				(*source) >> (*matrix)(i,j) ;
			}
			(*source) >> statfile::end_row() ;
		}
		if( source->number_of_rows_read() != number_of_samples ) {
			throw genfile::MalformedInputError( source->get_source_spec(), 0, source->number_of_rows_read() ) ;
		}

	}
}


#include <fstream>  // NOLINT

#include <Eigen/Core>
#include <Eigen/Dense>
#include <aslam/common/feature-descriptor-ref.h>
#include <descriptor-projection/descriptor-projection.h>
#include <descriptor-projection/flags.h>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <maplab-common/binary-serialization.h>
#include <maplab-common/eigen-proto.h>

namespace descriptor_projection {
void ProjectDescriptor(
    const aslam::common::FeatureDescriptorConstRef& raw_descriptor,
    const Eigen::MatrixXf& projection_matrix, int target_dimensions,
    std::vector<float>* projected_descriptor) {
  CHECK_NOTNULL(projected_descriptor);
  Eigen::Matrix<float, Eigen::Dynamic, 1> descriptor;
  const int descriptor_bits = raw_descriptor.size() * 8;
  descriptor.resize(descriptor_bits, Eigen::NoChange);
  DescriptorToEigenMatrix(raw_descriptor, descriptor);
  ProjectDescriptor(
      descriptor, projection_matrix, target_dimensions, projected_descriptor);
}

void ProjectDescriptorBlock(
    const std::vector<aslam::common::FeatureDescriptorConstRef>&
        raw_descriptors,
    const Eigen::MatrixXf& projection_matrix, int target_dimensions,
    Eigen::MatrixXf* projected_descriptors) {
  if (raw_descriptors.empty()) {
    return;
  }
  CHECK_NOTNULL(projected_descriptors);
  projected_descriptors->resize(target_dimensions, raw_descriptors.size());
  Eigen::MatrixXf converted_descriptors;
  const int num_descriptor_bytes = raw_descriptors[0].size();
  const int num_descriptor_bits = num_descriptor_bytes * 8;
  converted_descriptors.resize(num_descriptor_bits, raw_descriptors.size());
  for (size_t i = 0; i < raw_descriptors.size(); ++i) {
    DescriptorToEigenMatrix(
        raw_descriptors[i],
        converted_descriptors.block(0, i, num_descriptor_bits, 1));
  }
  *projected_descriptors =
      projection_matrix.block(
          0, 0, target_dimensions, projection_matrix.cols()) *
      converted_descriptors.block(
          0, 0, projection_matrix.cols(), projected_descriptors->cols());
}

void ProjectDescriptorBlock(
    const Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic>&
        raw_descriptors,
    const Eigen::MatrixXf& projection_matrix, int target_dimensions,
    Eigen::MatrixXf* projected_descriptors) {
  if (raw_descriptors.cols() == 0) {
    return;
  }
  CHECK_NOTNULL(projected_descriptors);
  projected_descriptors->resize(target_dimensions, raw_descriptors.cols());
  Eigen::MatrixXf converted_descriptors;
  const int num_descriptor_bytes = raw_descriptors.rows();
  const int num_descriptor_bits = num_descriptor_bytes * 8;
  converted_descriptors.resize(num_descriptor_bits, raw_descriptors.cols());
  for (int i = 0; i < raw_descriptors.cols(); ++i) {
    DescriptorToEigenMatrix(
        raw_descriptors.block(0, i, raw_descriptors.rows(), 1),
        converted_descriptors.block(0, i, num_descriptor_bits, 1));
  }

  if (projection_matrix.cols() == 471) {
    CHECK_EQ(512, num_descriptor_bits)
        << "Projection matrix dimensions don't match the descriptor length. "
        << "Double check your setting for feature_descriptor_type.";
  } else {
    CHECK_EQ(projection_matrix.cols(), num_descriptor_bits)
        << "Projection matrix dimensions don't match the descriptor length. "
        << "Double check your setting for feature_descriptor_type.";
  }

  *projected_descriptors =
      projection_matrix.block(
          0, 0, target_dimensions, projection_matrix.cols()) *
      converted_descriptors.block(
          0, 0, projection_matrix.cols(), projected_descriptors->cols());
}

bool LoadprojectionMatrix(Eigen::MatrixXf* projection_matrix) {
  CHECK_NOTNULL(projection_matrix);

  std::ifstream deserializer(FLAGS_lc_projection_matrix_filename);
  CHECK(deserializer.is_open()) << "Cannot load projection matrix from file: "
                                << FLAGS_lc_projection_matrix_filename;
  common::Deserialize(projection_matrix, &deserializer);
  return true;
}
}  // namespace descriptor_projection

namespace loop_closure {

void ProjectedImage::serialize(
    proto::ProjectedImage* proto_projected_image) const {
  CHECK_NOTNULL(proto_projected_image);
  proto_projected_image->set_timestamp_nanoseconds(timestamp_nanoseconds);
  keyframe_id.vertex_id.serialize(proto_projected_image->mutable_vertex_id());
  proto_projected_image->set_frame_index(keyframe_id.frame_index);
  dataset_id.serialize(proto_projected_image->mutable_dataset_id());

  common::eigen_proto::serialize(
      projected_descriptors,
      proto_projected_image->mutable_projected_descriptors());

  common::eigen_proto::serialize(
      measurements, proto_projected_image->mutable_measurements());

  for (const vi_map::LandmarkId& landmark_id : landmarks) {
    CHECK(landmark_id.isValid());
    common::proto::Id* proto_id = proto_projected_image->add_landmarks();
    CHECK_NOTNULL(proto_id);
    landmark_id.serialize(proto_id);
  }
}

void ProjectedImage::deserialize(
    const proto::ProjectedImage& proto_projected_image) {
  timestamp_nanoseconds = proto_projected_image.timestamp_nanoseconds();
  CHECK_GE(timestamp_nanoseconds, 0);

  keyframe_id.vertex_id.deserialize(proto_projected_image.vertex_id());
  CHECK(keyframe_id.vertex_id.isValid());

  keyframe_id.frame_index = proto_projected_image.frame_index();

  dataset_id.deserialize(proto_projected_image.dataset_id());
  CHECK(dataset_id.isValid());

  common::eigen_proto::deserialize(
      proto_projected_image.projected_descriptors(), &projected_descriptors);

  common::eigen_proto::deserialize(
      proto_projected_image.measurements(), &measurements);

  const int num_landmarks = proto_projected_image.landmarks_size();
  CHECK_GE(num_landmarks, 0);

  landmarks.clear();
  landmarks.reserve(num_landmarks);
  for (int landmark_idx = 0; landmark_idx < num_landmarks; ++landmark_idx) {
    vi_map::LandmarkId landmark_id;
    landmark_id.deserialize(
        proto_projected_image.landmarks(landmark_idx));
    CHECK(landmark_id.isValid());
    landmarks.push_back(landmark_id);
  }
}

}  // namespace loop_closure


#include <ilasynth/memvalues.hpp>
#include <set>

#include <boost/python.hpp>
#include <ilasynth/assert.hpp>
#include <ilasynth/ast.hpp>
#include <ilasynth/exception.hpp>
#include <ilasynth/logging.hpp>
#include <ilasynth/smt.hpp>
#include <ilasynth/util.hpp>

namespace ilasynth {
// ---------------------------------------------------------------------- //
z3::context MemValues::c_eq;
z3::solver MemValues::S_eq(c_eq);

// ---------------------------------------------------------------------- //
MemValues::MemValues() : MAX_ADDR(0), def_value(0) {}

MemValues::MemValues(int aw, int dw, const py::object& dv)
    : type(NodeType::getMem(aw, dw)), MAX_ADDR(mp_int_t(1) << aw),
      def_value(to_cpp_int(dv)) {}

MemValues::MemValues(int aw, int dw, const map_t values,
                     const mp_int_t def_value)
    : type(NodeType::getMem(aw, dw)), MAX_ADDR(mp_int_t(1) << aw),
      def_value(def_value), values(values) {}

MemValues::MemValues(Z3ExprAdapter& c, const z3::model& m, const MemVar* mem)
    : type(mem->type), MAX_ADDR(mp_int_t(1) << type.addrWidth) {
  using namespace z3;

  // extract model.
  expr mexp = c.getExpr(mem);
  log2("Z3Update.is_app") << "mem exp " << mexp << std::endl;
  expr mval = m.eval(mexp);
  log2("Z3Update.is_app") << "mem val " << mval << std::endl;
  func_decl mfd = mval.decl();

  // std::cout << "mval: " << mval << std::endl;
  // std::cout << "mfd: " << mfd << std::endl;

  // a bunch of assertions copied from stackoverflow.

  typedef std::pair<mp_int_t, mp_int_t> addr_data_pair;
  std::vector<addr_data_pair> ADStack;
  // peel the structural
  while (Z3_get_decl_kind(c.ctx(), mfd) == Z3_OP_STORE) {
    expr addr = mval.arg(1);
    expr data = mval.arg(2);
    std::string saddr(Z3_get_numeral_string(c.ctx(), addr));
    std::string sdata(Z3_get_numeral_string(c.ctx(), data));
    auto ki = boost::lexical_cast<mp_int_t>(saddr);
    auto kv = boost::lexical_cast<mp_int_t>(sdata);

    ADStack.push_back(addr_data_pair(ki, kv));

    mval = mval.arg(0);
    mfd = mval.decl();
  }

  // handle the rest of the cases
  if (Z3_get_decl_kind(c.ctx(), mfd) == Z3_OP_CONST_ARRAY) {
    ILA_ASSERT(Z3_is_app(c.ctx(), mval), "Expected application.");
    // let's try interpret it directly
    ILA_ASSERT(Z3_get_decl_num_parameters(c.ctx(), mfd) == 1,
               "BUG unable to handle");
    Z3_sort memsort = Z3_get_decl_sort_parameter(c.ctx(), mfd, 0);
    ILA_ASSERT(Z3_get_sort_kind(c.ctx(), memsort) == Z3_ARRAY_SORT,
               "expect ARRAY sort.");

    // std::cerr<<"mval: " << mval << std::endl;
    // std::cerr<<"mfd: " << mfd << std::endl;

    // std::cout<<"narg:"<<mval.num_args()<<std::endl;
    expr arg0 = mval.arg(0);
    // std::cout<<"arg0:"<<arg0<<std::endl;
    func_decl arg0decl = arg0.decl();
    // std::cout<<"arg0 decl:"<<arg0decl<<std::endl;

    std::string sdef(Z3_get_numeral_string(c.ctx(), arg0));
    def_value = boost::lexical_cast<mp_int_t>(sdef);
  } else if (Z3_get_decl_kind(c.ctx(), mfd) == Z3_OP_UNINTERPRETED) {
    def_value = 0;
  } else {
    ILA_ASSERT(Z3_get_decl_kind(c.ctx(), mfd) == Z3_OP_AS_ARRAY,
               "Expected array op.");
    ILA_ASSERT(Z3_is_app(c.ctx(), mval), "Expected application.");
    ILA_ASSERT(Z3_get_decl_num_parameters(c.ctx(), mfd) == 1,
               "Expected one parameter.");
    ILA_ASSERT(Z3_get_decl_parameter_kind(c.ctx(), mfd, 0) ==
                   Z3_PARAMETER_FUNC_DECL,
               "Expected parameter to a function declaration.");

    // get the function interpretation (also SO).
    func_decl ffd =
        func_decl(c.ctx(), Z3_get_decl_func_decl_parameter(c.ctx(), mfd, 0));
    func_interp fip = m.get_func_interp(ffd);

    // now walk through the values.
    for (unsigned i = 0, n = fip.num_entries(); i < n; i++) {
      func_entry ei = fip.entry(i);
      expr eki = ei.arg(0);
      expr evi = ei.value();
      std::string ski(Z3_get_numeral_string(c.ctx(), eki));
      std::string svi(Z3_get_numeral_string(c.ctx(), evi));

      auto ki = boost::lexical_cast<mp_int_t>(ski);
      auto vi = boost::lexical_cast<mp_int_t>(svi);
      values[ki] = vi;
    }
    std::string sdef(Z3_get_numeral_string(c.ctx(), fip.else_value()));
    def_value = boost::lexical_cast<mp_int_t>(sdef);
  }
  // let's pop back the things into the values
  for (auto&& pair : ADStack) {
    values[pair.first] = pair.second;
  }

  gc();
}

MemValues::MemValues(const MemValues& that)
    : type(that.type), MAX_ADDR(that.MAX_ADDR), def_value(that.def_value),
      values(that.values) {}

MemValues::~MemValues() {}

// ---------------------------------------------------------------------- //
MemValues& MemValues::operator=(const MemValues& that) {
  type = that.type;
  MAX_ADDR = that.MAX_ADDR;
  def_value = that.def_value;
  values = that.values;
  return *this;
}

// ---------------------------------------------------------------------- //
py::object MemValues::getDefault() const { return to_pyint(def_value); }

void MemValues::setDefault(const py::object& o) {
  try {
    mp_int_t dv = to_cpp_int(o);
    def_value = dv;
  } catch (const boost::bad_lexical_cast&) {
    throw PyILAException(PyExc_ValueError, "Invalid default value.");
  }
}

py::object MemValues::getValues() const {
  py::list l;
  for (auto p : values) {
    auto ai = to_pyint(p.first);
    auto di = to_pyint(p.second);
    auto ti = make_tuple(ai, di);
    l.append(ti);
  }
  return l;
}

mp_int_t MemValues::getItemInt(const mp_int_t& index) const {
  auto pos = values.find(index);
  if (pos == values.end())
    return def_value;
  else
    return pos->second;
}

py::object MemValues::getItem(const py::object& index_) const {
  try {
    mp_int_t index = to_cpp_int(index_);
    if (index < 0 || index >= MAX_ADDR) {
      throw PyILAException(PyExc_IndexError, "Index out of range.");
      return py::object();
    }
    return to_pyint(getItemInt(index));
  } catch (const boost::bad_lexical_cast&) {
    throw PyILAException(PyExc_ValueError, "Invalid index value.");
  }
}

void MemValues::setItem(const py::object& index_, const py::object& value_) {
  mp_int_t index;
  mp_int_t value;

  try {
    index = to_cpp_int(index_);
  } catch (const boost::bad_lexical_cast&) {
    throw PyILAException(PyExc_ValueError, "Invalid index value.");
  }

  try {
    value = to_cpp_int(value_);
  } catch (const boost::bad_lexical_cast&) {
    throw PyILAException(PyExc_ValueError, "Invalid value.");
  }

  if (value == def_value) {
    auto pos = values.find(index);
    if (pos != values.end()) {
      values.erase(pos);
    }
  } else {
    values[index] = value;
  }
}

bool MemValues::operator==(const MemValues& that) const {
  // different types then definitely not equal.
  if (type != that.type)
    return false;
  // these are syntactic checks.
  if (def_value != that.def_value)
    return false;
  for (auto it : values) {
    if (getItemInt(it.first) != that.getItemInt(it.first)) {
      return false;
    }
  }
  for (auto it : that.values) {
    if (that.getItemInt(it.first) != getItemInt(it.first)) {
      return false;
    }
  }
  return true;
}

bool MemValues::semanticEqual(const MemValues& that) const {
  using namespace z3;
  using namespace boost;

  log2("MemValues::semanticEqual")
      << "this=" << *this << "; that=" << that << std::endl;

  if (type != that.type)
    return false;
  if (*this == that)
    return true;

  std::string name("addr_" + lexical_cast<std::string>(type.addrWidth));
  expr addr = c_eq.bv_const(name.c_str(), type.addrWidth);
  expr mem1 = toZ3(c_eq);
  expr mem2 = that.toZ3(c_eq);
  expr rd1 = select(mem1, addr);
  expr rd2 = select(mem2, addr);
  S_eq.push();
  S_eq.add(rd1 != rd2);
  auto res = S_eq.check();
  S_eq.pop();

  return (res == unsat);
}

z3::expr MemValues::toZ3(z3::context& c) const {
  auto addrsort = c.bv_sort(type.addrWidth);
  auto datasort = c.bv_sort(type.dataWidth);

  auto defstr = boost::lexical_cast<std::string>(def_value);
  auto defexpr = c.bv_val(defstr.c_str(), type.dataWidth);

  auto e1 = const_array(addrsort, defexpr);
  for (auto p : values) {
    auto addrstr = boost::lexical_cast<std::string>(p.first);
    auto addrexpr = c.bv_val(addrstr.c_str(), type.addrWidth);
    auto datastr = boost::lexical_cast<std::string>(p.second);
    auto dataexpr = c.bv_val(datastr.c_str(), type.dataWidth);
    auto e2 = store(e1, addrexpr, dataexpr);
    e1 = e2;
  }
  return e1;
}

void MemValues::gc() {
  std::set<mp_int_t> indices_to_del;
  for (auto&& r : values) {
    if (r.second == def_value) {
      indices_to_del.insert(r.first);
    }
  }

  for (auto i : indices_to_del) {
    values.erase(i);
  }
}

std::string MemValues::str() const {
  std::ostringstream ostr;
  ostr << *this;
  return ostr.str();
}

std::ostream& operator<<(std::ostream& out, const MemValues& mv) {
  bool first = true;
  // save flags.
  auto f = out.flags();

  // output.
  out << "[";
  for (auto p : mv.values) {
    if (!first) {
      out << " ";
    } else {
      first = false;
    }
    out << std::hex << std::showbase << p.first << ":" << p.second;
  }
  if (!first) {
    out << " ";
  }
  out << "default: " << std::hex << std::showbase << mv.def_value << std::dec
      << "]";
  // restore flags.
  out.flags(f);
  return out;
}

} // namespace ilasynth


#ifndef _MARSUPIAL_CFG_HPP_
#define _MARSUPIAL_CFG_HPP_

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

// #include <g2o/core/factory.h>
#include <iomanip>
#include <Eigen/StdVector>

// #include "marsupial_optimizer/bisection_catenary_3D.hpp"

// #include "marsupial_optimizer/g2o_vertex_timediff.h"
// #include "marsupial_optimizer/g2o_vertex_pointxyz.h"

#include "ros/ros.h"

using namespace std;

struct structPose
{
	int id;
	Eigen::Vector3d position;
    g2o::VertexPointXYZ vertex;
};

struct structTime
{
	int id_from;
	int id_to;
	double time;
	// g2o::VertexTimeDiff vertex;
};

struct structDistance
{
	int id_from;
	int id_to;
	double dist;
};

class MarsupialCfg
{
	public:
		MarsupialCfg();
		// ~MarsupialCfg();
		// void loadDataCatenary(vector<structPose> &poses, vector<structDistance> &distances);	//Function to create a catenary from point A to B
		void setStraightTrajectory(double x1, double y1, double z1, double x2, double y2, double z2, int n_v_u_);	//Function to create a straight line from point A to B
		// void setStraightTrajectory();
		// void straightTrajectoryVertices(double x1, double y1, double z1, double x2, double y2, double z2, int _n_v_u, vector<Eigen::Vector3d> &_v);
		void setNumVertUnit(int n_);
		// void setDistance3D(float x1, float y1, float z1, float x2, float y2, float z2);
		// void setNumPoints(float d_, int n_v);	// To define approximately the number of points to analyze per length unity
		void setVelocity(double v_);
		void getSlopXYZAxes( vector<float> &m_);
		bool getParameterOptimizer(std::string fileName);
		// VertexPointXYZ poseVertex(int index);
		// VertexTimeDiff timeDiffVertex(int index) ;
		//! Container of poses that represent the spatial part of the trajectory
		typedef vector<structPose> PoseSequence;
		//! Container of time differences that define the temporal of the trajectory
		typedef vector<structTime> TimeSequence;
		//! Container of distances between two consecutive vertices of the trajectory
		typedef vector<structDistance> DistanceSequence;
		PoseSequence pose_vec_; //!< Internal container storing the sequence of optimzable pose vertices
		DistanceSequence dist_vec_; //!< Internal container storing the distances between two consecutive vertices
		TimeSequence time_vec_;  //!< Internal container storing the sequence of optimzable timediff vertices
		int n_vert_unit;	//number of Vertex per unit of lenght
		float d3D_;
		float n_points;
		double velocity;
		vector<float> slopeXYZ;
	protected:
	private:
};

MarsupialCfg::MarsupialCfg(){
}

// MarsupialCfg::~MarsupialCfg(){
// }

inline void MarsupialCfg::setNumVertUnit(int n_) {n_vert_unit = n_;}
// inline void MarsupialCfg::setDistance3D(float x1, float y1, float z1, float x2, float y2, float z2) {d3D_= sqrt(pow(x2-x1,2)+pow(y2-y1,2)+pow(z2-z1,2));}
inline void MarsupialCfg::setVelocity(double v_) {velocity = v_;}
// inline void MarsupialCfg::setNumPoints(float d_, int n_v_u_) {n_points = ceil(d_*n_v_u_);}
inline void MarsupialCfg::getSlopXYZAxes(vector<float> &m_) {m_=slopeXYZ;}

// void MarsupialCfg::loadDataCatenary(vector<structPose> &poses, vector<structDistance> &distances)
// {
// 	// bisection B;
// 	structPose ps;
// 	structDistance pd;
// 	for (size_t i = 0; i < B.catenary_chain_points_3D.size(); i++)
// 	{
// 		ps.id = i;
// 		ps.position = Eigen::Vector3d(B.catenary_chain_points_3D[i].x_,B.catenary_chain_points_3D[i].y_,B.catenary_chain_points_3D[i].z_);
// 		poses.push_back(ps);
// 	}
// 	for (size_t i = 0; i < B.catenary_chain_points_3D.size()-1; i++)
// 	{
// 		pd.id_from = i;
// 		pd.id_to = i+1;
// 		pd.dist = sqrt(pow(B.catenary_chain_points_3D[i].x_-B.catenary_chain_points_3D[i+1].x_,2)+
// 					pow(B.catenary_chain_points_3D[i].y_-B.catenary_chain_points_3D[i+1].y_,2)+
// 					pow(B.catenary_chain_points_3D[i].z_-B.catenary_chain_points_3D[i+1].z_,2));
// 		distances.push_back(pd);
// 	}
// }

void MarsupialCfg::setStraightTrajectory(double x1, double y1, double z1, double x2, double y2, double z2, int n_v_u_)
{
	structPose ps;
	structDistance pd;
	structTime pt;
	double x_, y_, z_;
	d3D_= sqrt(pow(x2-x1,2)+pow(y2-y1,2)+pow(z2-z1,2));
	ROS_INFO("Initial 3D distance of trayectory = %f",d3D_);
	n_points = ceil(d3D_*n_v_u_);
	double distance_xy = sqrt(pow(x2-x1,2)+pow(y2-y1,2));
	double distance_xz = sqrt(pow(x2-x1,2)+pow(z2-z1,2));
	double interval_xy = distance_xy/n_points;		// Size of the interval between points in axes xy
	double interval_xz = distance_xz/n_points;		// Size of the interval between points in axes xz
	slopeXYZ.push_back((x2-x1)/n_points);	//save the value of distance between two vertex in the X axes
	slopeXYZ.push_back((y2-y1)/n_points);	//save the value of distance between two vertex in the Y axes
	slopeXYZ.push_back((z2-z1)/n_points);	//save the value of distance between two vertex in the Z axes
	for(int i = 0 ; i< n_points+1 ; i++)
	{
		x_ = x1 + i*interval_xy * ((x2-x1)/distance_xy);
		y_ = y1 + i*interval_xy * ((y2-y1)/distance_xy);
		z_ = z1 + i*interval_xz * ((z2-z1)/distance_xz);
		// x_z = x1 + i*interval_xz * ((x2-x1)/distance_xz);
		ps.id = i;
		ps.position = Eigen::Vector3d(x_,y_,z_);
		ps.vertex.setEstimate(ps.position);
		pose_vec_.push_back(ps);
	}
	for(int i = 0 ; i< n_points ; i++)
	{
		pd.id_from = i;
		pd.id_to = i+1;
		pd.dist =  sqrt(pow(pose_vec_[i].position.x()-pose_vec_[i+1].position.x(),2)+
						pow(pose_vec_[i].position.y()-pose_vec_[i+1].position.y(),2)+
						pow(pose_vec_[i].position.z()-pose_vec_[i+1].position.z(),2));
		pt.id_from = i;
		pt.id_to = i+1;
		pt.time = pd.dist / velocity;
		dist_vec_.push_back(pd);
		time_vec_.push_back(pt);
	}
}

// void MarsupialCfg::setStraightTrajectory()
// {
// 	structDistance pd;
// 	for(unsigned i = 0 ; i< pose_vec_.size()-1 ; i++)
// 	{
// 		pd.id_from = i;
// 		pd.id_to = i+1;
// 		pd.dist =  sqrt(pow(pose_vec_[i].position.x()-pose_vec_[i+1].position.x(),2)+
// 						pow(pose_vec_[i].position.y()-pose_vec_[i+1].position.y(),2)+
// 						pow(pose_vec_[i].position.z()-pose_vec_[i+1].position.z(),2));
// 		dist_vec_.push_back(pd);
// 	}
// }

// void MarsupialCfg::straightTrajectoryVertices(double x1, double y1, double z1, double x2, double y2, double z2, int _n_v_u, vector<Eigen::Vector3d> &_v)
// {
// 	double _x, _y, _z;
// 	double _d= sqrt(pow(x2-x1,2)+pow(y2-y1,2)+pow(z2-z1,2));
// 	// double _n = ceil(_d*n_v_u_);
// 	double distance_xy = sqrt(pow(x2-x1,2)+pow(y2-y1,2));
// 	double distance_xz = sqrt(pow(x2-x1,2)+pow(z2-z1,2));
// 	double interval_xy = distance_xy/_n_v_u;		// Size of the interval between points in axes xy
// 	double interval_xz = distance_xz/_n_v_u;		// Size of the interval between points in axes xz
// 	for(int i = 0 ; i< _n_v_u ; i++)
// 	{
// 		_x = x1 + i*interval_xy * ((x2-x1)/distance_xy);
// 		_y = y1 + i*interval_xy * ((y2-y1)/distance_xy);
// 		_z = z1 + i*interval_xz * ((z2-z1)/distance_xz);

// 		_v.push_back(Eigen::Vector3d(_x,_y,_z));
// 	}

// }
// VertexPointXYZ MarsupialCfg::poseVertex(int index)
// {
// return pose_vec_.at(index).vertex;
// }
// VertexTimeDiff MarsupialCfg::timeDiffVertex(int index)
// {
// return time_vec_.at(index).vertex;
// }

#endif


/* Copyright 2017 Battelle Energy Alliance, LLC

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

   http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/
/*
 * distribution_1D.cpp
 *
 *  Created on: Mar 22, 2012
 *      Author: MANDD
 *      Modified: alfoa
 *      References:
 *      1- G. Cassella, R.G. Berger, "Statistical Inference", 2nd ed. Pacific Grove, CA: Duxbury Press (2001).
 *
 */

#include "distribution_1D.h"
#include "distributionFunctions.h"
#include <cmath>               // needed to use erfc error function
#include <string>
#include "dynamicArray.h"
#include <ctime>
#include <cstdlib>
//#include "InterpolationFunctions.h"
#include <string>
#include <limits>
#include <iso646.h>
#include <boost/math/distributions/uniform.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/lognormal.hpp>
#include <boost/math/distributions/triangular.hpp>
#include <boost/math/distributions/exponential.hpp>
#include <boost/math/distributions/weibull.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/beta.hpp>
#include <boost/math/distributions/poisson.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/math/distributions/logistic.hpp>
#include <boost/math/distributions/bernoulli.hpp>
#include <boost/math/distributions/laplace.hpp>
#include <boost/math/distributions/geometric.hpp>

#define _USE_MATH_DEFINES   // needed in order to use M_PI = 3.14159

#define throwError(msg) { std::cerr << "\n\n" << msg << "\n\n"; throw std::runtime_error("Error"); }

class DistributionBackend {
public:
  virtual double pdf(double x) = 0;
  virtual double cdf(double x) = 0;
  virtual double cdfComplement(double x) = 0;
  virtual double quantile(double x) = 0;
  virtual double mean() = 0;
  virtual double standard_deviation() = 0;
  virtual double median() = 0;
  virtual double mode() = 0;
  virtual double hazard(double x) = 0;
  virtual ~DistributionBackend() {};
};

/*
 * Class Basic Truncated Distribution
 * This class implements a basic truncated distribution that can
 * be inherited from.
 */

BasicTruncatedDistribution::BasicTruncatedDistribution(double x_min, double x_max)
{
  if(not hasParameter("truncation"))
  {
    _dist_parameters["truncation"] = 1.0;
  }
    _dist_parameters["xMin"] = x_min;
    _dist_parameters["xMax"] = x_max;
}

double
BasicTruncatedDistribution::pdf(double x){
  double value;
  double x_min = _dist_parameters.find("xMin") ->second;
  double x_max = _dist_parameters.find("xMax") ->second;

  if (_dist_parameters.find("truncation") ->second == 1) {
    if ((x<x_min)||(x>x_max)) {
      value=0;
    } else {
      value = 1/(untrCdf(x_max) - untrCdf(x_min)) * untrPdf(x);
    }
  } else {
    value=-1;
  }

  return value;
}

double
BasicTruncatedDistribution::cdf(double x){
  double value;
  double x_min = _dist_parameters.find("xMin") ->second;
  double x_max = _dist_parameters.find("xMax") ->second;

  if (_dist_parameters.find("truncation") ->second == 1) {
    if (x<x_min) {
      value=0;
    } else if (x>x_max) {
      value=1;
    } else{
      value = 1/(untrCdf(x_max) - untrCdf(x_min)) * (untrCdf(x)- untrCdf(x_min));
    }
  } else {
    value=-1;
  }

  return value;
}

double
BasicTruncatedDistribution::inverseCdf(double x){
  double value;
  double x_min = _dist_parameters.find("xMin") ->second;
  double x_max = _dist_parameters.find("xMax") ->second;

  if(x == 0.0) {
    //Using == in floats is generally a bad idea, but
    // 0.0 can be represented exactly.
    //In this case, return the minimum value
    return x_min;
  }
  if(x == 1.0) {
    //Using == in floats is generally a bad idea, but
    // 1.0 can be represented exactly.
    //In this case, return the maximum value
    return x_max;
  }
  if (_dist_parameters.find("truncation") ->second == 1){
    double temp=untrCdf(x_min)+x*(untrCdf(x_max)-untrCdf(x_min));
    value=untrInverseCdf(temp);
  } else {
    throwError("A valid solution for inverseCdf was not found!");
  }
  return value;
}

double BasicTruncatedDistribution::untrPdf(double x) {
  return _backend->pdf(x);
}

double BasicTruncatedDistribution::untrCdf(double x) {
  return _backend->cdf(x);
}

double BasicTruncatedDistribution::untrCdfComplement(double x) {
  return _backend->cdfComplement(x);
}

double BasicTruncatedDistribution::untrInverseCdf(double x) {
  return _backend->quantile(x);
}

double BasicTruncatedDistribution::untrMean() {
  return _backend->mean();
}

/**
   Calculates the untruncated standard deviation
   \return the standard deviation
*/
double BasicTruncatedDistribution::untrStdDev() {
  return _backend->standard_deviation();
}

double BasicTruncatedDistribution::untrMedian() {
  return _backend->median();
}

double BasicTruncatedDistribution::untrMode() {
  return _backend->mode();
}

double BasicTruncatedDistribution::untrHazard(double x) {
  return _backend->hazard(x);
}

/*
 * Class Basic Discrete Distribution
 * This class implements a basic discrete distribution that can
 * be inherited from.
 */

double BasicDiscreteDistribution::untrPdf(double x) {
  return _backend->pdf(x);
}

double BasicDiscreteDistribution::untrCdf(double x) {
  return _backend->cdf(x);
}

double BasicDiscreteDistribution::untrCdfComplement(double x) {
  return _backend->cdfComplement(x);
}

double BasicDiscreteDistribution::untrInverseCdf(double x) {
  return _backend->quantile(x);
}

double BasicDiscreteDistribution::untrMean() {
  return _backend->mean();
}

/**
   Calculates the untruncated standard deviation
   \return the standard deviation
*/
double BasicDiscreteDistribution::untrStdDev() {
  return _backend->standard_deviation();
}

double BasicDiscreteDistribution::untrMedian() {
  return _backend->median();
}

double BasicDiscreteDistribution::untrMode() {
  return _backend->mode();
}

double BasicDiscreteDistribution::untrHazard(double x) {
  return _backend->hazard(x);
}

double BasicDiscreteDistribution::pdf(double x) {
  return untrPdf(x);
}

double BasicDiscreteDistribution::cdf(double x) {
  return untrCdf(x);
}

double BasicDiscreteDistribution::inverseCdf(double x) {
  return untrInverseCdf(x);
}

/*
 * Class DistributionBackendTemplate implements a template that
 * can be used to create a DistributionBackend from a boost distribution
 */

template <class T>
class DistributionBackendTemplate : public DistributionBackend {
public:
  double pdf(double x) { return boost::math::pdf(*_backend, x); };
  double cdf(double x) { return boost::math::cdf(*_backend, x); };
  double cdfComplement(double x) { return boost::math::cdf(boost::math::complement(*_backend, x)); };
  double quantile(double x) { return boost::math::quantile(*_backend, x); };
  double mean() { return boost::math::mean(*_backend); };
  double standard_deviation() { return boost::math::standard_deviation(*_backend); };
  double median() { return boost::math::median(*_backend); };
  double mode() { return boost::math::mode(*_backend); };
  double hazard(double x) { return boost::math::hazard(*_backend, x); };
protected:
  T *_backend;
};

/*
 * CLASS UNIFORM DISTRIBUTION
 */

class UniformDistributionBackend : public DistributionBackendTemplate<boost::math::uniform> {
public:
  UniformDistributionBackend(double x_min, double x_max) {
    _backend = new boost::math::uniform(x_min,x_max);
  }
  ~UniformDistributionBackend() {
    delete _backend;
  }
};

BasicUniformDistribution::BasicUniformDistribution(double x_min, double x_max)
{
  _dist_parameters["xMin"] = x_min;
  _dist_parameters["xMax"] = x_max;
  _backend = new UniformDistributionBackend(x_min, x_max);

  if (x_min>x_max)
    throwError("ERROR: bounds for uniform distribution are incorrect");
}

BasicUniformDistribution::~BasicUniformDistribution()
{
  delete _backend;
}

double
BasicUniformDistribution::pdf(double x){
  return untrPdf(x);
}

double
BasicUniformDistribution::cdf(double x){
  return untrCdf(x);
}

double
BasicUniformDistribution::inverseCdf(double x){
  return untrInverseCdf(x);
}

class NormalDistributionBackend : public DistributionBackendTemplate<boost::math::normal> {
public:
  NormalDistributionBackend(double mean, double sd) {
    _backend = new boost::math::normal(mean, sd);
  }
  ~NormalDistributionBackend() {
    delete _backend;
  }
};

/*
 * CLASS NORMAL DISTRIBUTION
 */

BasicNormalDistribution::BasicNormalDistribution(double mu, double sigma) {
  _dist_parameters["mu"] = mu; //mean
  _dist_parameters["sigma"] = sigma; //sd
  if(not hasParameter("truncation")) {
    _dist_parameters["truncation"] = 1.0;
  }
  if(not hasParameter("xMin")) {
    _dist_parameters["xMin"] = -std::numeric_limits<double>::max( );
  }
  if(not hasParameter("xMax")) {
    _dist_parameters["xMax"] = std::numeric_limits<double>::max( );
  }
  //std::cout << "mu " << mu << " sigma " << sigma
  //          << " truncation " << _dist_parameters["truncation"]
  //          << " xMin " << _dist_parameters["xMin"]
  //          << " xMax " << _dist_parameters["xMax"] << std::endl;
  _backend = new NormalDistributionBackend(mu, sigma);
}

BasicNormalDistribution::BasicNormalDistribution(double mu, double sigma, double x_min, double x_max):
  BasicTruncatedDistribution(x_min,x_max)
{
  _dist_parameters["mu"] = mu; //mean
  _dist_parameters["sigma"] = sigma; //sd
  //if(not hasParameter("truncation")) {
  //  _dist_parameters["truncation"] = 1.0;
  //}
  //_dist_parameters["xMin"] = x_min;
  //_dist_parameters["xMax"] = x_max;
  //std::cout << "mu " << mu << " sigma " << sigma
  //          << " truncation " << _dist_parameters["truncation"]
  //          << " xMin " << _dist_parameters["xMin"]
  //          << " xMax " << _dist_parameters["xMax"] << std::endl;
  _backend = new NormalDistributionBackend(mu, sigma);

}

BasicNormalDistribution::~BasicNormalDistribution(){
  delete _backend;
}

double
BasicNormalDistribution::inverseCdf(double x){
  return BasicTruncatedDistribution::inverseCdf(x);
}

class LogNormalDistributionBackend : public DistributionBackendTemplate<boost::math::lognormal> {
public:
  LogNormalDistributionBackend(double mean, double sd) {
    _backend = new boost::math::lognormal(mean, sd);
  }
  ~LogNormalDistributionBackend() {
    delete  _backend;
  }
};

/*
 * CLASS LOG NORMAL DISTRIBUTION
 */

BasicLogNormalDistribution::BasicLogNormalDistribution(double mu, double sigma, double low)
{
  _dist_parameters["mu"] = mu;
  _dist_parameters["sigma"] = sigma;
  _dist_parameters["low"] = low;

  if(not hasParameter("truncation")) {
    _dist_parameters["truncation"] = 1.0;
  }
  if(not hasParameter("xMin")) {
    _dist_parameters["xMin"] = low;
  }
  if(not hasParameter("xMax")) {
    _dist_parameters["xMax"] = std::numeric_limits<double>::max( );
  }

  _backend = new LogNormalDistributionBackend(mu, sigma);

}

BasicLogNormalDistribution::BasicLogNormalDistribution(double mu, double sigma, double x_min, double x_max, double low):
  BasicTruncatedDistribution(x_min,x_max)
{
  _dist_parameters["mu"] = mu;
  _dist_parameters["sigma"] = sigma;
  _dist_parameters["low"] = low;

  _backend = new LogNormalDistributionBackend(mu, sigma);

}

BasicLogNormalDistribution::~BasicLogNormalDistribution()
{
  delete _backend;
}

double
BasicLogNormalDistribution::untrPdf(double x){
  double low = _dist_parameters.find("low") ->second;
  if(x <= low) {
    return 0.0;
  } else {
    return _backend->pdf(x-low);
  }
}

double
BasicLogNormalDistribution::untrCdf(double x){
  double low = _dist_parameters.find("low") ->second;
  if(x <= low) {
    return 0.0;
  } else {
    return _backend->cdf(x-low);
  }
}

double
BasicLogNormalDistribution::inverseCdf(double x){
  double low = _dist_parameters.find("low") ->second;
  return BasicTruncatedDistribution::inverseCdf(x)+low;
}

/*
 * CLASS LOGISTIC DISTRIBUTION
 */

class LogisticDistributionBackend : public DistributionBackendTemplate<boost::math::logistic_distribution<> > {
public:
  LogisticDistributionBackend(double location, double scale) {
    _backend = new boost::math::logistic_distribution<>(location, scale);
  }
  ~LogisticDistributionBackend() {
    delete _backend;
  }
};

BasicLogisticDistribution::BasicLogisticDistribution(double location, double scale)
{
  _dist_parameters["location"] = location;
  _dist_parameters["scale"] = scale;

  if(not hasParameter("truncation")) {
    _dist_parameters["truncation"] = 1.0;
  }
  if(not hasParameter("xMin")) {
    _dist_parameters["xMin"] = -std::numeric_limits<double>::max( );
  }
  if(not hasParameter("xMax")) {
    _dist_parameters["xMax"] = std::numeric_limits<double>::max( );
  }

  _backend = new LogisticDistributionBackend(location, scale);
}

BasicLogisticDistribution::BasicLogisticDistribution(double location, double scale, double x_min, double x_max):
  BasicTruncatedDistribution(x_min,x_max)
{
    _dist_parameters["location"] = location;
    _dist_parameters["scale"] = scale;

    _backend = new LogisticDistributionBackend(location, scale);
}

BasicLogisticDistribution::~BasicLogisticDistribution()
{
  delete _backend;
}

/*
 * CLASS LAPLACE DISTRIBUTION
 */
class LaplaceDistributionBackend : public DistributionBackendTemplate<boost::math::laplace_distribution<> > {
public:
  LaplaceDistributionBackend(double location, double scale) {
    _backend = new boost::math::laplace_distribution<>(location, scale);
  }
  ~LaplaceDistributionBackend() {
    delete _backend;
  }
};

BasicLaplaceDistribution::BasicLaplaceDistribution(double location, double scale, double x_min, double x_max):
    BasicTruncatedDistribution(x_min,x_max)
{
  _dist_parameters["location"] = location;
  _dist_parameters["scale"] = scale;

  _backend = new LaplaceDistributionBackend(location, scale);
}

BasicLaplaceDistribution::~BasicLaplaceDistribution()
{
  delete _backend;
}

/*
 * CLASS TRIANGULAR DISTRIBUTION
 */

class TriangularDistributionBackend : public DistributionBackendTemplate<boost::math::triangular> {
public:
  TriangularDistributionBackend(double lower, double mode, double upper) {
    _backend = new boost::math::triangular(lower, mode, upper);
  }
  ~TriangularDistributionBackend() {
    delete _backend;
  }
};

BasicTriangularDistribution::BasicTriangularDistribution(double x_peak, double lower_bound, double upper_bound)
{
  _dist_parameters["xPeak"] = x_peak;
  _dist_parameters["lowerBound"] = lower_bound;
  _dist_parameters["upperBound"] = upper_bound;

  if(not hasParameter("truncation")) {
    _dist_parameters["truncation"] = 1.0;
  }
  if(not hasParameter("xMin")) {
    _dist_parameters["xMin"] = lower_bound;
  }
  if(not hasParameter("xMax")) {
    _dist_parameters["xMax"] = upper_bound;
  }

  if (upper_bound < lower_bound)
    throwError("ERROR: bounds for triangular distribution are incorrect");
  if (upper_bound < _dist_parameters.find("xMin") ->second)
    throwError("ERROR: bounds and LB/UB are inconsistent for triangular distribution");
  if (lower_bound > _dist_parameters.find("xMax") ->second)
    throwError("ERROR: bounds and LB/UB are inconsistent for triangular distribution");
  _backend = new TriangularDistributionBackend(lower_bound, x_peak, upper_bound);

}
BasicTriangularDistribution::~BasicTriangularDistribution()
{
  delete _backend;
}

/*
 * CLASS EXPONENTIAL DISTRIBUTION
 */

class ExponentialDistributionBackend : public DistributionBackendTemplate<boost::math::exponential> {
public:
  ExponentialDistributionBackend(double lambda) {
    _backend = new boost::math::exponential(lambda);
  }
  ~ExponentialDistributionBackend() {
    delete _backend;
  }
};

BasicExponentialDistribution::BasicExponentialDistribution(double lambda, double low)
{
  _dist_parameters["lambda"] = lambda;
  _dist_parameters["low"] = low;

  if(not hasParameter("truncation")) {
    _dist_parameters["truncation"] = 1.0;
  }
  if(not hasParameter("xMin")) {
    _dist_parameters["xMin"] = low;
  }
  if(not hasParameter("xMax")) {
    _dist_parameters["xMax"] = std::numeric_limits<double>::max( );
  }

  if (lambda<0)
    throwError("ERROR: incorrect value of lambda for exponential distribution");

  _backend = new ExponentialDistributionBackend(lambda);
}

BasicExponentialDistribution::BasicExponentialDistribution(double lambda, double x_min, double x_max, double low):
  BasicTruncatedDistribution(x_min,x_max)
{
    _dist_parameters["lambda"] = lambda;
    _dist_parameters["low"] = low;
    if (lambda<0)
    throwError("ERROR: incorrect value of lambda for exponential distribution");
    _backend = new ExponentialDistributionBackend(lambda);
}

BasicExponentialDistribution::~BasicExponentialDistribution()
{
  delete _backend;
}

double
BasicExponentialDistribution::untrPdf(double x){
  double low = _dist_parameters.find("low") ->second;
  if(x >= low) {
    return _backend->pdf(x-low);
  } else {
    return 0.0;
  }
}

double
BasicExponentialDistribution::untrCdf(double x){
  double low = _dist_parameters.find("low") ->second;
  if(x >= low) {
    return _backend->cdf(x-low);
  } else {
    return 0.0;
  }
}

double
BasicExponentialDistribution::cdf(double x){
  return BasicTruncatedDistribution::cdf(x);
  //double low = _dist_parameters.find("low") ->second;
}

double
BasicExponentialDistribution::inverseCdf(double x){
  double low = _dist_parameters.find("low") ->second;
  return BasicTruncatedDistribution::inverseCdf(x) + low;
}

/*
 * CLASS WEIBULL DISTRIBUTION
 */

class WeibullDistributionBackend : public DistributionBackendTemplate< boost::math::weibull>  {
public:
  WeibullDistributionBackend(double shape, double scale) {
    _backend = new boost::math::weibull(shape, scale);
  }
  ~WeibullDistributionBackend() {
    delete _backend;
  }
};

BasicWeibullDistribution::BasicWeibullDistribution(double k, double lambda, double low)
{
  _dist_parameters["k"] = k; //shape
  _dist_parameters["lambda"] = lambda; //scale
  _dist_parameters["low"] = low; //scale

  if(not hasParameter("truncation")) {
    _dist_parameters["truncation"] = 1.0;
  }
  if(not hasParameter("xMin")) {
    _dist_parameters["xMin"] = low;
  }
  if(not hasParameter("xMax")) {
    _dist_parameters["xMax"] = std::numeric_limits<double>::max( );
  }

  if ((lambda<0) || (k<0))
    throwError("ERROR: incorrect value of k or lambda for weibull distribution");

  _backend = new WeibullDistributionBackend(k, lambda);
}

BasicWeibullDistribution::BasicWeibullDistribution(double k, double lambda, double x_min, double x_max, double low):
  BasicTruncatedDistribution(x_min,x_max)
{
    _dist_parameters["k"] = k; //shape
    _dist_parameters["lambda"] = lambda; //scale
    _dist_parameters["low"] = low; //scale

    if ((lambda<0) || (k<0))
    throwError("ERROR: incorrect value of k or lambda for weibull distribution");
    _backend = new WeibullDistributionBackend(k, lambda);
}

BasicWeibullDistribution::~BasicWeibullDistribution()
{
  delete _backend;
}

double
BasicWeibullDistribution::untrPdf(double x){
  double low = _dist_parameters.find("low") ->second;
  if(x >= low) {
    return _backend->pdf(x-low);
  } else {
    return 0.0;
  }
}

double
BasicWeibullDistribution::untrCdf(double x){
  double low = _dist_parameters.find("low") ->second;
  if(x >= low) {
    return _backend->cdf(x-low);
  } else {
    return 0.0;
  }
}

double
BasicWeibullDistribution::inverseCdf(double x){
  double low = _dist_parameters.find("low") ->second;
  return BasicTruncatedDistribution::inverseCdf(x) + low;
}

/*
 * CLASS GAMMA DISTRIBUTION
 */

class GammaDistributionBackend : public DistributionBackendTemplate<boost::math::gamma_distribution<> > {
public:
  GammaDistributionBackend(double shape, double scale) {
    _backend = new boost::math::gamma_distribution<>(shape, scale);
  }
  ~GammaDistributionBackend() {
    delete _backend;
  }
};

BasicGammaDistribution::BasicGammaDistribution(double k, double theta, double low)
{
  _dist_parameters["k"] = k; //shape
  _dist_parameters["theta"] = theta; //scale
  _dist_parameters["low"] = low; //low value shift. 0.0 would be a regular gamma
  // distribution

  if(not hasParameter("truncation")) {
    _dist_parameters["truncation"] = 1.0;
  }
  if(not hasParameter("xMin")) {
    _dist_parameters["xMin"] = low;
  }
  if(not hasParameter("xMax")) {
    _dist_parameters["xMax"] = std::numeric_limits<double>::max( );
  }

  if ((theta<0) || (k<0))
    throwError("ERROR: incorrect value of k or theta for gamma distribution");

  _backend = new GammaDistributionBackend(k, theta);
}

BasicGammaDistribution::BasicGammaDistribution(double k, double theta, double low, double x_min, double x_max):
  BasicTruncatedDistribution(x_min,x_max)
{
    _dist_parameters["k"] = k; //shape
    _dist_parameters["theta"] = theta; //scale
    _dist_parameters["low"] = low; //low value shift. 0.0 would be a regular gamma
    // distribution

    if ((theta<0) || (k<0))
    throwError("ERROR: incorrect value of k or theta for gamma distribution");

    _backend = new GammaDistributionBackend(k, theta);
}

BasicGammaDistribution::~BasicGammaDistribution()
{
  delete _backend;
}

double
BasicGammaDistribution::untrCdf(double x){
  double low = _dist_parameters.find("low") ->second;
  if(x > 1.0e100) {
    return 1.0;
  } else if(x >= low) {
    return _backend->cdf(x - low);
  } else  {
    return 0.0;
  }
}

double
BasicGammaDistribution::untrPdf(double x){
  double low = _dist_parameters.find("low") ->second;
  return BasicTruncatedDistribution::untrPdf(x - low);
}

double
BasicGammaDistribution::untrInverseCdf(double x){
  double low = _dist_parameters.find("low") ->second;
  return BasicTruncatedDistribution::untrInverseCdf(x) + low;
}

/*
 * CLASS BETA DISTRIBUTION
 */

class BetaDistributionBackend : public DistributionBackendTemplate<boost::math::beta_distribution<> > {
public:
  BetaDistributionBackend(double alpha, double beta) {
    _backend = new boost::math::beta_distribution<>(alpha, beta);
  }
  ~BetaDistributionBackend() {
    delete _backend;
  }
};

BasicBetaDistribution::BasicBetaDistribution(double alpha, double beta, double scale, double low)
{
  _dist_parameters["alpha"] = alpha;
  _dist_parameters["beta" ] = beta;
  _dist_parameters["scale"] = scale;
  _dist_parameters["low"  ] = low;

  if(not hasParameter("truncation")) {
    _dist_parameters["truncation"] = 1.0;
  }
  if(not hasParameter("xMin")) {
    _dist_parameters["xMin"] = low;
  }
  if(not hasParameter("xMax")) {
    _dist_parameters["xMax"] = low+scale;
  }

  if ((alpha<0) || (beta<0))
    throwError("ERROR: incorrect value of alpha or beta for beta distribution");

  _backend = new BetaDistributionBackend(alpha, beta);
}

BasicBetaDistribution::BasicBetaDistribution(double alpha, double beta, double scale, double x_min, double x_max, double low):
  BasicTruncatedDistribution(x_min,x_max)
{
    _dist_parameters["alpha"] = alpha;
    _dist_parameters["beta" ] = beta;
    _dist_parameters["scale"] = scale;
    _dist_parameters["low"  ] = low;

    if ((alpha<0) || (beta<0))
    throwError("ERROR: incorrect value of alpha or beta for beta distribution");

    _backend = new BetaDistributionBackend(alpha, beta);
}

BasicBetaDistribution::~BasicBetaDistribution()
{
  delete _backend;
}

double
BasicBetaDistribution::untrPdf(double x){
  double scale = _dist_parameters.find("scale") ->second;
  double low   = _dist_parameters.find("low"  ) ->second;
  return _backend->pdf( (x-low)/scale);
}

double
BasicBetaDistribution::untrCdf(double x){
  double scale = _dist_parameters.find("scale") ->second;
  double low   = _dist_parameters.find("low"  ) ->second;
  if(x >= low and x <= low+scale) {
    return _backend->cdf( (x-low)/scale );
  } else if(x < low){
    return 0.0;
  } else {
    return 1.0;
  }
}

double
BasicBetaDistribution::pdf(double x){
  double scale   = _dist_parameters.find("scale"  ) ->second;
  return BasicTruncatedDistribution::pdf( x )/scale;// scaling happens in untrPdf
}

double
BasicBetaDistribution::cdf(double x){
  //double scale = _dist_parameters.find("scale") ->second;
  //double low   = _dist_parameters.find("low"  ) ->second;
  return BasicTruncatedDistribution::cdf( x );// -low)/scale ); scaling happens in untrCdf
}

double
BasicBetaDistribution::inverseCdf(double x){
  double scale = _dist_parameters.find("scale") ->second;
  double low   = _dist_parameters.find("low"  ) ->second;
  return BasicTruncatedDistribution::inverseCdf( x )*scale+low;
}

/*
 * CLASS POISSON DISTRIBUTION
 */

class PoissonDistributionBackend : public DistributionBackendTemplate<boost::math::poisson_distribution<> > {
public:
  PoissonDistributionBackend(double mu) {
    _backend = new boost::math::poisson_distribution<>(mu);
  }
  ~PoissonDistributionBackend() {
    delete _backend;
  }
};

BasicPoissonDistribution::BasicPoissonDistribution(double mu)
{
  _dist_parameters["mu"] = mu;

  if(not hasParameter("truncation")) {
    _dist_parameters["truncation"] = 1.0;
  }
  if(not hasParameter("xMin")) {
    _dist_parameters["xMin"] = -std::numeric_limits<double>::max( );
  }
  if(not hasParameter("xMax")) {
    _dist_parameters["xMax"] = std::numeric_limits<double>::max( );
  }

  if (mu<0)
    throwError("ERROR: incorrect value of mu for poisson distribution");

  _backend = new PoissonDistributionBackend(mu);
}

BasicPoissonDistribution::~BasicPoissonDistribution()
{
  delete _backend;
}

double
BasicPoissonDistribution::untrCdf(double x){
  if(x >= 0) {
    return _backend->cdf(x);
  } else {
    return 0.0;
  }
}

double
BasicPoissonDistribution::pdf(double x){
   double x_min = _dist_parameters.find("xMin") ->second;
   double x_max = _dist_parameters.find("xMax") ->second;

   double value;

   if (_dist_parameters.find("truncation") ->second == 1)
          if (x<x_min)
                  value=0;
          else if (x>x_max)
                  value=0;
          else
                  value = 1/(untrCdf(x_max) - untrCdf(x_min)) * untrPdf(x);
   else
      value=-1;

   return value;
}

double
BasicPoissonDistribution::cdf(double x){
   double x_min = _dist_parameters.find("xMin") ->second;
   double x_max = _dist_parameters.find("xMax") ->second;

   double value;

   if (_dist_parameters.find("truncation") ->second == 1)
          if (x<x_min)
                  value=0;
          else if (x>x_max)
                  value=1;
          else
                  value = 1/(untrCdf(x_max) - untrCdf(x_min)) * (untrCdf(x) - untrCdf(x_min));
   else
      value=-1;

   return value;
}

double
BasicPoissonDistribution::inverseCdf(double x){
   double value;
   double x_min = _dist_parameters.find("xMin") ->second;
   double x_max = _dist_parameters.find("xMax") ->second;

   if(x == 1.0) {
     return x_max;
   }
   if (_dist_parameters.find("truncation") ->second == 1){
     double temp = untrCdf(x_min) + x * (untrCdf(x_max)-untrCdf(x_min));
     value=untrInverseCdf(temp);
   } else {
      value=-1;
   }
   return value;
}

/*
 * CLASS BINOMIAL DISTRIBUTION
 */

class BinomialDistributionBackend : public DistributionBackendTemplate<boost::math::binomial_distribution<> > {
public:
  BinomialDistributionBackend(double n, double p) {
    _backend = new boost::math::binomial_distribution<>(n, p);
  }
  ~BinomialDistributionBackend() {
    delete _backend;
  }
};

BasicBinomialDistribution::BasicBinomialDistribution(double n, double p)
{
  _dist_parameters["n"] = n;
  _dist_parameters["p"] = p;

  if (n<0 or p<0)
    throwError("ERROR: incorrect value of n or p for binomial distribution");

  _backend = new BinomialDistributionBackend(n, p);
}

BasicBinomialDistribution::~BasicBinomialDistribution()
{
  delete _backend;
}

double
BasicBinomialDistribution::untrCdf(double x){
  if(x >= 0) {
    return _backend->cdf(x);
  } else {
    return 0.0;
  }
}

/*
 * CLASS BERNOULLI DISTRIBUTION
 */

class BernoulliDistributionBackend : public DistributionBackendTemplate<boost::math::bernoulli_distribution<> > {
public:
  BernoulliDistributionBackend(double p) {
    _backend = new boost::math::bernoulli_distribution<>(p);
  }
  ~BernoulliDistributionBackend() {
    delete _backend;
  }
};

BasicBernoulliDistribution::BasicBernoulliDistribution(double p)
{
  _dist_parameters["p"] = p;

  if (p<0)
    throwError("ERROR: incorrect value of p for bernoulli distribution");

  _backend = new BernoulliDistributionBackend(p);
}

BasicBernoulliDistribution::~BasicBernoulliDistribution()
{
  delete _backend;
}

/*
 * CLASS GEOMETRIC DISTRIBUTION
 */

class GeometricDistributionBackend : public DistributionBackendTemplate<boost::math::geometric_distribution<> > {
public:
  GeometricDistributionBackend(double p) {
    _backend = new boost::math::geometric_distribution<>(p);
  }
  ~GeometricDistributionBackend() {
    delete _backend;
  }
};

BasicGeometricDistribution::BasicGeometricDistribution(double p)
{
  _dist_parameters["p"] = p;

  if (p<0)
    throwError("ERROR: incorrect value of p for geometric distribution");

  _backend = new GeometricDistributionBackend(p);
}

BasicGeometricDistribution::~BasicGeometricDistribution()
{
  delete _backend;
}

/*
 * CLASS CONSTANT DISTRIBUTION
 */

BasicConstantDistribution::BasicConstantDistribution(double value){
  _value = value;
}
BasicConstantDistribution::~BasicConstantDistribution(){}
double  BasicConstantDistribution::pdf(double x){
  return untrPdf(x);
}
double  BasicConstantDistribution::cdf(double x){
  return untrCdf(x);
}
double  BasicConstantDistribution::inverseCdf(double x){
  return untrInverseCdf(x);
}

double BasicConstantDistribution::untrPdf(double x){
  if(x == _value){
    return std::numeric_limits<double>::max( )/2.0;
  } else {
    return 0.0;
  }
}

double BasicConstantDistribution::untrCdf(double x){
  if(x < _value) {
    return 0.0;
  } else if(x > _value) {
    return 1.0;
  } else {
    return 0.5;
  }
}

double BasicConstantDistribution::untrCdfComplement(double){
  throwError("Not Implemented");
  return _value;
}

double BasicConstantDistribution::untrInverseCdf(double){
  return _value;
}

double BasicConstantDistribution::untrMean(){
  return _value;
}

/**
   Calculates the untruncated standard deviation
   \return the standard deviation
*/
double BasicConstantDistribution::untrStdDev(){
  return 0.0;
}

double BasicConstantDistribution::untrMedian(){
  return _value;
}

double BasicConstantDistribution::untrMode(){
  return _value;
}

double BasicConstantDistribution::untrHazard(double /*x*/){
  throwError("Not implemented");
  return _value;
}

/*
 * CLASS CUSTOM DISTRIBUTION
 */

// BasicCustomDistribution::BasicCustomDistribution(double x_coordinates, double y_coordinates, int fitting_type, double n_points)
// {
//    _dist_parameters["x_coordinates"] = x_coordinates;
//    _dist_parameters["y_coordinates"] = y_coordinates;
//    _dist_parameters["fitting_type"] = fitting_type;
//    _dist_parameters["n_points"] = n_points;

// }

// BasicCustomDistribution::~BasicCustomDistribution()
// {
// }

// double
// BasicCustomDistribution::pdf(double & x){
//    double value=_interpolation.interpolation(x);

//    return value;
// }

// double
// BasicCustomDistribution::cdf(double & ){
//   //XXX implement
//    double value=-1;

//    return value;
// }

// double
// BasicCustomDistribution::inverseCdf(double & ){
//   //XXX implement
//    double value=-1;
//    return value;
// }

//
//   // Beta pdf
//      double distribution_1D::betaPdf (double x){
//         // parameter1=alpha   >0
//         // parameter2=beta    >0
//         // 0<x<1
//
//         double value;
//
//         /*if ((x > 0)&&(x < 1)&&(_parameter1 > 0)&&(_parameter2 > 0))
//              value = 1/betaFunc(_parameter1,_parameter2)*pow(x,_parameter1-1)*pow(1-x,_parameter2-1);
//              else */
//            value=-1;
//
//         return value;
//      }
//
//      double distribution_1D::betaCdf (double x){
//         // parameter1=alpha   >0
//         // parameter2=beta    >0
//         // 0<x<1
//
//         double value;
//
//         /*if ((x > 0)&&(x < 1)&&(_parameter1 > 0)&&(_parameter2 > 0))
//            value = betaInc(_parameter1,_parameter2 ,x);
//                else */
//            value=-1;
//
//         return value;
//      }

//
//   // Gamma pdf
//      double distribution_1D::gammaPdf(double x){
//         // parameter1= k   >0
//         // parameter2= theta     >0
//         // x>=0
//
//         double value;
//
//         /* if ((x >= 0)&&(_parameter1 > 0)&&(_parameter2 > 0))
//            value=1/gammaFunc(_parameter1)/pow(_parameter2,_parameter1)*pow(x,_parameter1-1)*exp(-x/_parameter2);
//                else */
//            value=1;
//
//         return value;
//      }
//
//      double distribution_1D::gammaCdf(double x){
//         // parameter1=alpha, k   >0
//         // parameter2=beta, theta     >0
//         // x>=0
//
//         double value;
//
//         /* if ((x >= 0)&&(_parameter1 > 0)&&(_parameter2 > 0))
//              value= gammp(_parameter1,x/_parameter2);
//              else */
//            value=1;
//
//         return value;
//      }

//      double distribution_1D::gammaRandNumberGenerator(){
//          double value=-1;//gammaRNG(_parameter1,_parameter2);
//         return value;
//      }
//
//      double distribution_1D::betaRandNumberGenerator(){
//          double value=-1;//betaRNG(_parameter1,_parameter2);
//         return value;
//      }
//
//      double distribution_1D::triangularRandNumberGenerator(){
//         double value;
//         double RNG = rand()/double(RAND_MAX);
//         double referenceValue=(_parameter1-_x_min)/(_x_max-_x_min);
//
//         if (RNG<referenceValue)
//            value= _x_min+sqrt(RNG*(_parameter1-_x_min)*(_x_max-_x_min));
//         else
//            value=_x_max-sqrt((1-RNG)*(_x_max-_parameter1)*(_x_max-_x_min));
//         return value;
//      }


#include <gtest/gtest.h>
#include <EventID.h>
#include <BaseEvent.h>
#include <ID.h>
#include <IO.h>
#include <FormatID.h>

#include <vector>
#include <tuple>
#include <boost/mpl/list.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/remove.hpp>
#include <boost/mpl/for_each.hpp>

namespace tests {

namespace eventTestSuite {

using namespace ::id::attribute;
using namespace ::boost::mpl;

struct ScaleTestSuite : public ::testing::Test{
  struct createEvent  {
    template<typename Scale>
    struct apply {
      struct EventConfig : public BaseConfig {
        using PositionScale = Scale;
      };
      using type = std::tuple<Position, Scale, BaseEvent<EventConfig>>;
    };
  };

  struct createExtendedEvent  {
    template<typename Scale>
    struct apply {
      using ValueType = Value<float, 1>;
      using Attr = Attribute<Angle, ValueType, Radian, Scale>;
      using ExtE = typename BaseEvent<>::append<Attr>::type;
      using type = std::tuple<Angle, Scale, ExtE>;
    };
  };

  struct FormatTester {
    template<typename Tuple>
    void operator()(Tuple t){
      using A = typename std::tuple_element<0, Tuple>::type;
      using S = typename std::tuple_element<1, Tuple>::type;
      using E = typename std::tuple_element<2, Tuple>::type;
      EventType type  = (EventType)E();
      EventType comp = type;
      comp.attribute(A::value())->scale() = (ScaleType)S();
      EXPECT_EQ(type, comp) << "Scale Types are not equivalent " << type.attribute(A::value())->scale()
                            << " <-> " << comp.attribute(A::value())->scale() ;
      EXPECT_EQ(FormatID(type), FormatID(comp)) << "FormatIDs are not eqivalent";
    }
  };
  template<typename T0>
  struct FormatInEqHelper {
    template<typename T1>
    void operator()(T1 t){
      using E0 = typename std::tuple_element<2, T0>::type;
      using E1 = typename std::tuple_element<2, T1>::type;
      EventType t0  = (EventType)E0();
      EventType t1  = (EventType)E1();
      EXPECT_NE(t0, t1) << "Scale Types are equivalent " << t0 << " <-> " << t1;
      EXPECT_NE(FormatID(t0), FormatID(t1)) << "FormatIDs are eqivalent";
    }
  };
  template<typename List>
  struct FormatInEqTester {
    template<typename Tuple>
    void operator()(Tuple t){
      using CleanList = typename boost::mpl::remove<List, Tuple>::type;
      for_each<CleanList>(FormatInEqHelper<Tuple>());
    }
  };

  using ScaleList = list< Scale<>,
                          Scale<std::milli>,
                          Scale<std::ratio<1>, 1>,
                          Scale<std::milli, 1>
                    >::type;
  using BasicEventList = transform<ScaleList, createEvent>::type;
  using ExtendedEventList = transform<ScaleList, createExtendedEvent>::type;
};

TEST_F(ScaleTestSuite, equalityTest) {
  for_each< BasicEventList >(FormatTester());
  for_each< ExtendedEventList >(ScaleTestSuite::FormatTester());
}
TEST_F(ScaleTestSuite, inEqualityTest) {
  for_each< BasicEventList >(FormatInEqTester<BasicEventList>());
  for_each< ExtendedEventList >(FormatInEqTester<ExtendedEventList>());
}

}}


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

namespace boost { namespace simd { namespace tag
  {
   /*!
     @brief fast_frexp generic tag

     Represents the fast_frexp function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct fast_frexp_ : ext::elementwise_<fast_frexp_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<fast_frexp_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_fast_frexp_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::fast_frexp_, Site> dispatching_fast_frexp_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::fast_frexp_, Site>();
   }
   template<class... Args>
   struct impl_fast_frexp_;
  }

  /*!
    Returns the mantissa and exponent of the input

    @par Semantic:

    @code
    tie(m, e) = fast_frexp(x);
    @endcode

    is similar to:

    @code
    as_integer<T > e = exponent(x)+1;
    T m = mantissa(x)/2;
    @endcode

    @par Note:
    The fast prefix indicates that, for speed consideration, the result may be
    incorrect for special values like inf, -inf nan and zero that deserve
    special treatment. If you are not sure use @funcref{frexp} at the expense of some
    more machine cycles.

    @see  @funcref{frexp}
    @param a0 Value to decompose

    @return A pair containing the signed mantissa and exponent of @c a0
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::fast_frexp_, fast_frexp, 1)

  /*!
    Returns the mantissa computes the  exponent of the input

    @code
    m = fast_frexp(x, e);
    @endcode

    is similar to:

    @code
    as_integer<T > e = exponent(x)+1;
    T m = mantissa(x)/2;
    @endcode

    @par Note:

    @par Note:
    The fast prefix indicates that for speed sake
    the result may be incorrect for limiting values:
    inf, -inf nan and zero that deserve special treatment.
    If you are not sure use @c frexp at the expense
    of some more machine cycles.

    @param a0 Value to decompose
    @param a1 L-Value that will receive the exponent of @c a0
    @return The mantissa of @c a0
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL( tag::fast_frexp_
                                            , fast_frexp,(A0 const&)(A1&)
                                            , 2
                                            )

  /*!
    Computes the mantissa and the  exponent of the input

    @code
    m = fast_frexp(x, e);
    @endcode

    is similar to:

    @code
    as_integer<T > e = exponent(x)+1;
    T m = mantissa(x)/2;
    @endcode

    @par Note:

    @par Note:
    The fast prefix indicates that for speed sake
    the result may be incorrect for limiting values:
    inf, -inf nan and zero that deserve special treatment.
    If you are not sure use @c frexp at the expense
    of some more machine cycles.

    @param a0 Value to decompose
    @param a1 L-Value that will receive the mantissa of @c a0
    @param a2 L-Value that will receive the exponent of @c a0
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL( tag::fast_frexp_
                                            , fast_frexp,(A0 const&)(A0&)(A1&)
                                            , 2
                                            )
} }

#endif