#include "BlocksEngine/pch.h"
#include "BlocksEngine/Core/Math/Vector3.h"

#include <boost/container_hash/hash.hpp>

using namespace BlocksEngine;
using namespace DirectX;

//------------------------------------------------------------------------------
// Constructors
//------------------------------------------------------------------------------

template <class T>
Vector3<T>::Vector3() noexcept
    : Vector3Base<T>::Base{0, 0, 0}
{
}

template <class T>
constexpr Vector3<T>::Vector3(const T a) noexcept
    : Vector3Base<T>::Base{a, a, a}
{
}

/*template <class T>
constexpr Vector3<T>::Vector3(const T x, const T y, const T z) noexcept
    : Vector3Base<T>::Base{x, y, z}
{
}*/

template <typename T>
Vector3<T>::Vector3(const physx::PxVec3& vec3) noexcept
    : Vector3Base<T>::Base{
        static_cast<float>(vec3.x),
        static_cast<float>(vec3.y),
        static_cast<float>(vec3.z)
    }
{
}

template <typename T>
Vector3<T>::Vector3(const physx::PxExtendedVec3& vec3) noexcept
    : Vector3Base<T>::Base{
        static_cast<float>(vec3.x),
        static_cast<float>(vec3.y),
        static_cast<float>(vec3.z)
    }
{
}

template <class T>
Vector3<T>::Vector3(_In_reads_(3) const T* pArray) noexcept
    : Vector3Base<T>::Base{pArray}
{
}

template <class T>
Vector3<T>::Vector3(FXMVECTOR v) noexcept // NOLINT(cppcoreguidelines-pro-type-member-init)
{
    Store(this, v);
}

template <class T>
Vector3<T>::Vector3(const typename Vector3Base<T>::Base& v) noexcept
    : Vector3Base<T>::Base{v.x, v.y, v.z}
{
}

template <>
Vector3<float>::Vector3(const Vector3Base<float>::Vector& v) noexcept
    : Vector3Base<float>::Base{v.f[0], v.f[1], v.f[2]}
{
}

template <>
Vector3<int32_t>::Vector3(const Vector3Base<int32_t>::Vector& v) noexcept
    : Vector3Base<int32_t>::Base{v.i[0], v.i[1], v.i[2]}
{
}

template <>
Vector3<uint32_t>::Vector3(const Vector3Base<uint32_t>::Vector& v) noexcept
    : Vector3Base<uint32_t>::Base{v.u[0], v.u[1], v.u[2]}
{
}

//------------------------------------------------------------------------------
// Converter operators
//------------------------------------------------------------------------------

template <class T>
Vector3<T>::operator XMVECTOR() const noexcept
{
    return Load(this);
}

//------------------------------------------------------------------------------
// Comparision operators
//------------------------------------------------------------------------------

template <class T>
bool Vector3<T>::operator==(const Vector3& v) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    return XMVector3Equal(v1, v2);
}

template <class T>
bool Vector3<T>::operator!=(const Vector3& v) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    return XMVector3NotEqual(v1, v2);
}

//------------------------------------------------------------------------------
// Assignment operators
//------------------------------------------------------------------------------

template <>
Vector3<float>& Vector3<float>::operator=(const Vector3Base<float>::Vector& v) noexcept
{
    this->x = v.f[0];
    this->y = v.f[1];
    this->z = v.f[2];

    return *this;
}

template <>
Vector3<int32_t>& Vector3<int32_t>::operator=(const Vector3Base<int32_t>::Vector& v) noexcept
{
    this->x = v.i[0];
    this->y = v.i[1];
    this->z = v.i[2];

    return *this;
}

template <>
Vector3<uint32_t>& Vector3<uint32_t>::operator=(const Vector3Base<uint32_t>::Vector& v) noexcept
{
    this->x = v.u[0];
    this->y = v.u[1];
    this->z = v.u[2];

    return *this;
}

template <class T>
Vector3<T>& Vector3<T>::operator/=(const float s) noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVectorScale(v1, 1.0f / s);
    Store(this, x);

    return *this;
}

//------------------------------------------------------------------------------
// Unary operators
//------------------------------------------------------------------------------

template <class T>
Vector3<T> Vector3<T>::operator+() const noexcept
{
    return *this;
}

template <class T>
Vector3<T> Vector3<T>::operator-() const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVectorNegate(v1);

    Vector3<T> r;
    Store(&r, x);
    return r;
}

//------------------------------------------------------------------------------
// Hashing
//------------------------------------------------------------------------------

template <class T>
std::size_t hash_value(const Vector3<T>& v)
{
    std::size_t seed = 0;
    boost::hash_combine(seed, v.x);
    boost::hash_combine(seed, v.y);
    boost::hash_combine(seed, v.z);

    return seed;
}

//------------------------------------------------------------------------------
// Vector operations
//------------------------------------------------------------------------------

template <class T>
template <class U>
bool Vector3<T>::InBounds(const Vector3<U>& bounds) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&bounds);
    return XMVector3InBounds(v1, v2);
}

template <class T>
float Vector3<T>::Length() const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVector3Length(v1);
    return XMVectorGetX(x);
}

template <class T>
float Vector3<T>::LengthSquared() const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVector3LengthSq(v1);
    return XMVectorGetX(x);
}

template <class T>
template <class U>
float Vector3<T>::Dot(const Vector3<U>& v) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR x = XMVector3Dot(v1, v2);
    return XMVectorGetX(x);
}

template <class T>
template <class U>
void Vector3<T>::Cross(const Vector3<U>& v, Vector3<T>& result) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR x = XMVector3Cross(v1, v2);
    Store(&result, x);
}

template <class T>
template <class U>
Vector3<T> Vector3<T>::Cross(const Vector3<U>& v) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR v2 = Load(&v);
    const XMVECTOR x = XMVector3Cross(v1, v2);

    Vector3<T> result;
    Store(&result, x);
    return result;
}

template <class T>
void Vector3<T>::Normalize() noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVector3Normalize(v1);
    Store(this, x);
}

template <class T>
template <class U>
void Vector3<T>::Normalize(Vector3<U>& result) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVector3Normalize(v1);
    Store(&result, x);
}

//------------------------------------------------------------------------------
// Static functions
//------------------------------------------------------------------------------

template <typename T>
void Vector3<T>::Abs() noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVectorAbs(v1);
    Store(this, x);
}

template <typename T>
void Vector3<T>::Abs(Vector3<T>& result) const noexcept
{
    const XMVECTOR v1 = Load(this);
    const XMVECTOR x = XMVectorAbs(v1);
    Store(&result, x);
}

template <class T>
template <class U, class V>
float Vector3<T>::Distance(const Vector3<U>& v1, const Vector3<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR v = XMVectorSubtract(x2, x1);
    const XMVECTOR x = XMVector3Length(v);

    return XMVectorGetX(x);
}

template <class T>
template <class U, class V>
float Vector3<T>::DistanceSquared(const Vector3<U>& v1, const Vector3<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR v = XMVectorSubtract(x2, x1);
    const XMVECTOR x = XMVector3LengthSq(v);
    return XMVectorGetX(x);
}

template <class T>
template <class U, class V>
void Vector3<T>::Min(const Vector3<U>& v1, const Vector3<V>& v2, Vector3<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMin(x1, x2);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector3<T> Vector3<T>::Min(const Vector3<U>& v1, const Vector3<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMin(x2, x1);

    Vector3<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector3<T>::Max(const Vector3<U>& v1, const Vector3<V>& v2, Vector3<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMax(x2, x1);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector3<T> Vector3<T>::Max(const Vector3<U>& v1, const Vector3<V>& v2) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorMax(x2, x1);

    Vector3<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector3<T>::Lerp(const Vector3<U>& v1, const Vector3<V>& v2, const float t, Vector3<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorLerp(x2, x1, t);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector3<T> Vector3<T>::Lerp(const Vector3<U>& v1, const Vector3<V>& v2, const float t) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorLerp(x2, x1, t);

    Vector3<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector3<T>::SmoothStep(const Vector3<U>& v1, const Vector3<V>& v2, float t, Vector3<T>& result) noexcept
{
    t = t > 1.0f ? 1.0f : t < 0.0f ? 0.0f : t; // Clamp value between 0 and 1
    t = t * t * (3.0f - 2.0f * t);
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorLerp(x1, x2, t);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector3<T> Vector3<T>::SmoothStep(const Vector3<U>& v1, const Vector3<V>& v2, float t) noexcept
{
    t = t > 1.0f ? 1.0f : t < 0.0f ? 0.0f : t; // Clamp value between 0 and 1
    t = t * t * (3.0f - 2.0f * t);
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x = XMVectorLerp(x1, x2, t);

    Vector3<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V, class W>
void Vector3<T>::Barycentric(const Vector3<U>& v1, const Vector3<V>& v2, const Vector3<W>& v3, const float f,
                             const float g, Vector3<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x3 = Load(&v3);
    const XMVECTOR x = XMVectorBaryCentric(x1, x2, x3, f, g);

    Store(&result, x);
}

template <class T>
template <class U, class V, class W>
Vector3<T> Vector3<T>::Barycentric(const Vector3<U>& v1, const Vector3<V>& v2, const Vector3<W>& v3,
                                   const float f, const float g) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x3 = Load(&v3);
    const XMVECTOR x = XMVectorBaryCentric(x1, x2, x3, f, g);

    Vector3<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V, class W, class X>
void Vector3<T>::CatmullRom(const Vector3<U>& v1, const Vector3<V>& v2, const Vector3<W>& v3, const Vector3<X>& v4,
                            const float t, Vector3<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x3 = Load(&v3);
    const XMVECTOR x4 = Load(&v4);
    const XMVECTOR x = XMVectorCatmullRom(x1, x2, x3, x4, t);

    Store(&result, x);
}

template <class T>
template <class U, class V, class W, class X>
Vector3<T> Vector3<T>::CatmullRom(const Vector3<U>& v1, const Vector3<V>& v2, const Vector3<W>& v3,
                                  const Vector3<X>& v4, const float t) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&v2);
    const XMVECTOR x3 = Load(&v3);
    const XMVECTOR x4 = Load(&v4);
    const XMVECTOR x = XMVectorCatmullRom(x1, x2, x3, x4, t);

    Vector3<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V, class W, class X>
void Vector3<T>::Hermite(const Vector3<U>& v1, const Vector3<V>& t1, const Vector3<W>& v2, const Vector3<X>& t2,
                         const float t, Vector3<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&t1);
    const XMVECTOR x3 = Load(&v2);
    const XMVECTOR x4 = Load(&t2);
    const XMVECTOR x = XMVectorHermite(x1, x2, x3, x4, t);

    Store(&result, x);
}

template <class T>
template <class U, class V, class W, class X>
Vector3<T> Vector3<T>::Hermite(const Vector3<U>& v1, const Vector3<V>& t1, const Vector3<W>& v2,
                               const Vector3<X>& t2, const float t) noexcept
{
    const XMVECTOR x1 = Load(&v1);
    const XMVECTOR x2 = Load(&t1);
    const XMVECTOR x3 = Load(&v2);
    const XMVECTOR x4 = Load(&t2);
    const XMVECTOR x = XMVectorHermite(x1, x2, x3, x4, t);

    Vector3<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector3<T>::Reflect(const Vector3<U>& iVec, const Vector3<V>& nVec, Vector3<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&iVec);
    const XMVECTOR x2 = Load(&nVec);
    const XMVECTOR x = XMVector3Reflect(x1, x2);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector3<T> Vector3<T>::Reflect(const Vector3<U>& iVec, const Vector3<V>& nVec) noexcept
{
    const XMVECTOR x1 = Load(&iVec);
    const XMVECTOR x2 = Load(&nVec);
    const XMVECTOR x = XMVector3Reflect(x1, x2);

    Vector3<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U, class V>
void Vector3<T>::Refract(const Vector3<U>& iVec, const Vector3<V>& nVec, const float refractionIndex,
                         Vector3<T>& result) noexcept
{
    const XMVECTOR x1 = Load(&iVec);
    const XMVECTOR x2 = Load(&nVec);
    const XMVECTOR x = XMVector3Refract(x1, x2, refractionIndex);

    Store(&result, x);
}

template <class T>
template <class U, class V>
Vector3<T> Vector3<T>::Refract(const Vector3<U>& iVec, const Vector3<V>& nVec,
                               const float refractionIndex) noexcept
{
    const XMVECTOR x1 = Load(&iVec);
    const XMVECTOR x2 = Load(&nVec);
    const XMVECTOR x = XMVector3Refract(x1, x2, refractionIndex);

    Vector3<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U>
_Use_decl_annotations_

void Vector3<T>::Transform(const Vector3<U>* varray, const size_t count, const Matrix& m,
                           Vector3<T>* resultArray) noexcept
{
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    XMVector3TransformCoordStream(resultArray, sizeof Vector3Base<U>::Vector, varray, sizeof Vector3Base<T>::Vector,
                                  count, m1);
}

template <class T>
template <class U>
void Vector3<T>::TransformNormal(const Vector3<U>& v, const Matrix& m, Vector3<T>& result) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    const XMVECTOR x = XMVector3TransformNormal(v1, m1);

    Store(&result, x);
}

template <class T>
template <class U>
Vector3<T> Vector3<T>::TransformNormal(const Vector3<U>& v, const Matrix& m) noexcept
{
    const XMVECTOR v1 = Load(&v);
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    const XMVECTOR x = XMVector3TransformNormal(v1, m1);

    Vector3<T> result;
    Store(&result, x);
    return result;
}

template <class T>
template <class U>
_Use_decl_annotations_

void Vector3<T>::TransformNormal(const Vector3<U>* varray, const size_t count, const Matrix& m,
                                 Vector3<T>* resultArray) noexcept
{
    const XMMATRIX m1 = XMLoadFloat4x4(&m);
    XMVector3TransformNormalStream(resultArray, sizeof Vector3Base<U>::Vector, varray, sizeof Vector3Base<T>::Vector,
                                   count, m1);
}

template struct Vector3<float>;
template struct Vector3<int32_t>;
template struct Vector3<uint32_t>;


//---------------------------------------------------------------------------//
//!
//! \file   Utility_InterpolationPolicy_def.hpp
//! \author Alex Robinson
//! \brief  Interpolation policy struct definitions
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_INTERPOLATION_POLICY_DEF_HPP
#define UTILITY_INTERPOLATION_POLICY_DEF_HPP

// Std Lib Includes
#include <cmath>

// Boost Includes
#include <boost/mpl/or.hpp>

// FRENSIE Includes
#include "Utility_QuantityTraits.hpp"
#include "Utility_IsFloatingPoint.hpp"
#include "Utility_DesignByContract.hpp"

namespace Utility{

// Interpolate between two processed points
template<typename ParentInterpolationType>
template<typename T>
T InterpolationHelper<ParentInterpolationType>::interpolate(
                                                 const T processed_indep_var_0,
                                                 const T processed_indep_var,
                                                 const T processed_dep_var_0,
                                                 const T processed_slope )
{
  // T must be a floating point type
  testStaticPrecondition( (std::is_floating_point<T>::value) );
  // Make sure the processed independent variables are valid
  testPrecondition( !Utility::QuantityTraits<T>::isnaninf(
						     processed_indep_var_0 ) );
  testPrecondition( !Utility::QuantityTraits<T>::isnaninf(
						       processed_indep_var ) );
  testPrecondition( processed_indep_var_0 <= processed_indep_var );
  // Make sure the processed dependent variable is valid
  testPrecondition( !Utility::QuantityTraits<T>::isnaninf(
						       processed_dep_var_0 ) );
  // Make sure that the slope is valid
  testPrecondition( !Utility::QuantityTraits<T>::isnaninf( processed_slope ) );

  return ParentInterpolationType::recoverProcessedDepVar(
               processed_dep_var_0 +
               processed_slope*(processed_indep_var - processed_indep_var_0) );
}

// Interpolate between two processed points and return the processed value
template<typename ParentInterpolationType>
template<typename T>
T InterpolationHelper<ParentInterpolationType>::interpolateAndProcess(
                                                 const T processed_indep_var_0,
                                                 const T processed_indep_var,
                                                 const T processed_dep_var_0,
                                                 const T processed_slope )
{
  // T must be a floating point type
  testStaticPrecondition( (std::is_floating_point<T>::value) );
  // Make sure the processed independent variables are valid
  testPrecondition( !Utility::QuantityTraits<T>::isnaninf(
						     processed_indep_var_0 ) );
  testPrecondition( !Utility::QuantityTraits<T>::isnaninf(
						       processed_indep_var ) );
  testPrecondition( processed_indep_var_0 <= processed_indep_var );
  // Make sure the processed dependent variable is valid
  testPrecondition( !Utility::QuantityTraits<T>::isnaninf(
						       processed_dep_var_0 ) );
  // Make sure that the slope is valid
  testPrecondition( !Utility::QuantityTraits<T>::isnaninf( processed_slope ) );

  return processed_dep_var_0 +
    processed_slope*(processed_indep_var - processed_indep_var_0);
}

// Calculate the unit base grid length (L)
/*! \details For LinLog and LogLog interpolation types the grid length
 * that is calculated is not a traditional length. It is the distance between
 * the processed upper independent value and the processed lower
 * independent value. This is why any units associated with the independent
 * grid limits are stripped away.
 */
template<typename ParentInterpolationType>
template<typename IndepType>
inline typename QuantityTraits<IndepType>::RawType
InterpolationHelper<ParentInterpolationType>::calculateUnitBaseGridLength(
                                       const IndepType grid_lower_indep_value,
                                       const IndepType grid_upper_indep_value )
{
  // Make sure the grid is valid
  testPrecondition( grid_lower_indep_value <= grid_upper_indep_value );
  testPrecondition( ParentInterpolationType::isIndepVarInValidRange(
                                                    grid_lower_indep_value ) );

  return ThisType::calculateUnitBaseGridLengthProcessed(
          ParentInterpolationType::processIndepVar( grid_lower_indep_value ),
          ParentInterpolationType::processIndepVar( grid_upper_indep_value ) );
}

// Calculate the unit base grid length from a processed grid (L)
/*! \details For LinLog and LogLog interpolation types the grid length
 * that is calculated is not a traditional length. It is the distance between
 * the processed upper independent value and the processed lower
 * independent value. This is why any units associated with the independent
 * grid limits are stripped away. Due to conversion of the independent
 * values from a cosine (mu) to a delta cosine ( 1 - mu ) + nudge for LogLogCos
 * and LinLogCos, it is assumed the processed grids are inverted to ensure they
 * are in ascending order.
 */
template<typename ParentInterpolationType>
template<typename T>
inline T InterpolationHelper<ParentInterpolationType>::calculateUnitBaseGridLengthProcessed(
                                     const T processed_grid_lower_indep_value,
                                     const T processed_grid_upper_indep_value )
{
  // Make sure the grid is valid
  testPrecondition( processed_grid_lower_indep_value <=
                    processed_grid_upper_indep_value );

  const T processed_grid_length =
    processed_grid_upper_indep_value - processed_grid_lower_indep_value;

  // Make sure the grid length is valid
  testPrecondition( processed_grid_length >= 0.0 );

  return processed_grid_length;
}

// Calculate the unit base independent variable (eta)
/*! \details The independent grid length is calculated using the
 * processed independent grid limits. It has been found that a tolerance of
 * 1e-3 works best for most applications.
 */
template<typename ParentInterpolationType>
template<typename IndepType>
inline typename QuantityTraits<IndepType>::RawType
InterpolationHelper<ParentInterpolationType>::calculateUnitBaseIndepVar(
          const IndepType indep_var,
          const IndepType indep_var_min,
          const typename QuantityTraits<IndepType>::RawType indep_grid_length,
          const double tol )
{
  // Make sure the intermediate grid min indep var is valid
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_min ) );
  testPrecondition( ParentInterpolationType::isIndepVarInValidRange( indep_var_min ) );
  // Make sure the intermediate grid length is valid
  testPrecondition( !QuantityTraits<typename QuantityTraits<IndepType>::RawType>::isnaninf( indep_grid_length ) );
  testPrecondition( indep_grid_length > 0.0 );
  // Make sure the independent variable is valid
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var ) );
  testPrecondition( ParentInterpolationType::isIndepVarInValidRange( indep_var ) );
  testPrecondition( indep_var >=
                    ThisType::calculateFuzzyLowerBound( indep_var_min, tol ) );
  remember( typename QuantityTraits<IndepType>::RawType test_difference =
            ParentInterpolationType::processIndepVar(indep_var) -
            ParentInterpolationType::processIndepVar(indep_var_min) );

  testPrecondition( test_difference <= ThisType::calculateFuzzyUpperBound(
                                                    indep_grid_length, tol ) );

  return calculateUnitBaseIndepVarProcessed(
                       ParentInterpolationType::processIndepVar(indep_var),
                       ParentInterpolationType::processIndepVar(indep_var_min),
                       indep_grid_length,
                       tol );
}

// Calculate the unit base independent variable (eta)
/*! \details It has been found that a tolerance of 1e-3 works best for most
 * applications.
 */
template<typename ParentInterpolationType>
template<typename T>
inline T InterpolationHelper<ParentInterpolationType>::calculateUnitBaseIndepVarProcessed(
                                               const T processed_indep_var,
                                               const T processed_indep_var_min,
                                               const T indep_grid_length,
                                               const double tol )
{
  // Make sure the intermediate grid min indep var is valid
  testPrecondition( !QuantityTraits<T>::isnaninf( processed_indep_var_min ) );
  // Make sure the independent y variable is valid
  testPrecondition( !QuantityTraits<T>::isnaninf(processed_indep_var) );
  testPrecondition( processed_indep_var >= ThisType::calculateFuzzyLowerBound(
                                              processed_indep_var_min, tol ) );
  remember( T test_difference = processed_indep_var - processed_indep_var_min);
  testPrecondition( test_difference <= ThisType::calculateFuzzyUpperBound(
                                                    indep_grid_length, tol ) );
  // Make sure the intermediate grid length is valid
  testPrecondition( !QuantityTraits<T>::isnaninf( indep_grid_length ) );
  testPrecondition( indep_grid_length > 0.0 );

  T eta = (processed_indep_var - processed_indep_var_min)/indep_grid_length;

  // Check for rounding errors and correct
  if( eta > 1.0 )
  {
    if( eta - 1.0 < tol )
      eta = 1.0;
  }
  else if( eta < 0.0 )
  {
    if( eta > -tol )
      eta = 0.0;
  }

  // Make sure eta is valid
  testPostcondition( eta >= 0.0 );
  testPostcondition( eta <= 1.0 );

  return eta;
}

// Calculate the independent variable (from eta)
/*! \details It has been found that a tolerance of 1e-3 works best for most
 * applications.
 */
template<typename ParentInterpolationType>
template<typename IndepType>
inline IndepType
InterpolationHelper<ParentInterpolationType>::calculateIndepVar(
          const typename QuantityTraits<IndepType>::RawType eta,
          const IndepType indep_var_min,
          const typename QuantityTraits<IndepType>::RawType indep_grid_length,
          const double tol )
{
  // Make sure the eta value is valid
  testPrecondition( eta >= 0.0 );
  testPrecondition( eta <= 1.0 );
  // Make sure the grid min indep var is valid
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_min ) );
  testPrecondition( ParentInterpolationType::isIndepVarInValidRange( indep_var_min ) );
  // Make sure the grid length is valid
  testPrecondition( !QuantityTraits<typename QuantityTraits<IndepType>::RawType>::isnaninf( indep_grid_length ) );
  testPrecondition( indep_grid_length >= 0.0 );

  IndepType grid_indep_var( QuantityTraits<IndepType>::initializeQuantity(
    ParentInterpolationType::recoverProcessedIndepVar(
                    ParentInterpolationType::processIndepVar( indep_var_min ) +
                    indep_grid_length*eta ) ) );

  // Check for rounding errors
  if( grid_indep_var < indep_var_min &&
      grid_indep_var >= ThisType::calculateFuzzyLowerBound(indep_var_min, tol))
    grid_indep_var = indep_var_min;

  // Make sure the calculated independent variable is valid
  testPostcondition( grid_indep_var >= indep_var_min );

  return grid_indep_var;
}

// Calculate the processed independent variable (from eta)
/*! \details A tolerance is not required with this method because no variable
 * processing is done. Due to conversion of the independent values from a cosine
 * (mu) to a delta cosine ( 1 - mu ) + nudge for LogLogCos and LinLogCos, it is
 * assumed the processed grids are inverted to ensure they are in ascending
 * order.
 */
template<typename ParentInterpolationType>
template<typename T>
inline T InterpolationHelper<ParentInterpolationType>::calculateProcessedIndepVar(
                                               const T eta,
                                               const T processed_indep_var_min,
                                               const T indep_grid_length )
{
  // Make sure the eta value is valid
  testPrecondition( eta >= 0.0 );
  testPrecondition( eta <= 1.0 );
  // Make sure the grid min indep var is valid
  testPrecondition( !QuantityTraits<T>::isnaninf( processed_indep_var_min ) );
  // Make sure the grid length is valid
  testPrecondition( !QuantityTraits<T>::isnaninf( indep_grid_length ) );
  testPrecondition( indep_grid_length >= 0.0 );

  return processed_indep_var_min + indep_grid_length*eta;
}

// Calculate the "fuzzy" lower bound (lower bound with roundoff tolerance)
/*! \details It has been found that a tolerance of 1e-3 works best for most
 * applications.
 */
template<typename ParentInterpolationType>
template<typename T>
inline T InterpolationHelper<ParentInterpolationType>::calculateFuzzyLowerBound(
                                                             const T value,
                                                             const double tol )
{
  if( value < QuantityTraits<T>::zero() )
    return value*(1+tol);
  else
    return value*(1-tol);
}

// Calculate the "fuzzy" upper bound (upper bound with roundoff tolerance)
/*! \details It has been found that a tolerance of 1e-3 works best for most
 * applications
 */
template<typename ParentInterpolationType>
template<typename T>
inline T InterpolationHelper<ParentInterpolationType>::calculateFuzzyUpperBound(
                                                             const T value,
                                                             const double tol )
{
  if( value > QuantityTraits<T>::zero() )
    return value*(1+tol);
  else
    return value*(1-tol);
}

// The name of the policy
template<typename ParentInterpolationType>
inline std::string InterpolationHelper<ParentInterpolationType>::name()
{
  return Utility::typeName<ParentInterpolationType>();
}

// Get the interpolation type
inline InterpolationType LogLog::getInterpolationType()
{
  return LOGLOG_INTERPOLATION;
}

// Interpolate between two points
template<typename IndepType, typename DepType>
inline DepType LogLog::interpolate( const IndepType indep_var_0,
				    const IndepType indep_var_1,
				    const IndepType indep_var,
				    const DepType dep_var_0,
				    const DepType dep_var_1 )
{
  // The IndepType must be a floating point type
  testStaticPrecondition( (Utility::IsFloatingPoint<IndepType>::value) );
  testStaticPrecondition( (Utility::IsFloatingPoint<DepType>::value) );
  // Make sure the independent variables are valid
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_0 ) );
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_1 ) );
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var ) );
  testPrecondition( LogLog::isIndepVarInValidRange( indep_var_0 ) );
  testPrecondition( LogLog::isIndepVarInValidRange( indep_var_1 ) );
  testPrecondition( LogLog::isIndepVarInValidRange( indep_var ) );
  testPrecondition( indep_var_0 < indep_var_1 );
  testPrecondition( indep_var >= indep_var_0 );
  testPrecondition( indep_var <= indep_var_1 );
  // Make sure the dependent variables are valid
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_0 ) );
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_1 ) );
  testPrecondition( LogLog::isDepVarInValidRange( dep_var_0 ) );
  testPrecondition( LogLog::isDepVarInValidRange( dep_var_1 ) );

  return dep_var_0*
    pow((dep_var_1/dep_var_0),
	log(indep_var/indep_var_0)/log(indep_var_1/indep_var_0));
}

// Interpolate between two points using the indep variable ratio (beta)
template<typename T, typename DepType>
inline DepType LogLog::interpolate( const T beta,
                                    const DepType dep_var_0,
                                    const DepType dep_var_1 )
{
  // The IndepType must be a floating point type
  testStaticPrecondition( (std::is_floating_point<typename QuantityTraits<T>::RawType>::value) );
  testStaticPrecondition( (std::is_floating_point<typename QuantityTraits<DepType>::RawType>::value ) );
  // Make sure the independent variables are valid
  testPrecondition( beta >= QuantityTraits<T>::zero() );
  testPrecondition( beta <= QuantityTraits<T>::one() );
  // Make sure the dependent variables are valid
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_0 ) );
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_1 ) );
  testPrecondition( LogLog::isDepVarInValidRange( dep_var_0 ) );
  testPrecondition( LogLog::isDepVarInValidRange( dep_var_1 ) );

  return dep_var_0*pow((dep_var_1/dep_var_0),beta);
}

// Interpolate between two points and return the processed value
template<typename IndepType, typename DepType>
inline typename QuantityTraits<DepType>::RawType
LogLog::interpolateAndProcess( const IndepType indep_var_0,
			       const IndepType indep_var_1,
			       const IndepType indep_var,
			       const DepType dep_var_0,
			       const DepType dep_var_1 )
{
  // T must be a floating point type
  testStaticPrecondition( (Utility::IsFloatingPoint<IndepType>::value) );
  testStaticPrecondition( (Utility::IsFloatingPoint<DepType>::value) );
  // Make sure the independent variables are valid
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_0 ) );
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_1 ) );
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var ) );
  testPrecondition( LogLog::isIndepVarInValidRange( indep_var_0 ) );
  testPrecondition( LogLog::isIndepVarInValidRange( indep_var_1 ) );
  testPrecondition( LogLog::isIndepVarInValidRange( indep_var ) );
  testPrecondition( indep_var_0 < indep_var_1 );
  testPrecondition( indep_var >= indep_var_0 );
  testPrecondition( indep_var <= indep_var_1 );
  // Make sure the dependent variables are valid
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_0 ) );
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_1 ) );
  testPrecondition( LogLog::isDepVarInValidRange( dep_var_0 ) );
  testPrecondition( LogLog::isDepVarInValidRange( dep_var_1 ) );

  return log( getRawQuantity(dep_var_0) ) + log ( dep_var_1/dep_var_0 )*
    log( indep_var/indep_var_0 )/log( indep_var_1/indep_var_0 );
}

// Process the independent value
template<typename T>
inline typename QuantityTraits<T>::RawType
LogLog::processIndepVar( const T indep_var )
{
  // Make sure the indep var value is valid
  testPrecondition( LogLog::isIndepVarInValidRange( indep_var ) );

  return log( getRawQuantity(indep_var) );
}

// Process the dependent value
template<typename T>
inline typename QuantityTraits<T>::RawType
LogLog::processDepVar( const T dep_var )
{
  // Make sure the indep var value is valid
  testPrecondition( LogLog::isDepVarInValidRange( dep_var ) );

  return log( getRawQuantity(dep_var) );
}

// Recover the processed independent value
template<typename T>
inline T LogLog::recoverProcessedIndepVar( const T processed_indep_var )
{
  return exp( processed_indep_var );
}

// Recover the processed dependent value
template<typename T>
inline T LogLog::recoverProcessedDepVar( const T processed_dep_var )
{
  return exp( processed_dep_var );
}

// Test if the independent value is in a valid range (doesn't check nan/inf)
template<typename T>
inline bool LogLog::isIndepVarInValidRange( const T indep_var )
{
  return indep_var > QuantityTraits<T>::zero();
}

// Test if the dependent value is in a valid range (doesn't check nan/inf)
template<typename T>
inline bool LogLog::isDepVarInValidRange( const T dep_var )
{
  // Make sure the indep var is not inf or nan
  testPrecondition( !QuantityTraits<T>::isnaninf( dep_var ) );

  return dep_var > QuantityTraits<T>::zero();
}

// Get the interpolation type
inline InterpolationType LogLin::getInterpolationType()
{
  return LOGLIN_INTERPOLATION;
}

// Interpolate between two points
template<typename IndepType, typename DepType>
inline DepType LogLin::interpolate( const IndepType indep_var_0,
				    const IndepType indep_var_1,
				    const IndepType indep_var,
				    const DepType dep_var_0,
				    const DepType dep_var_1 )
{
  // T must be a floating point type
  testStaticPrecondition( (Utility::IsFloatingPoint<IndepType>::value) );
  testStaticPrecondition( (Utility::IsFloatingPoint<DepType>::value) );
  // Make sure the independent variables are valid
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_0 ) );
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_1 ) );
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var ) );
  testPrecondition( LogLin::isIndepVarInValidRange( indep_var_0 ) );
  testPrecondition( LogLin::isIndepVarInValidRange( indep_var_1 ) );
  testPrecondition( LogLin::isIndepVarInValidRange( indep_var ) );
  testPrecondition( indep_var_0 < indep_var_1 );
  testPrecondition( indep_var >= indep_var_0 );
  testPrecondition( indep_var <= indep_var_1 );
  // Make sure the dependent variables are valid
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_0 ) );
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_1 ) );
  testPrecondition( LogLin::isDepVarInValidRange( dep_var_0 ) );
  testPrecondition( LogLin::isDepVarInValidRange( dep_var_1 ) );

  return dep_var_0*pow((dep_var_1/dep_var_0), (indep_var-indep_var_0)/(indep_var_1-indep_var_0));
}

// Interpolate between two points using the indep variable ratio (beta)
template<typename T, typename DepType>
inline DepType LogLin::interpolate( const T beta,
                                    const DepType dep_var_0,
                                    const DepType dep_var_1 )
{
  // The IndepType must be a floating point type
  testStaticPrecondition( (std::is_floating_point<typename QuantityTraits<T>::RawType>::value) );
  testStaticPrecondition( (std::is_floating_point<typename QuantityTraits<DepType>::RawType>::value) );
  // Make sure the independent variables are valid
  testPrecondition( beta >= QuantityTraits<T>::zero() );
  testPrecondition( beta <= QuantityTraits<T>::one() );
  // Make sure the dependent variables are valid
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_0 ) );
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_1 ) );
  testPrecondition( LogLin::isDepVarInValidRange( dep_var_0 ) );
  testPrecondition( LogLin::isDepVarInValidRange( dep_var_1 ) );

  return dep_var_0*pow((dep_var_1/dep_var_0),beta);
}

// Interpolate between two points and return the processed value
template<typename IndepType, typename DepType>
inline typename QuantityTraits<DepType>::RawType
LogLin::interpolateAndProcess( const IndepType indep_var_0,
			       const IndepType indep_var_1,
			       const IndepType indep_var,
			       const DepType dep_var_0,
			       const DepType dep_var_1 )
{
  // T must be a floating point type
  testStaticPrecondition( (Utility::IsFloatingPoint<IndepType>::value) );
  testStaticPrecondition( (Utility::IsFloatingPoint<DepType>::value) );
  // Make sure the independent variables are valid
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_0 ) );
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_1 ) );
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var ) );
  testPrecondition( LogLin::isIndepVarInValidRange( indep_var_0 ) );
  testPrecondition( LogLin::isIndepVarInValidRange( indep_var_1 ) );
  testPrecondition( LogLin::isIndepVarInValidRange( indep_var ) );
  testPrecondition( indep_var_0 < indep_var_1 );
  testPrecondition( indep_var >= indep_var_0 );
  testPrecondition( indep_var <= indep_var_1 );
  // Make sure the dependent variables are valid
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_0 ) );
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_1 ) );
  testPrecondition( LogLin::isDepVarInValidRange( dep_var_0 ) );
  testPrecondition( LogLin::isDepVarInValidRange( dep_var_1 ) );

  return log( getRawQuantity(dep_var_0) ) + log( dep_var_1/dep_var_0 )*
    (indep_var-indep_var_0)/(indep_var_1-indep_var_0);
}

// Process the independent value
template<typename T>
inline typename QuantityTraits<T>::RawType
LogLin::processIndepVar( const T indep_var )
{
  // Make sure the indep var value is valid
  testPrecondition( LogLin::isIndepVarInValidRange( indep_var ) );

  return getRawQuantity(indep_var);
}

// Process the dependent value
template<typename T>
inline typename QuantityTraits<T>::RawType
LogLin::processDepVar( const T dep_var )
{
  // Make sure the indep var value is valid
  testPrecondition( LogLin::isDepVarInValidRange( dep_var ) );

  return log( getRawQuantity(dep_var) );
}

// Recover the processed independent value
template<typename T>
inline T LogLin::recoverProcessedIndepVar( const T processed_indep_var )
{
  return processed_indep_var;
}

// Recover the processed dependent value
template<typename T>
inline T LogLin::recoverProcessedDepVar( const T processed_dep_var )
{
  return exp( processed_dep_var );
}

// Test if the independent value is in a valid range (doesn't check nan/inf)
template<typename T>
inline bool LogLin::isIndepVarInValidRange( const T indep_var )
{
  return true;
}

// Test if the dependent value is in a valid range (doesn't check nan/inf)
template<typename T>
inline bool LogLin::isDepVarInValidRange( const T dep_var )
{
  // Make sure the indep var is not inf or nan
  testPrecondition( !QuantityTraits<T>::isnaninf( dep_var ) );

  return dep_var > QuantityTraits<T>::zero();
}

// Get the interpolation type
inline InterpolationType LinLog::getInterpolationType()
{
  return LINLOG_INTERPOLATION;
}

// Interpolate between two points
template<typename IndepType, typename DepType>
inline DepType LinLog::interpolate( const IndepType indep_var_0,
				    const IndepType indep_var_1,
				    const IndepType indep_var,
				    const DepType dep_var_0,
				    const DepType dep_var_1 )
{
  // T must be a floating point type
  testStaticPrecondition( (Utility::IsFloatingPoint<IndepType>::value) );
  testStaticPrecondition( (Utility::IsFloatingPoint<DepType>::value) );
  // Make sure the independent variables are valid
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_0 ) );
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_1 ) );
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var ) );
  testPrecondition( LinLog::isIndepVarInValidRange( indep_var_0 ) );
  testPrecondition( LinLog::isIndepVarInValidRange( indep_var_1 ) );
  testPrecondition( LinLog::isIndepVarInValidRange( indep_var ) );
  testPrecondition( indep_var_0 < indep_var_1 );
  testPrecondition( indep_var >= indep_var_0 );
  testPrecondition( indep_var <= indep_var_1 );
  // Make sure the dependent variables are valid
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_0 ) );
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_1 ) );
  testPrecondition( LinLog::isDepVarInValidRange( dep_var_0 ) );
  testPrecondition( LinLog::isDepVarInValidRange( dep_var_1 ) );

  DepType term_2( (dep_var_1 - dep_var_0)*log(indep_var/indep_var_0)/
		  log(indep_var_1/indep_var_0) );

  return dep_var_0 + term_2;
}

// Interpolate between two points using the indep variable ratio (beta)
template<typename T, typename DepType>
inline DepType LinLog::interpolate( const T beta,
                                    const DepType dep_var_0,
                                    const DepType dep_var_1 )
{
  // The IndepType must be a floating point type
  testStaticPrecondition( (std::is_floating_point<typename QuantityTraits<T>::RawType>::value) );
  testStaticPrecondition( (std::is_floating_point<typename QuantityTraits<DepType>::RawType>::value) );
  // Make sure the independent variables are valid
  testPrecondition( beta >= QuantityTraits<T>::zero() );
  testPrecondition( beta <= QuantityTraits<T>::one() );
  // Make sure the dependent variables are valid
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_0 ) );
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_1 ) );
  testPrecondition( LinLog::isDepVarInValidRange( dep_var_0 ) );
  testPrecondition( LinLog::isDepVarInValidRange( dep_var_1 ) );

  return dep_var_0 + (dep_var_1 - dep_var_0)*beta;
}

// Interpolate between two points and return the processed value
template<typename IndepType, typename DepType>
inline typename QuantityTraits<DepType>::RawType
LinLog::interpolateAndProcess( const IndepType indep_var_0,
			       const IndepType indep_var_1,
			       const IndepType indep_var,
			       const DepType dep_var_0,
			       const DepType dep_var_1 )
{
  return getRawQuantity( interpolate( indep_var_0,
				      indep_var_1,
				      indep_var,
				      dep_var_0,
				      dep_var_1 ) );
}

// Process the independent value
template<typename T>
inline typename QuantityTraits<T>::RawType
LinLog::processIndepVar( const T indep_var )
{
  // Make sure the indep var value is valid
  testPrecondition( LinLog::isIndepVarInValidRange( indep_var ) );

  return log( getRawQuantity(indep_var) );
}

// Process the dependent value
template<typename T>
inline typename QuantityTraits<T>::RawType
LinLog::processDepVar( const T dep_var )
{
  // Make sure the indep var value is valid
  testPrecondition( LinLog::isDepVarInValidRange( dep_var ) );

  return getRawQuantity(dep_var);
}

// Recover the processed independent value
template<typename T>
inline T LinLog::recoverProcessedIndepVar( const T processed_indep_var )
{
  return exp( processed_indep_var );
}

// Recover the processed dependent value
template<typename T>
inline T LinLog::recoverProcessedDepVar( const T processed_dep_var )
{
  return processed_dep_var;
}

// Test if the independent value is in a valid range (doesn't check nan/inf)
template<typename T>
inline bool LinLog::isIndepVarInValidRange( const T indep_var )
{
  return indep_var > QuantityTraits<T>::zero();
}

// Test if the dependent value is in a valid range (doesn't check nan/inf)
template<typename T>
inline bool LinLog::isDepVarInValidRange( const T dep_var )
{
  // Make sure the indep var is not inf or nan
  testPrecondition( !QuantityTraits<T>::isnaninf( dep_var ) );

  return true;
}

// Get the interpolation type
inline InterpolationType LinLin::getInterpolationType()
{
  return LINLIN_INTERPOLATION;
}

// Interpolate between two points
template<typename IndepType, typename DepType>
inline DepType LinLin::interpolate( const IndepType indep_var_0,
				    const IndepType indep_var_1,
				    const IndepType indep_var,
				    const DepType dep_var_0,
				    const DepType dep_var_1 )
{
  // T must be a floating point type
  testStaticPrecondition( (Utility::IsFloatingPoint<IndepType>::value) );
  testStaticPrecondition( (Utility::IsFloatingPoint<DepType>::value) );
  // Make sure the independent variables are valid
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_0 ) );
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var_1 ) );
  testPrecondition( !QuantityTraits<IndepType>::isnaninf( indep_var ) );
  testPrecondition( LinLin::isIndepVarInValidRange( indep_var_0 ) );
  testPrecondition( LinLin::isIndepVarInValidRange( indep_var_1 ) );
  testPrecondition( LinLin::isIndepVarInValidRange( indep_var ) );
  testPrecondition( indep_var_0 < indep_var_1 );
  testPrecondition( indep_var >= indep_var_0 );
  testPrecondition( indep_var <= indep_var_1 );
  // Make sure the dependent variables are valid
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_0 ) );
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_1 ) );
  testPrecondition( LinLin::isDepVarInValidRange( dep_var_0 ) );
  testPrecondition( LinLin::isDepVarInValidRange( dep_var_1 ) );

  DepType term_2( (dep_var_1 - dep_var_0)/(indep_var_1 - indep_var_0)*
		  (indep_var - indep_var_0) );

  return dep_var_0 + term_2;
}

// Interpolate between two points using the indep variable ratio (beta)
template<typename T, typename DepType>
inline DepType LinLin::interpolate( const T beta,
                                    const DepType dep_var_0,
                                    const DepType dep_var_1 )
{
  // The IndepType must be a floating point type
  testStaticPrecondition( (std::is_floating_point<typename QuantityTraits<T>::RawType>::value) );
  testStaticPrecondition( (std::is_floating_point<typename QuantityTraits<DepType>::RawType>::value) );
  // Make sure the independent variables are valid
  testPrecondition( beta >= QuantityTraits<T>::zero() );
  testPrecondition( beta <= QuantityTraits<T>::one() );
  // Make sure the dependent variables are valid
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_0 ) );
  testPrecondition( !QuantityTraits<DepType>::isnaninf( dep_var_1 ) );
  testPrecondition( LinLin::isDepVarInValidRange( dep_var_0 ) );
  testPrecondition( LinLin::isDepVarInValidRange( dep_var_1 ) );

  return dep_var_0 + (dep_var_1 - dep_var_0)*beta;
}

// Interpolate between two points and return the processed value
template<typename IndepType, typename DepType>
inline typename QuantityTraits<DepType>::RawType
LinLin::interpolateAndProcess( const IndepType indep_var_0,
			       const IndepType indep_var_1,
			       const IndepType indep_var,
			       const DepType dep_var_0,
			       const DepType dep_var_1 )
{
  return getRawQuantity( interpolate( indep_var_0,
				      indep_var_1,
				      indep_var,
				      dep_var_0,
				      dep_var_1 ) );
}

// Process the independent value
template<typename T>
inline typename QuantityTraits<T>::RawType
LinLin::processIndepVar( const T indep_var )
{
  // Make sure the indep var value is valid
  testPrecondition( LinLin::isIndepVarInValidRange( indep_var  ) );

  return getRawQuantity(indep_var);
}

// Process the dependent value
template<typename T>
inline typename QuantityTraits<T>::RawType
LinLin::processDepVar( const T dep_var )
{
  // Make sure the indep var value is valid
  testPrecondition( LinLin::isDepVarInValidRange( dep_var ) );

  return getRawQuantity(dep_var);
}

// Recover the processed independent value
template<typename T>
inline T LinLin::recoverProcessedIndepVar( const T processed_indep_var )
{
  return processed_indep_var;
}

// Recover the processed dependent value
template<typename T>
inline T LinLin::recoverProcessedDepVar( const T processed_dep_var )
{
  return processed_dep_var;
}

// Test if the independent value is in a valid range (doesn't check nan/inf)
template<typename T>
inline bool LinLin::isIndepVarInValidRange( const T indep_var )
{
  return true;
}

// Test if the dependent value is in a valid range (doesn't check nan/inf)
template<typename T>
inline bool LinLin::isDepVarInValidRange( const T dep_var )
{
  // Make sure the indep var is not inf or nan
  testPrecondition( !QuantityTraits<T>::isnaninf( dep_var ) );

  return true;
}

} // end Utility namespace

#endif // end UTILITY_INTERPOLATION_POLICY_DEF_HPP

//---------------------------------------------------------------------------//
// end Utility_InterpolationPolicy_def.hpp
//---------------------------------------------------------------------------//


#include <boost/geometry/strategies/spherical/azimuth.hpp>


#pragma once

#include "fold.hpp"

#include <ap_int.h>
#include <boost/operators.hpp>

namespace ntl {
    template <typename T, typename Counter = ap_uint<16> >
    class counter : public ntl::fold<T, Counter, true>
    {
    public:
        typedef ntl::fold<T, Counter, true> base;
        typedef typename base::in_t in_t;
        counter() : base(-1) {}

        void step(in_t& in)
        {
    #pragma HLS pipeline
            base::step(in, [](Counter cnt, const T& t) {
                return Counter(++cnt);
            });
        }
    };

    template <int width, int limit = ~0>
    class maxed_int : public ap_uint<width>, public boost::incrementable<maxed_int<width>>
    {
    public:
        typedef ap_uint<width> base;
        maxed_int() : base() {}
        maxed_int(const base& b) : base(b) {}
#define CTOR(TYPE)				\
        INLINE maxed_int(TYPE v) : base(v) {}
        CTOR(bool)
        CTOR(signed char)
        CTOR(unsigned char)
        CTOR(short)
        CTOR(unsigned short)
        CTOR(int)
        CTOR(unsigned int)
        CTOR(long)
        CTOR(unsigned long)
        CTOR(unsigned long long)
        CTOR(long long)
        CTOR(half)
        CTOR(float)
        CTOR(double)
        CTOR(const char*)
#undef CTOR

        maxed_int& operator++()
        {
            if (*this != limit)
                ap_uint<width>::operator++(*this);
            return *this;
        }
    };
}


//*********************************************************
//
// Copyright (c) Microsoft. All rights reserved.
// This code is licensed under the MIT License (MIT).
// THIS CODE IS PROVIDED *AS IS* WITHOUT WARRANTY OF
// ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING ANY
// IMPLIED WARRANTIES OF FITNESS FOR A PARTICULAR
// PURPOSE, MERCHANTABILITY, OR NON-INFRINGEMENT.
//
//*********************************************************

#include "ros/ros.h"
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <boost/concept_check.hpp>
#include <termios.h>
#include <geometry_msgs/Twist.h>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/LU>
#include <eigen3/Eigen/Eigen>
#include <iostream>
#include<fstream>

#include "calibration.h"

using namespace Eigen;

/*****************************************************************************/
/*                                                                           */
/*****************************************************************************/
void printHelp()
{
    std::cout<<"---------------------------------------------------------"<<std::endl
      <<"Robot-HoloLens calibration"<<std::endl
      <<"  space: record position"<<std::endl
      <<"  d: delete last recorded position"<<std::endl
      <<"  c: conduct calibration"<<std::endl
      <<"  a: auto calibration (Pepper Robot)"<<std::endl
      <<"  w: write calibration data"<<std::endl
      <<"  z: write calibration parameter log as text file"<<std::endl
      <<"  t: toggle calibration mode (clear all recorded position)"<<std::endl
      <<"  h,?: print this help"<<std::endl
      <<"  q: quit calibration tool"<<std::endl
      << std::endl
      <<"Robot commands"<<std::endl
      <<"  i,j,k,l: move robot forward, left, backward, right"<<std::endl
      <<"  u,o: rotate robot counter-clockwise, clockwise"<<std::endl
      <<"  s: stop robot movement"<<std::endl
      << std::endl
      <<"Pepper commands"<<std::endl
      <<"  e: wakeup"<<std::endl
      <<"  r: rest"<<std::endl
      <<"  p: toggle stiffness"<<std::endl
      <<"  0,1,2,3: various head positions"<<std::endl
      <<"---------------------------------------------------------"<<std::endl;
}
/*****************************************************************************/
/*                                                                           */
/*****************************************************************************/
int main(int argc, char **argv)
{
    ros::init(argc, argv, "static_calibration");

    if (argc < 3) {
        std::cout<<"Usage: static_calibration <robot's odom frame name> <robot's frame name linking to hololens> <robot's foot frame name> <optional: calibration file>"<<std::endl;
        return 0;
    }

    std::string odomFrame(argv[1]);
    std::string holoLinkedFrame(argv[2]);
    std::string robotFootFrame(argv[3]);
    std::string fpath;

    if (argc >= 5) {
        fpath = std::string(argv[4]);
    }

    printHelp();

    // set up publish rate
    calibrator calib(holoLinkedFrame, odomFrame, robotFootFrame, fpath);

    calib.run();

    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2013 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2013 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2013 Mateusz Loskot, London, UK.
// Copyright (c) 2013-2014 Adam Wulkiewicz, Lodz, Poland.

// Use, modification and distribution is 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_GEOMETRY_ALGORITHMS_DETAIL_EXTREME_POINTS_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXTREME_POINTS_HPP

#include <cstddef>

#include <boost/range.hpp>

#include <boost/geometry/algorithms/detail/interior_iterator.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/ring_type.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>
#include <boost/geometry/iterators/ever_circling_iterator.hpp>

#include <boost/geometry/algorithms/detail/assign_box_corners.hpp>

#include <boost/geometry/strategies/side.hpp>

#include <boost/geometry/util/math.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace extreme_points
{

template <std::size_t Dimension>
struct compare
{
    template <typename Point>
    inline bool operator()(Point const& lhs, Point const& rhs)
    {
        return geometry::get<Dimension>(lhs) < geometry::get<Dimension>(rhs);
    }
};

template <std::size_t Dimension, typename PointType, typename CoordinateType>
inline void move_along_vector(PointType& point, PointType const& extreme, CoordinateType const& base_value)
{
    // Moves a point along the vector (point, extreme) in the direction of the extreme  point
    // This adapts the possibly uneven legs of the triangle (or trapezium-like shape)
    //      _____extreme            _____
    //     /     \                 /     \                                    .
    //    /base   \         =>    /       \ point                             .
    //             \ point                                                    .
    //
    // For so-called intruders, it can be used to adapt both legs to the level of "base"
    // For the base, it can be used to adapt both legs to the level of the max-value of the intruders
    // If there are 2 or more extreme values, use the one close to 'point' to have a correct vector

    CoordinateType const value = geometry::get<Dimension>(point);
    //if (geometry::math::equals(value, base_value))
    if (value >= base_value)
    {
        return;
    }

    PointType vector = point;
    subtract_point(vector, extreme);

    CoordinateType const diff = geometry::get<Dimension>(vector);

    // diff should never be zero
    // because of the way our triangle/trapezium is build.
    // We just return if it would be the case.
    if (geometry::math::equals(diff, 0))
    {
        return;
    }

    CoordinateType const base_diff = base_value - geometry::get<Dimension>(extreme);

    multiply_value(vector, base_diff);
    divide_value(vector, diff);

    // The real move:
    point = extreme;
    add_point(point, vector);
}

template <std::size_t Dimension, typename Range, typename CoordinateType>
inline void move_along_vector(Range& range, CoordinateType const& base_value)
{
    if (range.size() >= 3)
    {
        move_along_vector<Dimension>(range.front(), *(range.begin() + 1), base_value);
        move_along_vector<Dimension>(range.back(), *(range.rbegin() + 1), base_value);
    }
}

template<typename Ring, std::size_t Dimension>
struct extreme_points_on_ring
{

    typedef typename geometry::coordinate_type<Ring>::type coordinate_type;
    typedef typename geofeatures_boost::range_iterator<Ring const>::type range_iterator;
    typedef typename geometry::point_type<Ring>::type point_type;

    typedef typename geometry::strategy::side::services::default_strategy
        <
            typename geometry::cs_tag<point_type>::type
        >::type side_strategy;

    template <typename CirclingIterator, typename Points>
    static inline bool extend(CirclingIterator& it,
            std::size_t n,
            coordinate_type max_coordinate_value,
            Points& points, int direction)
    {
        std::size_t safe_index = 0;
        do
        {
            it += direction;

            points.push_back(*it);

            if (safe_index++ >= n)
            {
                // E.g.: ring is completely horizontal or vertical (= invalid, but we don't want to have an infinite loop)
                return false;
            }
        } while (geometry::math::equals(geometry::get<Dimension>(*it), max_coordinate_value));

        return true;
    }

    // Overload without adding to poinst
    template <typename CirclingIterator>
    static inline bool extend(CirclingIterator& it,
            std::size_t n,
            coordinate_type max_coordinate_value,
            int direction)
    {
        std::size_t safe_index = 0;
        do
        {
            it += direction;

            if (safe_index++ >= n)
            {
                // E.g.: ring is completely horizontal or vertical (= invalid, but we don't want to have an infinite loop)
                return false;
            }
        } while (geometry::math::equals(geometry::get<Dimension>(*it), max_coordinate_value));

        return true;
    }

    template <typename CirclingIterator>
    static inline bool extent_both_sides(Ring const& ring,
            point_type extreme,
            CirclingIterator& left,
            CirclingIterator& right)
    {
        std::size_t const n = geofeatures_boost::size(ring);
        coordinate_type const max_coordinate_value = geometry::get<Dimension>(extreme);

        if (! extend(left, n, max_coordinate_value, -1))
        {
            return false;
        }
        if (! extend(right, n, max_coordinate_value, +1))
        {
            return false;
        }

        return true;
    }

    template <typename Collection, typename CirclingIterator>
    static inline bool collect(Ring const& ring,
            point_type extreme,
            Collection& points,
            CirclingIterator& left,
            CirclingIterator& right)
    {
        std::size_t const n = geofeatures_boost::size(ring);
        coordinate_type const max_coordinate_value = geometry::get<Dimension>(extreme);

        // Collects first left, which is reversed (if more than one point) then adds the top itself, then right
        if (! extend(left, n, max_coordinate_value, points, -1))
        {
            return false;
        }
        std::reverse(points.begin(), points.end());
        points.push_back(extreme);
        if (! extend(right, n, max_coordinate_value, points, +1))
        {
            return false;
        }

        return true;
    }

    template <typename Extremes, typename Intruders, typename CirclingIterator>
    static inline void get_intruders(Ring const& ring, CirclingIterator left, CirclingIterator right,
            Extremes const& extremes,
            Intruders& intruders)
    {
        if (geofeatures_boost::size(extremes) < 3)
        {
            return;
        }
        coordinate_type const min_value = geometry::get<Dimension>(*std::min_element(geofeatures_boost::begin(extremes), geofeatures_boost::end(extremes), compare<Dimension>()));

        // Also select left/right (if Dimension=1)
        coordinate_type const other_min = geometry::get<1 - Dimension>(*std::min_element(geofeatures_boost::begin(extremes), geofeatures_boost::end(extremes), compare<1 - Dimension>()));
        coordinate_type const other_max = geometry::get<1 - Dimension>(*std::max_element(geofeatures_boost::begin(extremes), geofeatures_boost::end(extremes), compare<1 - Dimension>()));

        std::size_t defensive_check_index = 0; // in case we skip over left/right check, collect modifies right too
        std::size_t const n = geofeatures_boost::size(ring);
        while (left != right && defensive_check_index < n)
        {
            coordinate_type const coordinate = geometry::get<Dimension>(*right);
            coordinate_type const other_coordinate = geometry::get<1 - Dimension>(*right);
            if (coordinate > min_value && other_coordinate > other_min && other_coordinate < other_max)
            {
                int const factor = geometry::point_order<Ring>::value == geometry::clockwise ? 1 : -1;
                int const first_side = side_strategy::apply(*right, extremes.front(), *(extremes.begin() + 1)) * factor;
                int const last_side = side_strategy::apply(*right, *(extremes.rbegin() + 1), extremes.back()) * factor;

                // If not lying left from any of the extemes side
                if (first_side != 1 && last_side != 1)
                {
                    //std::cout << "first " << first_side << " last " << last_side << std::endl;

                    // we start at this intrusion until it is handled, and don't affect our initial left iterator
                    CirclingIterator left_intrusion_it = right;
                    typename geofeatures_boost::range_value<Intruders>::type intruder;
                    collect(ring, *right, intruder, left_intrusion_it, right);

                    // Also moves these to base-level, makes sorting possible which can be done in case of self-tangencies
                    // (we might postpone this action, it is often not necessary. However it is not time-consuming)
                    move_along_vector<Dimension>(intruder, min_value);
                    intruders.push_back(intruder);
                    --right;
                }
            }
            ++right;
            defensive_check_index++;
        }
    }

    template <typename Extremes, typename Intruders>
    static inline void get_intruders(Ring const& ring,
            Extremes const& extremes,
            Intruders& intruders)
    {
        std::size_t const n = geofeatures_boost::size(ring);
        if (n >= 3)
        {
            geometry::ever_circling_range_iterator<Ring const> left(ring);
            geometry::ever_circling_range_iterator<Ring const> right(ring);
            ++right;

            get_intruders(ring, left, right, extremes, intruders);
        }
    }

    template <typename Iterator>
    static inline bool right_turn(Ring const& ring, Iterator it)
    {
        typename std::iterator_traits<Iterator>::difference_type const index
            = std::distance(geofeatures_boost::begin(ring), it);
        geometry::ever_circling_range_iterator<Ring const> left(ring);
        geometry::ever_circling_range_iterator<Ring const> right(ring);
        left += index;
        right += index;

        if (! extent_both_sides(ring, *it, left, right))
        {
            return false;
        }

        int const factor = geometry::point_order<Ring>::value == geometry::clockwise ? 1 : -1;
        int const first_side = side_strategy::apply(*(right - 1), *right, *left) * factor;
        int const last_side = side_strategy::apply(*left, *(left + 1), *right) * factor;

//std::cout << "Candidate at " << geometry::wkt(*it) << " first=" << first_side << " last=" << last_side << std::endl;

        // Turn should not be left (actually, it should be right because extent removes horizontal/collinear cases)
        return first_side != 1 && last_side != 1;
    }

    // Gets the extreme segments (top point plus neighbouring points), plus intruders, if any, on the same ring
    template <typename Extremes, typename Intruders>
    static inline bool apply(Ring const& ring, Extremes& extremes, Intruders& intruders)
    {
        std::size_t const n = geofeatures_boost::size(ring);
        if (n < 3)
        {
            return false;
        }

        // Get all maxima, usually one. In case of self-tangencies, or self-crossings,
        // the max might be is not valid. A valid max should make a right turn
        range_iterator max_it = geofeatures_boost::begin(ring);
        compare<Dimension> smaller;
        for (range_iterator it = max_it + 1; it != geofeatures_boost::end(ring); ++it)
        {
            if (smaller(*max_it, *it) && right_turn(ring, it))
            {
                max_it = it;
            }
        }

        if (max_it == geofeatures_boost::end(ring))
        {
            return false;
        }

        typename std::iterator_traits<range_iterator>::difference_type const
            index = std::distance(geofeatures_boost::begin(ring), max_it);
//std::cout << "Extreme point lies at " << index << " having " << geometry::wkt(*max_it) << std::endl;

        geometry::ever_circling_range_iterator<Ring const> left(ring);
        geometry::ever_circling_range_iterator<Ring const> right(ring);
        left += index;
        right += index;

        // Collect all points (often 3) in a temporary vector
        std::vector<point_type> points;
        points.reserve(3);
        if (! collect(ring, *max_it, points, left, right))
        {
            return false;
        }

//std::cout << "Built vector of " << points.size() << std::endl;

        coordinate_type const front_value = geometry::get<Dimension>(points.front());
        coordinate_type const back_value = geometry::get<Dimension>(points.back());
        coordinate_type const base_value = (std::max)(front_value, back_value);
        if (front_value < back_value)
        {
            move_along_vector<Dimension>(points.front(), *(points.begin() + 1), base_value);
        }
        else
        {
            move_along_vector<Dimension>(points.back(), *(points.rbegin() + 1), base_value);
        }

        std::copy(points.begin(), points.end(), std::back_inserter(extremes));

        get_intruders(ring, left, right, extremes, intruders);

        return true;
    }
};

}} // namespace detail::extreme_points
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry,
    std::size_t Dimension,
    typename GeometryTag = typename tag<Geometry>::type
>
struct extreme_points
{};

template<typename Ring, std::size_t Dimension>
struct extreme_points<Ring, Dimension, ring_tag>
    : detail::extreme_points::extreme_points_on_ring<Ring, Dimension>
{};

template<typename Polygon, std::size_t Dimension>
struct extreme_points<Polygon, Dimension, polygon_tag>
{
    template <typename Extremes, typename Intruders>
    static inline bool apply(Polygon const& polygon, Extremes& extremes, Intruders& intruders)
    {
        typedef typename geometry::ring_type<Polygon>::type ring_type;
        typedef detail::extreme_points::extreme_points_on_ring
            <
                ring_type, Dimension
            > ring_implementation;

        if (! ring_implementation::apply(geometry::exterior_ring(polygon), extremes, intruders))
        {
            return false;
        }

        // For a polygon, its interior rings can contain intruders
        typename interior_return_type<Polygon const>::type
            rings = interior_rings(polygon);
        for (typename detail::interior_iterator<Polygon const>::type
                it = geofeatures_boost::begin(rings); it != geofeatures_boost::end(rings); ++it)
        {
            ring_implementation::get_intruders(*it, extremes,  intruders);
        }

        return true;
    }
};

template<typename Box>
struct extreme_points<Box, 1, box_tag>
{
    template <typename Extremes, typename Intruders>
    static inline bool apply(Box const& box, Extremes& extremes, Intruders& )
    {
        extremes.resize(4);
        geometry::detail::assign_box_corners_oriented<false>(box, extremes);
        // ll,ul,ur,lr, contains too exactly the right info
        return true;
    }
};

template<typename Box>
struct extreme_points<Box, 0, box_tag>
{
    template <typename Extremes, typename Intruders>
    static inline bool apply(Box const& box, Extremes& extremes, Intruders& )
    {
        extremes.resize(4);
        geometry::detail::assign_box_corners_oriented<false>(box, extremes);
        // ll,ul,ur,lr, rotate one to start with UL and end with LL
        std::rotate(extremes.begin(), extremes.begin() + 1, extremes.end());
        return true;
    }
};

template<typename MultiPolygon, std::size_t Dimension>
struct extreme_points<MultiPolygon, Dimension, multi_polygon_tag>
{
    template <typename Extremes, typename Intruders>
    static inline bool apply(MultiPolygon const& multi, Extremes& extremes, Intruders& intruders)
    {
        // Get one for the very first polygon, that is (for the moment) enough.
        // It is not guaranteed the "extreme" then, but for the current purpose
        // (point_on_surface) it can just be this point.
        if (geofeatures_boost::size(multi) >= 1)
        {
            return extreme_points
                <
                    typename geofeatures_boost::range_value<MultiPolygon const>::type,
                    Dimension,
                    polygon_tag
                >::apply(*geofeatures_boost::begin(multi), extremes, intruders);
        }

        return false;
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief Returns extreme points (for Edge=1 in dimension 1, so the top,
       for Edge=0 in dimension 0, the right side)
\note We could specify a strategy (less/greater) to get bottom/left side too. However, until now we don't need that.
 */
template <std::size_t Edge, typename Geometry, typename Extremes, typename Intruders>
inline bool extreme_points(Geometry const& geometry, Extremes& extremes, Intruders& intruders)
{
    concept::check<Geometry const>();

    // Extremes is not required to follow a geometry concept (but it should support an output iterator),
    // but its elements should fulfil the point-concept
    concept::check<typename geofeatures_boost::range_value<Extremes>::type>();

    // Intruders should contain collections which value type is point-concept
    // Extremes might be anything (supporting an output iterator), but its elements should fulfil the point-concept
    concept::check
        <
            typename geofeatures_boost::range_value
                <
                    typename geofeatures_boost::range_value<Intruders>::type
                >::type
            const
        >();

    return dispatch::extreme_points<Geometry, Edge>::apply(geometry, extremes, intruders);
}

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXTREME_POINTS_HPP


/* boosted_stumps.cc
   Jeremy Barnes, 6 June 2003
   Copyright (c) 2003 Jeremy Barnes.  All rights reserved.
   $Source$

   Implementation of boosted decision stumps.
*/

#include "boosted_stumps.h"
#include "classifier_persist_impl.h"
#include "jml/math/xdiv.h"
#include "jml/arch/simd_vector.h"
#include "boosted_stumps_impl.h"
#include "stump_predict.h"
#include "jml/utils/environment.h"
#include <boost/timer.hpp>
#include <boost/progress.hpp>
#include "jml/utils/floating_point.h"
#include "jml/utils/vector_utils.h"
#include "boosting_core.h"
#include "boosting_training.h"
#include "config_impl.h"
#include "binary_symmetric.h"
#include "jml/utils/worker_task.h"
#include "jml/utils/guard.h"
#include <iostream>
#include <boost/bind.hpp>

using namespace std;
using namespace DB;

namespace ML {

namespace {

Env_Option<bool> profile("PROFILE_BOOSTED_STUMPS", false);

double t_train = 0.0, t_update = 0.0, t_predict = 0.0, t_accuracy = 0.0;

struct Stats {
    ~Stats()
    {
        if (profile) {
            cerr << "Boosted stumps profile: " << endl;
            cerr << "  train:          " << t_train       << "s" << endl;
            cerr << "  update:         " << t_update      << "s" << endl;
            cerr << "  predict:        " << t_predict     << "s" << endl;
            cerr << "  accuracy:       " << t_accuracy    << "s" << endl;
        }
    }
} stats;

class Function_Profiler {
public:
    boost::timer * t;
    double & var;
    Function_Profiler(double & var)
        : t(0), var(var)
    {
        if (profile) t = new boost::timer();
    }
    ~Function_Profiler()
    {
        if (t) var += t->elapsed();
        delete t;
    }
};

#define PROFILE_FUNCTION(var) \
Function_Profiler __profiler(var);

} // file scope

/*****************************************************************************/
/* BOOSTED_STUMPS                                                            */
/*****************************************************************************/

Boosted_Stumps::Boosted_Stumps()
{
}

Boosted_Stumps::
Boosted_Stumps(const std::shared_ptr<const Feature_Space> & feature_space,
               const Feature & predicted)
    : Classifier_Impl(feature_space, predicted)
{
    output = RAW;
}

Boosted_Stumps::
Boosted_Stumps(DB::Store_Reader & reader,
               const std::shared_ptr<const Feature_Space> & feature_space)
{
    this->reconstitute(reader, feature_space);
}

Boosted_Stumps::
Boosted_Stumps(const std::shared_ptr<const Feature_Space> & feature_space,
               const Feature & predicted,
               size_t label_count)
    : Classifier_Impl(feature_space, predicted, label_count)
{
}

namespace {

/** Class for the results which updates a whole distribution at once. */
struct Results_Dist {
    Results_Dist(distribution<float> & results,
                 const Feature_Space & fs)
        : results(results), fs(fs) {}

    void operator () (const distribution<float> & dist, float weight,
                      const Feature & feature) const
    {
        //cerr << "feature = " << fs.print(feature) << endl;
        //cerr << "  adding " << weight << " * " << dist;
        results += weight * dist;
        //cerr << "  result now " << results << endl;
    }

    distribution<float> & results;
    const Feature_Space & fs;
};

/** Class for the results which updates a single distribution at once. */
struct Results_Single {
    Results_Single(float & result, int label)
        : result(result), label(label) {}

    JML_ALWAYS_INLINE
    void operator () (const distribution<float> & dist, float weight,
                      const Feature & feature) const
    {
        result += weight * dist[label];
    }

    float & result;
    int label;

};

} // file scope

distribution<float>
Boosted_Stumps::
predict(const Feature_Set & features) const
{
    PROFILE_FUNCTION(t_predict);
    distribution<float> result(label_count());
    if (bias.size()) result += bias;
    predict_core(features, Results_Dist(result, *feature_space()));

    for (unsigned i = 0;  i < result.size();  ++i) {
        if (!finite(result[i])) {
            cerr << "result = " << result << endl;
            throw Exception("Boosted_Stumps::predict(): non-finite result");
        }
    }

    //cerr << "result = " << result << endl;

    //result =- result.max();

    //result = exp(result);
    //result.normalize();
    //result -= 0.5;

    double total = 0.0;

    if (output == LOGIT || output == LOGIT_NORM) {
        for (unsigned i = 0;  i < result.size();  ++i) {
            /* Avoid an overflow from the exp. */
            if (result[i] > fp_traits<float>::max_exp_arg * 0.9)
                result[i] = fp_traits<float>::max_exp_arg * 0.9;
            double e = exp(result[i]);
            double x = e / (e + (1.0 / e));
            total += x;
            result[i] = x;
        }
        if (output == LOGIT_NORM) {
            if ((float)total == 0.0F) {
                cerr << "warning: boosted stumps says no results are correct"
                     << endl;
                result.fill(1);  // assign all elements
                result.normalize();
            }
            else result /= total;
        }
    }

    for (unsigned i = 0;  i < result.size();  ++i) {
        if (!finite(result[i])) {
            distribution<float> result2(label_count());
            if (bias.size()) result2 += bias;
            predict_core(features, Results_Dist(result2, *feature_space()));

            cerr << "result = " << result << endl;
            cerr << "output of predict_core = " << result2 << endl;
            cerr << "total = " << total << endl;

            for (unsigned i = 0;  i < result2.size();  ++i) {
                /* Avoid an overflow from the exp. */
                if (result2[i] > fp_traits<float>::max_exp_arg * 0.9)
                    result2[i] = fp_traits<float>::max_exp_arg * 0.9;
                double e = exp(result2[i]);
                double x = e / (e + (1.0 / e));
                total += x;
                result[i] = x;
                cerr << "  i = " << i << " e = " << e << " x = " << x
                     << endl;
            }

            cerr << "after logit: result = " << result2 << endl;

            throw Exception("non-finite result");
        }
    }

    return result;
}

float
Boosted_Stumps::
predict(int label, const Feature_Set & features) const
{
    PROFILE_FUNCTION(t_predict);
    if (label < 0 || label >= label_count())
        throw Exception(format("Boosted_Stumps::predict(int, Feature_Set): "
                               "Attempt to predict label %d with label_count "
                               " %zd", label, label_count()));

    if (output == LOGIT_NORM) {
        /* Need to predict all, so we know how to normalize.  Could be much
           slower. */
        return predict(features)[label];
    }

    float result = 0.0;
    if (bias.size()) result += bias[label];
    predict_core(features, Results_Single(result, label));
    if (output == LOGIT) {
        double e = exp(result);
        result = e / (e + 1.0 / e);
    }
    return result;
}

Boosted_Stumps::iterator Boosted_Stumps::
insert(const Stump & stump, float weight)
{
    if (stump.split.feature() == MISSING_FEATURE) return end();

    iterator it = find(stump.split);
    if (it == end())
        return iterator
            (stumps.insert(make_pair(stump.split,
                                     stump.scaled(weight))).first);
    else it->merge(stump, weight);
    return it;
}

void Boosted_Stumps::insert(const std::vector<Stump> & stumps)
{
    distribution<float> scales(stumps.size(), 1.0 / stumps.size());
    insert(stumps, scales);
}

void Boosted_Stumps::
insert(const std::vector<Stump> & stumps, const distribution<float> & scale)
{
    if (scale.size() != stumps.size())
        throw Exception(format("Boosted_Stumps::insert(): stumps.size() = %zd"
                               " != scale.size() = %zd", stumps.size(),
                               scale.size()));
    for (unsigned i = 0;  i < stumps.size();  ++i)
        insert(stumps[i], scale[i]);
}

#if 0

namespace {

struct Accuracy_Job_Info {
    const Training_Data & data;
    const distribution<float> & example_weights;
    const Boosted_Stumps & stumps;
    const Optimization_Info & opt_info;
    boost::multi_array<float, 2> & output;
    bool bin_sym;

    Lock lock;
    double & correct;
    double & margin;

    Accuracy_Job_Info(const Training_Data & data,
                      const distribution<float> & example_weights,
                      const Boosted_Stumps & stumps,
                      const Optimization_Info & opt_info,
                      boost::multi_array<float, 2> & output,
                      bool bin_sym,
                      double & correct, double & margin)
        : data(data), example_weights(example_weights),
          stumps(stumps), opt_info(opt_info),
          output(output), bin_sym(bin_sym), correct(correct), margin(margin)
    {
    }

    void calc(unsigned start_x, unsigned end_x)
    {
        double sub_correct = 0.0, sub_margin = 0.0;

        int nl = output.shape()[1];

        const std::vector<Label> & labels
            = data.index().labels(stumps.predicted());

        if (bin_sym) {
            for (Boosted_Stumps::const_iterator it = stumps.begin();
                 it != stumps.end();  ++it) {

                typedef Binsym_Updater<Boosting_Predict> Updater;
                typedef Update_Weights<Updater> Update;
                Updater updater;
                Update update(updater);

                update(*it, opt_info, 1.0, output, data, start_x, end_x);
            }

            /* Now for the scoring. */
            typedef Binsym_Scorer Scorer;
            Scorer scorer;
            if (example_weights.empty()) {
                for (unsigned x = start_x;  x < end_x;  ++x)
                    sub_correct += scorer(labels[x], &output[x][0],
                                          &output[x][0]);
            }
            else {
                for (unsigned x = start_x;  x < end_x;  ++x)
                    sub_correct += scorer(labels[x], &output[x][0],
                                          &output[x][0] + 1)
                        * example_weights[x];
            }
        }
        else {
            for (Boosted_Stumps::const_iterator it = stumps.begin();
                 it != stumps.end();  ++it) {

                typedef Normal_Updater<Boosting_Predict> Updater;
                typedef Update_Weights<Updater> Update;
                Updater updater(nl);
                Update update(updater);

                update(*it, opt_info, 1.0, output, data, start_x, end_x);
            }

            /* Now for the scoring. */
            typedef Normal_Scorer Scorer;
            Scorer scorer;
            if (example_weights.empty()) {
                for (unsigned x = start_x;  x < end_x;  ++x)
                    sub_correct += scorer(labels[x], &output[x][0],
                                          &output[x][0]);
            }
            else {
                for (unsigned x = start_x;  x < end_x;  ++x)
                    sub_correct += scorer(labels[x], &output[x][0],
                                          &output[x][0] + 1)
                        * example_weights[x];
            }
        }

        Guard guard(lock);
        correct += sub_correct;
    }
};

struct Accuracy_Job {
    Accuracy_Job(Accuracy_Job_Info & info,
                 int x_start, int x_end)
        : info(info), x_start(x_start), x_end(x_end)
    {
    }

    Accuracy_Job_Info & info;
    int x_start, x_end;

    void operator () () const
    {
        info.calc(x_start, x_end);
    }
};

} // file scope

std::pair<float, float>
Boosted_Stumps::
accuracy(const Training_Data & data,
         const distribution<float> & example_weights,
         const Optimization_Info * opt_info_ptr) const
{
    PROFILE_FUNCTION(t_accuracy);
    unsigned nx = data.example_count();
    unsigned nl = label_count();

    boost::multi_array<float, 2> scores(boost::extents[nx][nl]);

    bool bin_sym = convert_bin_sym(scores, data, predicted_, all_features());

    double correct = 0.0, margin = 0.0;

    Optimization_Info new_opt_info;
    const Optimization_Info & opt_info
        = (opt_info_ptr ? *opt_info_ptr : new_opt_info);

    Accuracy_Job_Info info(data, example_weights, *this, opt_info,
                           scores, bin_sym,
                           correct, margin);

    static Worker_Task & worker = Worker_Task::instance(num_threads() - 1);

    int group;
    {
        int parent = -1;  // no parent group
        group = worker.get_group(NO_JOB,
                                 format("Boosted_Stumps::accuracy under %d", parent),
                                 parent);
        Call_Guard guard(boost::bind(&Worker_Task::unlock_group,
                                     boost::ref(worker),
                                     group));

        unsigned job_ex = 2048 / scores.shape()[1];

        for (unsigned x = 0;  x < data.example_count();  x += job_ex) {
            unsigned end = std::min(x + job_ex, nx);
            worker.add(Accuracy_Job(info, x, end),
                       format("Boosted_Stumps::accuracy() job %d-%d under %d",
                              x, end, group),
                       group);
        }
    }

    worker.run_until_finished(group);

    double total
        = (example_weights.empty() ? (float)nx : example_weights.total());

    return make_pair(correct / total, margin / total);
}

#else
std::pair<float, float>
Boosted_Stumps::
accuracy(const Training_Data & data,
         const distribution<float> & example_weights,
         const Optimization_Info * opt_info_ptr) const
{
    return Classifier_Impl::accuracy(data, example_weights, opt_info_ptr);
}
#endif

std::string Boosted_Stumps::print() const
{
    string result;
    for (const_iterator it = begin();  it != end();  ++it) {
        result += it->print();
        result += '\n';
    }
    return result;
}

std::vector<ML::Feature> Boosted_Stumps::all_features() const
{
    std::vector<ML::Feature> result;
    for (stumps_type::const_iterator it = stumps.begin();
         it != stumps.end();  ++it)
        result.push_back(it->first.feature());
    make_vector_set(result);
    return result;
}

Output_Encoding
Boosted_Stumps::
output_encoding() const
{
    switch (output) {
    case RAW: return OE_PM_INF;
    case LOGIT:
    case LOGIT_NORM:
        return OE_PROB;
    default:
        throw Exception("Boosted_Stumps::output_encoding(): uknown encoding");
    }
}

void Boosted_Stumps::calc_sum_missing()
{
    distribution<double> totals(label_count());
    for (const_iterator it = begin();  it != end();  ++it) {
        distribution<double> this_val(it->action.pred_missing.begin(),
                                      it->action.pred_missing.end());
        totals += this_val;
    }

    sum_missing = distribution<float>(totals.begin(), totals.end());
}

namespace {

static const std::string BOOSTED_STUMPS_MAGIC = "BOOSTED_STUMPS";
static const compact_size_t BOOSTED_STUMPS_VERSION = 4;

void serialize_dist(const distribution<float> & dist,
                    DB::Store_Writer & store)
{
    bool non_zero = false;
    for (unsigned i = 0;  i < dist.size();  ++i) {
        //if (dist[i] != 0.0) { non_zero = true;  break; }
        if (abs(dist[i]) > 1e-10) { non_zero = true;  break; }
    }

    if (non_zero) {
        bool pos_neg = (dist.size() == 2 && (abs(dist[0] + dist[1]) < 1e-10));
        if (pos_neg)
            store << compact_const(1) << dist[0];
        else store << dist;
    }
    else store << compact_const(0);
}

void reconstitute_dist(distribution<float> & dist,
                       DB::Store_Reader & store,
                       size_t nl)
{
    dist.clear();
    compact_size_t size(store);

    if (size == 0) dist.resize(nl);
    else if (size == 1) {
        dist.resize(2);
        store >> dist[0];
        dist[1] = -dist[0];
    }
    else {
        if (size != nl)
            throw Exception
                (format("reconstituting Boosted_Stumps dist of wrong size: "
                        "is %zd, should be %zd", size.size_, nl));

        dist.resize(size);
        for (unsigned i = 0;  i < nl;  ++i)
            store >> dist[i];
    }
}

} // file scope

COMPACT_PERSISTENT_ENUM_DECL(Boosted_Stumps::Output);
COMPACT_PERSISTENT_ENUM_IMPL(Boosted_Stumps::Output);

void Boosted_Stumps::serialize(DB::Store_Writer & store) const
{
    store << BOOSTED_STUMPS_MAGIC << BOOSTED_STUMPS_VERSION
          << compact_const(label_count());
    feature_space()->serialize(store, predicted_);
    store << output << bias;
    store << compact_const(stumps.size());

    for (stumps_type::const_iterator it = stumps.begin();
         it != stumps.end();  /* no inc */) {

        const Feature & feat = it->first.feature();
        feature_space()->serialize(store, feat);

        /* Work out how many entries we have with this feature. */
        int nf = 0;
        stumps_type::const_iterator first = it;
        while (it != stumps.end() && it->first.feature() == feat) {
            ++nf;
            ++it;
        }

        store << compact_const(nf);
        std::swap(it, first);

        for (unsigned i = 0;  i < nf;  ++i,++it) {
            const Stump & stump = it->second;
            stump.serialize_lw(store);
        }

        if (it != first)
            throw Exception("Boosted_Stumps::serialize(): logic error");
    }
}

void Boosted_Stumps::
reconstitute(DB::Store_Reader & store,
             const std::shared_ptr<const Feature_Space> & features)
{
    /* Implement the strong exception guarantee, except for the store. */
    string magic;
    compact_size_t version;
    store >> magic >> version;
    if (magic != BOOSTED_STUMPS_MAGIC)
        throw Exception("Attempt to reconstitute \"" + magic
                                + "\" with boosted stumps reconstitutor");
    if (version > BOOSTED_STUMPS_VERSION)
        throw Exception(format("Attemp to reconstitute boosted stumps "
                               "version %zd, only <= %zd supported",
                               version.size_,
                               BOOSTED_STUMPS_VERSION.size_));

    compact_size_t label_count(store);

    predicted_ = MISSING_FEATURE;
    if (version >= 4) { // added in version 4
        features->reconstitute(store, predicted_);
    }

    Boosted_Stumps new_me(features, predicted_, label_count);

    if (version >= 1) {  // added in version 1
        store >> new_me.output;
    }
    else new_me.output = RAW;

    if (version >= 2) // added in version 2
        store >> new_me.bias;
    else new_me.bias.clear();

    //ofstream dump("stump-load-dump.txt");

    if (version >= 3) {
        /* Load compact version... */
        compact_size_t stump_count(store);
        int ns = 0;

        while (ns < stump_count) {
            Feature feature;
            features->reconstitute(store, feature);
            compact_size_t nsf(store);  // number of stumps for this feature

            for (unsigned i = 0;  i < nsf;  ++i) {
                Stump stump(features, predicted_, label_count);
                stump.reconstitute_lw(store, feature);
                new_me.insert(stump);
                ++ns;

#if 0
                dump << "stump " << ns << ": " << features->print(stump.feature)
                     << " " << stump.arg << " " << stump.pred_true << " "
                     << stump.pred_false << " " << stump.pred_missing << endl;
#endif
            }
        }

        if (ns != stump_count)
            throw Exception("Boosted_Stumps::reconstitute(): logic error");

        calc_sum_missing();
    }
    else {
        compact_size_t stump_count(store);

        /* Load non-compact versions... */
        for (unsigned i = 0;  i < stump_count;  ++i) {
            Stump stump(store, features);
#if 0
            dump << "stump " << i << ": " << features->print(stump.feature)
                 << " " << stump.arg << " " << stump.pred_true << " "
                 << stump.pred_false << " " << stump.pred_missing << endl;
#endif

            new_me.insert(stump);
        }

        calc_sum_missing();
    }

    swap(new_me);

    //cerr << "predicted_ = " << predicted_ << endl;
}

Boosted_Stumps * Boosted_Stumps::make_copy() const
{
    return new Boosted_Stumps(*this);
}

void Boosted_Stumps::combine(const Boosted_Stumps & with, float scale)
{
    /* Check they are compatible. */
    if (with.label_count() != label_count())
        throw Exception("Attempt to combine two boosted stumps with different "
                        "label counts.");
    if (with.feature_space().get() != feature_space().get())
        throw Exception("Attempt to combine two boosted stumps with different "
                        "feature spaces.");

    for (const_iterator it = with.begin();  it != with.end();  ++it)
        insert(*it, scale);
}

bool
Boosted_Stumps::
merge_into(const Classifier_Impl & other, float weight)
{
    /* If the other is a stump, we merge it. */
    const Stump * as_stump = dynamic_cast<const Stump *>(&other);
    if (as_stump) {
        insert(*as_stump, weight);
        return true;
    }
    const Boosted_Stumps * as_stumps
        = dynamic_cast<const Boosted_Stumps *>(&other);
    if (as_stumps) {
        combine(*as_stumps, weight);
        return true;
    }

    return false;
}

Classifier_Impl *
Boosted_Stumps::
merge(const Classifier_Impl & other, float weight) const
{
    /* If the other is a boosted stumps or a stump, we can merge it. */

    const Stump * as_stump = dynamic_cast<const Stump *>(&other);
    if (as_stump) {
        unique_ptr<Boosted_Stumps> me_copy(make_copy());
        me_copy->insert(*as_stump, weight);
        return me_copy.release();
    }

    const Boosted_Stumps * as_stumps
        = dynamic_cast<const Boosted_Stumps *>(&other);
    if (as_stumps) {
        unique_ptr<Boosted_Stumps> me_copy(make_copy());
        me_copy->combine(*as_stumps, weight);
        return me_copy.release();
    }

    return 0;
}

/*****************************************************************************/
/* REGISTRATION                                                              */
/*****************************************************************************/

namespace {

Register_Factory<Classifier_Impl, Boosted_Stumps>
    STUMP_REGISTER("BOOSTED_STUMPS");

} // file scope
} // namespace ML

ENUM_INFO_NAMESPACE

const Enum_Opt<ML::Boosted_Stumps::Output>
Enum_Info<ML::Boosted_Stumps::Output>::OPT[3] = {
    { "raw",         ML::Boosted_Stumps::RAW        },
    { "logit",       ML::Boosted_Stumps::LOGIT      },
    { "logit_norm",  ML::Boosted_Stumps::LOGIT_NORM } };

const char * Enum_Info<ML::Boosted_Stumps::Output>::NAME
    = "Boosted_Stumps::Output";

END_ENUM_INFO_NAMESPACE