//---------------------------------------------------------------------------//
//!
//! \file   Utility_Measurement_def.hpp
//! \author Alex Robinson
//! \brief  The measurement class declaration.
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_MEASUREMENT_DEF_HPP
#define UTILITY_MEASUREMENT_DEF_HPP

// Std Lib Includes
#include <cmath>
#include <functional>

// Boost Includes
#include <boost/io/ios_state.hpp>

namespace Utility{

// Constructor
template<typename T>
Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::Measurement(
                                                 const ValueType& value,
                                                 const ValueType& uncertainty )
  : d_value( value ),
    d_uncertainty( uncertainty )
{
  // Make sure the value is valid
  testPrecondition( !Utility::isnaninf( value ) );
  // Make sure the uncertainty is valid
  testPrecondition( !Utility::isnaninf( uncertainty ) );
  testPrecondition( uncertainty >= 0.0 );
}

// Copy constructor
template<typename T>
Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::Measurement( const Measurement<T>& other_measurement )
  : d_value( other_measurement.d_value ),
    d_uncertainty( other_measurement.d_uncertainty )
{
  // Make sure the other measurement is valid
  testPrecondition( !Utility::isnaninf( other_measurement.d_value ) );
  testPrecondition( !Utility::isnaninf( other_measurement.d_uncertainty ) );
  testPrecondition( other_measurement.d_uncertainty >= 0.0 );
}

// Method for placing the object in an output stream
template<typename T>
inline void Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::toStream( std::ostream& os ) const
{
  os << Utility::toString( d_value ) << " +/- "
     << Utility::toString( d_uncertainty );
}

// Return the value of the measurement
template<typename T>
inline auto Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::getValue() const -> const ValueType&
{
  return d_value;
}

// Return the uncertainty of the measurement
template<typename T>
inline auto Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::getUncertainty() const -> const ValueType&
{
  return d_uncertainty;
}

// Return the relative uncertainty of the measurement
template<typename T>
inline auto Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::getRelativeUncertainty() const -> ValueType
{
  if( d_value != 0.0 )
    return d_uncertainty/fabs(d_value);
  else
    return 0.0;
}

// Return the lower bound of the measurement
template<typename T>
inline auto Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::getLowerBound() const -> ValueType
{
  return d_value - d_uncertainty;
}

// Return the upper bound of the measurement
template<typename T>
inline auto Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::getUpperBound() const -> ValueType
{
  return d_value + d_uncertainty;
}

// In-place addition operator
template<typename T>
inline Measurement<T>& Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::operator+=( const ValueType& value )
{
  // Make sure the value is valid
  testPrecondition( !Utility::isnaninf( value ) );

  d_value += value;

  return *this;
}

// In-place addition operator
template<typename T>
inline Measurement<T>& Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::operator+=( const Measurement<T>& other_measurement )
{
  // Make sure the other measurement is valid
  testPrecondition( !Utility::isnaninf( other_measurement.d_value ) );
  testPrecondition( !Utility::isnaninf( other_measurement.d_uncertainty ) );
  testPrecondition( other_measurement.d_uncertainty >= 0.0 );

  d_value += other_measurement.d_value;

  // Propagate the uncertainty of the measurements
  d_uncertainty = std::sqrt( d_uncertainty*d_uncertainty +
			     other_measurement.d_uncertainty*
			     other_measurement.d_uncertainty );

  return *this;
}

// In-place subtraction operator
template<typename T>
inline Measurement<T>& Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::operator-=( const ValueType& value )
{
  // Make sure the value is valid
  testPrecondition( !Utility::isnaninf( value ) );

  d_value -= value;

  return *this;
}

// In-place subtraction operator
template<typename T>
inline Measurement<T>& Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::operator-=( const Measurement<T>& other_measurement )
{
  // Make sure the other measurement is valid
  testPrecondition( !Utility::isnaninf( other_measurement.d_value ) );
  testPrecondition( !Utility::isnaninf( other_measurement.d_uncertainty ) );
  testPrecondition( other_measurement.d_uncertainty >= 0.0 );

  d_value -= other_measurement.d_value;

  // Propagate the uncertainty of the measurements
  d_uncertainty = std::sqrt( d_uncertainty*d_uncertainty +
			     other_measurement.d_uncertainty*
			     other_measurement.d_uncertainty );

  return *this;
}

// In-place multiplication operator
template<typename T>
inline Measurement<T>& Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::operator*=( const ValueType& value )
{
  // Make sure the value is valid
  testPrecondition( !Utility::isnaninf( value ) );

  d_value *= value;

  d_uncertainty *= fabs( value );

  return *this;
}

// In-place multiplication operator
template<typename T>
inline Measurement<T>& Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::operator*=( const Measurement<T>& other_measurement )
{
  // Make sure the other measurement is valid
  testPrecondition( !Utility::isnaninf( other_measurement.d_value ) );
  testPrecondition( !Utility::isnaninf( other_measurement.d_uncertainty ) );

  // Propagate the uncertainty of the measurements
  d_uncertainty = std::sqrt( d_uncertainty*d_uncertainty*
	       other_measurement.d_value*other_measurement.d_value +
	       other_measurement.d_uncertainty*other_measurement.d_uncertainty*
	       d_value*d_value );

  d_value *= other_measurement.d_value;

  return *this;
}

// In-place division operator
template<typename T>
inline Measurement<T>& Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::operator/=( const ValueType& value )
{
  // Make sure the value is valid
  testPrecondition( !Utility::isnaninf( value ) );
  testPrecondition( value != 0.0 );

  d_value /= value;

  d_uncertainty /= fabs( value );

  return *this;
}

// In-place division operator
template<typename T>
inline Measurement<T>& Measurement<T,typename std::enable_if<std::is_floating_point<T>::value>::type>::operator/=( const Measurement<T>& other_measurement )
{
  // Make sure the other measurement is valid
  testPrecondition( !Utility::isnaninf( other_measurement.d_value ) );
  testPrecondition( !Utility::isnaninf( other_measurement.d_uncertainty ) );
  testPrecondition( other_measurement.d_value != 0.0 );
  testPrecondition( other_measurement.d_uncertainty >= 0.0 );

  double other_value_sqr = other_measurement.d_value*other_measurement.d_value;

  double term_1 = d_uncertainty*d_uncertainty/other_value_sqr;

  double term_2 =
    other_measurement.d_uncertainty*other_measurement.d_uncertainty*
    d_value*d_value/(other_value_sqr*other_value_sqr);

  // Propagate the uncertainty of the measurements
  d_uncertainty = std::sqrt( term_1 + term_2 );

  d_value /= other_measurement.d_value;

  return *this;
}

namespace Details{

//! Calculate the propagated uncertainty of a measurement to a real power
template<typename T1, typename T2>
inline T1 calculatePropagatedUncertaintyFromRealPowerOp(
                                                const T1 value,
                                                const T1 value_to_exponent,
                                                const T1 uncertainty,
                                                const T2 exponent )
{ return std::fabs(exponent*value_to_exponent*(uncertainty/value)); }

//! The pow method implementation
template<typename T, typename ExponentType, typename BasePowMethod>
inline Utility::Measurement<T> powImpl( const Utility::Measurement<T>& x,
                                       const ExponentType exponent,
                                       BasePowMethod basePow )
{
  if( x.getValue() != Utility::QuantityTraits<T>::zero() )
  {
    const T new_value = basePow( x.getValue() );

    const T propagated_uncertainty =
      Utility::Details::calculatePropagatedUncertaintyFromRealPowerOp(
                                                            x.getValue(),
                                                            new_value,
                                                            x.getUncertainty(),
                                                            exponent );

    return Utility::Measurement<T>( new_value, propagated_uncertainty );
  }
  else if( x.getValue() == Utility::QuantityTraits<T>::zero() &&
           x.getUncertainty() > Utility::QuantityTraits<T>::zero() )
  {
    return Utility::Measurement<T>( Utility::QuantityTraits<T>::zero(),
                                    Utility::QuantityTraits<T>::inf() );
  }
  // quantity.getValue() == 0, quantity.getUncertainty() == 0
  else
  {
    return Utility::Measurement<T>( Utility::QuantityTraits<T>::zero(), Utility::QuantityTraits<T>::zero() );
  }
}

} // end Details namespace

} // end Utility namespace

namespace std{

// Overload of sqrt for a measurement
template<typename T>
inline Utility::Measurement<T> sqrt( const Utility::Measurement<T>& x )
{
  if( x.getValue() > Utility::QuantityTraits<T>::zero() )
  {
    const T new_value = std::sqrt( x.getValue() );

    const T propagated_uncertainty =
      Utility::Details::calculatePropagatedUncertaintyFromRealPowerOp(
                                                            x.getValue(),
                                                            new_value,
                                                            x.getUncertainty(),
                                                            0.5 );
    return Utility::Measurement<T>( new_value, propagated_uncertainty );
  }
  else if( x.getValue() == Utility::QuantityTraits<T>::zero() &&
           x.getUncertainty() > Utility::QuantityTraits<T>::zero() )
  {
    return Utility::Measurement<T>( Utility::QuantityTraits<T>::zero(),
                                    Utility::QuantityTraits<T>::inf() );
  }
  else if( x.getValue() == Utility::QuantityTraits<T>::zero() &&
           x.getUncertainty() == Utility::QuantityTraits<T>::zero() )
  {
    return Utility::Measurement<T>( Utility::QuantityTraits<T>::zero(), Utility::QuantityTraits<T>::zero() );
  }
  else
  {
    return Utility::Measurement<T>( Utility::QuantityTraits<T>::nan(), Utility::QuantityTraits<T>::nan() );
  }
}

// Overload of cbrt for a measurement
template<typename T>
inline Utility::Measurement<T> cbrt( const Utility::Measurement<T>& x )
{
  if( x.getValue() != Utility::QuantityTraits<T>::zero() )
  {
    const T new_value = std::cbrt( x.getValue() );

    const T propagated_uncertainty =
      Utility::Details::calculatePropagatedUncertaintyFromRealPowerOp(
                                                            x.getValue(),
                                                            new_value,
                                                            x.getUncertainty(),
                                                            1.0/3 );
    return Utility::Measurement<T>( new_value, propagated_uncertainty );
  }
  else if( x.getValue() == Utility::QuantityTraits<T>::zero() &&
           x.getUncertainty() > Utility::QuantityTraits<T>::zero() )
  {
    return Utility::Measurement<T>( Utility::QuantityTraits<T>::zero(),
                                    Utility::QuantityTraits<T>::inf() );
  }
  // quantity.getValue() == 0, quantity.getUncertainty() == 0
  else
  {
    return Utility::Measurement<T>( Utility::QuantityTraits<T>::zero(), Utility::QuantityTraits<T>::zero() );
  }
}

// Overload of pow for a measurement
template<typename T, typename ExponentType>
inline Utility::Measurement<T> pow( const Utility::Measurement<T>& x,
                                    const ExponentType exponent )
{
  return Utility::Details::powImpl( x, exponent, std::bind<T>( static_cast<T (*)(T,T)>(&std::pow<T>), std::placeholders::_1, static_cast<T>(exponent) ) );
}

} // end std namespace

namespace Utility{

// Partial specialization of Utility::QuantityTraits for Utility::Measurement
template<typename T>
struct QuantityTraits<Measurement<T>,typename std::enable_if<std::is_floating_point<T>::value>::type>
{
private:

  // The quantity traits for the raw type
  typedef QuantityTraits<T> RawQuantityTraits;

  // These quantity traits
  typedef QuantityTraits<Measurement<T>,typename std::enable_if<std::is_floating_point<T>::value>::type> ThisType;

public:

  //! The unit type
  typedef void UnitType;

  //! The raw quantity type
  typedef Measurement<T> RawType;

  //! The raw floating-point quantity type
  typedef Measurement<T> RawFloatingPointType;

  //! The real raw floating-point quantity type
  typedef Measurement<T> RealRawFloatingPointType;

  //! The quantity type
  typedef Measurement<T> QuantityType;

  //! The floating-point quantity type
  typedef Measurement<T> FloatingPointQuantityType;

  //! The real floating-point quantity type
  typedef Measurement<T> RealFloatingPointQuantityType;

  //! The quantity raised to power N/D type
  template<boost::units::integer_type N, boost::units::integer_type D = 1>
  struct GetQuantityToPowerType
  {
    typedef QuantityType type;
    typedef FloatingPointQuantityType AsFloatingPointType;
  };

  //! Used to check if the traits class for quantity type is specialized
  typedef typename RawQuantityTraits::is_specialized is_specialized;

  //! Used to check if the quantity is signed
  typedef typename RawQuantityTraits::is_signed is_signed;

  //! Used to check if the quantity is an integer type
  typedef typename RawQuantityTraits::is_integer is_integer;

  //! Used to check if the quantity is complex
  typedef typename RawQuantityTraits::is_complex is_complex;

  //! Used to check if the quantity uses exact representations
  typedef typename RawQuantityTraits::is_exact is_exact;

  //! Used to check if a quantity adheres to IEC-559/IEEE-754 standard
  typedef typename RawQuantityTraits::is_iec559 is_iec559;

  //! Used to check if the set of values represented by the quantity is finite
  typedef typename RawQuantityTraits::is_bounded is_bounded;

  //! Used to check if the quantity is modulo
  typedef typename RawQuantityTraits::is_modulo is_modulo;

  //! Used to check if the quantity has a representation for positive infinity
  typedef typename RawQuantityTraits::has_infinity has_infinity;

  //! Used to check if the quantity has a representation for quiet nan
  typedef typename RawQuantityTraits::has_quiet_nan has_quiet_nan;

  //! Used to check if the quantity has a representation for signaling nan
  typedef typename RawQuantityTraits::has_signaling_nan has_signaling_nan;

  //! Used to check if the quantity allows denormalized values
  typedef typename RawQuantityTraits::has_denorm has_denorm;

  //! Used to check if a loss of accuracy is detected as a denormalization loss instead of an inexact result
  typedef typename RawQuantityTraits::has_denorm_loss has_denorm_loss;

  //! Used to check if trapping is implemented for the quantity
  typedef typename RawQuantityTraits::traps traps;

  //! Used to check if tinyness is implemented before rounding
  typedef typename RawQuantityTraits::tinyness_before tinyness_before;

  //! Used to check the rounding style of floating-point quantities
  typedef typename RawQuantityTraits::round_style round_style;

    //! The number of non-sign bits
  typedef typename RawQuantityTraits::digits digits;

  //! The number of digits (in decimal base) that can be represented
  typedef typename RawQuantityTraits::digits10 digits10;

  //! The number of digits (in decimal base) required to ensure that values that differ are always differentiated
  typedef typename RawQuantityTraits::max_digits10 max_digits10;

  //! Base of the representation
  typedef typename RawQuantityTraits::radix radix;

  //! Min negative integer value such that radix raised to (this-1) generates a normalized floating-point quantity
  typedef typename RawQuantityTraits::min_exponent min_exponent;

  //! Min negative integer value such that 10 raised to that power generates a normalized floating-point quantity
  typedef typename RawQuantityTraits::min_exponent10 min_exponent10;

  //! One more than the largest integer power of the radix that is a valid finite floating-point value
  typedef typename RawQuantityTraits::max_exponent max_exponent;

  //! Max integer power of 10 that is a valid finite floating-point value
  typedef typename RawQuantityTraits::max_exponent10 max_exponent10;

  //! Get the zero quantity
  static inline QuantityType zero() noexcept
  { return QuantityType(RawQuantityTraits::zero(), RawQuantityTraits::zero()); }

  //! Get the one quantity
  static inline QuantityType one() noexcept
  { return QuantityType(RawQuantityTraits::one(), RawQuantityTraits::zero()); }

  //! Get the min quantity
  static inline QuantityType min() noexcept
  { return QuantityType(RawQuantityTraits::min(), RawQuantityTraits::zero()); }

  //! Get the min positive denormalized value
  static inline QuantityType denormMin() noexcept
  { return QuantityType(RawQuantityTraits::denormMin(), RawQuantityTraits::zero()); }

  //! Get the max quantity
  static inline QuantityType max() noexcept
  { return QuantityType(RawQuantityTraits::max(), RawQuantityTraits::zero()); }

  //! Get the lowest quantity
  static inline QuantityType lowest() noexcept
  { return QuantityType(RawQuantityTraits::lowest(), RawQuantityTraits::zero()); }

  //! Get the machine epsilon (only defined for floating-point types)
  static inline QuantityType epsilon() noexcept
  { return QuantityType(RawQuantityTraits::epsilon(), RawQuantityTraits::zero()); }

  //! Get the maximum rounding error (only defined for floating-point types)
  static inline QuantityType roundError() noexcept
  { return QuantityType(RawQuantityTraits::roundError(), RawQuantityTraits::zero()); }

  //! Get the inf quantity (only defined for floating-point types)
  static inline QuantityType inf() noexcept
  { return QuantityType(RawQuantityTraits::inf(), RawQuantityTraits::zero()); }

  //! Get the quiet nan quantity (only defined for floating-point types)
  static inline QuantityType nan() noexcept
  { return QuantityType(RawQuantityTraits::nan(), RawQuantityTraits::zero()); }

  //! Get the signaling nan quantity (only defined for floating-point types)
  static inline QuantityType signalingNan() noexcept
  { return QuantityType(RawQuantityTraits::signalingNan(), RawQuantityTraits::zero()); }

  //! Get the absolute value of a quantity
  static inline RealFloatingPointQuantityType abs( const QuantityType& a )
  { return RealFloatingPointQuantityType(RawQuantityTraits::abs(a.getValue()), a.getUncertainty()); }

  //! Get the conjugate of a quantity
  static inline FloatingPointQuantityType conj( const QuantityType& a )
  { return a; }

  //! Get the real part of the quantity
  static inline RealFloatingPointQuantityType real( const QuantityType& a )
  { return a; }

  //! Get the imaginary part of the quantity
  static inline RealFloatingPointQuantityType imag( const QuantityType& a )
  { return RealFloatingPointQuantityType(RawQuantityTraits::zero(),RawQuantityTraits::zero()); }

  //! Test if the quantity is a nan or inf
  static inline bool isnaninf( const QuantityType& a )
  {
    if( RawQuantityTraits::isnaninf( a.getValue() ) ||
        RawQuantityTraits::isnaninf( a.getUncertainty() ) )
      return true;

    return false;
  }

  //! Take the square root of a quantity (only one root will be calculated)
  static inline typename GetQuantityToPowerType<1,2>::AsFloatingPointType sqrt( const QuantityType& quantity )
  { return std::sqrt( quantity ); }

  //! Take the cube root of a quantity (only one root will be calculated)
  static inline typename GetQuantityToPowerType<1,3>::AsFloatingPointType cbrt( const QuantityType& quantity )
  { return std::cbrt( quantity ); }

  //! Take a quantity to the desired rational power
  template<boost::units::integer_type N, boost::units::integer_type D>
  static typename GetQuantityToPowerType<N,D>::AsFloatingPointType rpow( const QuantityType& quantity )
  {
    return Details::powImpl( quantity, static_cast<double>(N)/D, &RawQuantityTraits::template rpow<N,D> );
  }

  //! Initialize a quantity (potentially dangerous!)
  static inline QuantityType initializeQuantity( const RawType& raw_quantity ) noexcept
  { return raw_quantity; }

  //! Get the raw value of a quantity
  static inline const RawType& getRawQuantity( const QuantityType& quantity ) noexcept
  { return quantity; }

  //! Set the value of a quantity (potentially dangerous!)
  static inline void setQuantity( QuantityType& quantity,
				  const RawType& raw_quantity ) noexcept
  { quantity = raw_quantity; }
};

// Partial specialization of Utility::QuantityTraits for
// boost::units::quantity<Unit,Utility::Measurement> types
template<typename Unit, typename T>
struct QuantityTraits<boost::units::quantity<Unit,Measurement<T> >,typename std::enable_if<std::is_floating_point<T>::value>::type>
{
  // The quantity traits for the raw type
  typedef QuantityTraits<Measurement<T> > RawQuantityTraits;

  // These quantity traits
  typedef QuantityTraits<boost::units::quantity<Unit,Measurement<T> >,typename std::enable_if<std::is_floating_point<T>::value>::type> ThisType;

public:

  //! The unit type
  typedef Unit UnitType;

  //! The raw quantity type
  typedef Measurement<T> RawType;

  //! The raw floating-point quantity type
  typedef Measurement<T> RawFloatingPointType;

  //! The real raw floating-point quantity type
  typedef Measurement<T> RealRawFloatingPointType;

  //! The quantity type
  typedef boost::units::quantity<Unit,Measurement<T> > QuantityType;

  //! The floating-point quantity type
  typedef boost::units::quantity<Unit,Measurement<T> > FloatingPointQuantityType;

  //! The real floating-point quantity type
  typedef boost::units::quantity<Unit,Measurement<T> > RealFloatingPointQuantityType;

  //! The quantity raised to power N/D type
  template<boost::units::integer_type N, boost::units::integer_type D = 1>
  struct GetQuantityToPowerType
  {
    typedef typename Details::QuantityToPowerTypeHelper<N,D,QuantityType>::QuantityToRpowType type;
    typedef typename Details::QuantityToPowerTypeHelper<N,D,FloatingPointQuantityType>::QuantityToRpowType AsFloatingPointType;
  };

  //! Used to check if the traits class for quantity type is specialized
  typedef typename RawQuantityTraits::is_specialized is_specialized;

  //! Used to check if the quantity is signed
  typedef typename RawQuantityTraits::is_signed is_signed;

  //! Used to check if the quantity is an integer type
  typedef typename RawQuantityTraits::is_integer is_integer;

  //! Used to check if the quantity is complex
  typedef typename RawQuantityTraits::is_complex is_complex;

  //! Used to check if the quantity uses exact representations
  typedef typename RawQuantityTraits::is_exact is_exact;

  //! Used to check if a quantity adheres to IEC-559/IEEE-754 standard
  typedef typename RawQuantityTraits::is_iec559 is_iec559;

  //! Used to check if the set of values represented by the quantity is finite
  typedef typename RawQuantityTraits::is_bounded is_bounded;

  //! Used to check if the quantity is modulo
  typedef typename RawQuantityTraits::is_modulo is_modulo;

  //! Used to check if the quantity has a representation for positive infinity
  typedef typename RawQuantityTraits::has_infinity has_infinity;

  //! Used to check if the quantity has a representation for quiet nan
  typedef typename RawQuantityTraits::has_quiet_nan has_quiet_nan;

  //! Used to check if the quantity has a representation for signaling nan
  typedef typename RawQuantityTraits::has_signaling_nan has_signaling_nan;

  //! Used to check if the quantity allows denormalized values
  typedef typename RawQuantityTraits::has_denorm has_denorm;

  //! Used to check if a loss of accuracy is detected as a denormalization loss instead of an inexact result
  typedef typename RawQuantityTraits::has_denorm_loss has_denorm_loss;

  //! Used to check if trapping is implemented for the quantity
  typedef typename RawQuantityTraits::traps traps;

  //! Used to check if tinyness is implemented before rounding
  typedef typename RawQuantityTraits::tinyness_before tinyness_before;

  //! Used to check the rounding style of floating-point quantities
  typedef typename RawQuantityTraits::round_style round_style;

    //! The number of non-sign bits
  typedef typename RawQuantityTraits::digits digits;

  //! The number of digits (in decimal base) that can be represented
  typedef typename RawQuantityTraits::digits10 digits10;

  //! The number of digits (in decimal base) required to ensure that values that differ are always differentiated
  typedef typename RawQuantityTraits::max_digits10 max_digits10;

  //! Base of the representation
  typedef typename RawQuantityTraits::radix radix;

  //! Min negative integer value such that radix raised to (this-1) generates a normalized floating-point quantity
  typedef typename RawQuantityTraits::min_exponent min_exponent;

  //! Min negative integer value such that 10 raised to that power generates a normalized floating-point quantity
  typedef typename RawQuantityTraits::min_exponent10 min_exponent10;

  //! One more than the largest integer power of the radix that is a valid finite floating-point value
  typedef typename RawQuantityTraits::max_exponent max_exponent;

  //! Max integer power of 10 that is a valid finite floating-point value
  typedef typename RawQuantityTraits::max_exponent10 max_exponent10;

  //! Get the zero quantity
  static inline QuantityType zero() noexcept
  { return QuantityType::from_value( RawQuantityTraits::zero() ); }

  //! Get the one quantity
  static inline QuantityType one() noexcept
  { return QuantityType::from_value( RawQuantityTraits::one() ); }

  //! Get the min quantity
  static inline QuantityType min() noexcept
  { return QuantityType::from_value( RawQuantityTraits::min() ); }

  //! Get the min positive denormalized value
  static inline QuantityType denormMin() noexcept
  { return QuantityType::from_value( RawQuantityTraits::denormMin() ); }

  //! Get the max quantity
  static inline QuantityType max() noexcept
  { return QuantityType::from_value( RawQuantityTraits::max() ); }

  //! Get the lowest quantity
  static inline QuantityType lowest() noexcept
  { return QuantityType::from_value( RawQuantityTraits::lowest() ); }

  //! Get the machine epsilon (only defined for floating-point types)
  static inline QuantityType epsilon() noexcept
  { return QuantityType::from_value( RawQuantityTraits::epsilon() ); }

  //! Get the maximum rounding error (only defined for floating-point types)
  static inline QuantityType roundError() noexcept
  { return QuantityType::from_value( RawQuantityTraits::roundError() ); }

  //! Get the inf quantity (only defined for floating-point types)
  static inline QuantityType inf() noexcept
  { return QuantityType::from_value( RawQuantityTraits::inf() ); }

  //! Get the quiet nan quantity (only defined for floating-point types)
  static inline QuantityType nan() noexcept
  { return QuantityType::from_value( RawQuantityTraits::nan() ); }

  //! Get the signaling nan quantity (only defined for floating-point types)
  static inline QuantityType signalingNan() noexcept
  { return QuantityType::from_value( RawQuantityTraits::signalingNan() ); }

  //! Get the absolute value of a quantity
  static inline RealFloatingPointQuantityType abs( const QuantityType& a )
  { return RealFloatingPointQuantityType::from_value( RawQuantityTraits::abs(a.value()) ); }

  //! Get the conjugate of a quantity
  static inline FloatingPointQuantityType conj( const QuantityType& a )
  { return FloatingPointQuantityType::from_value( RawQuantityTraits::conj(a.value()) ); }

  //! Get the real part of the quantity
  static inline RealFloatingPointQuantityType real( const QuantityType& a )
  { return RealFloatingPointQuantityType::from_value( RawQuantityTraits::real(a.value()) ); }

  //! Get the imaginary part of the quantity
  static inline RealFloatingPointQuantityType imag( const QuantityType& a )
  { return RealFloatingPointQuantityType::from_value( RawQuantityTraits::imag(a.value()) ); }

  //! Test if the quantity is a nan or inf
  static inline bool isnaninf( const QuantityType& a )
  { return RawQuantityTraits::isnaninf(a.value()); }

  //! Take the square root of a quantity (only one root will be calculated)
  static inline typename GetQuantityToPowerType<1,2>::AsFloatingPointType sqrt( const QuantityType& quantity )
  { return GetQuantityToPowerType<1,2>::AsFloatingPointType::from_value( RawQuantityTraits::sqrt( quantity.value() ) ); }

  //! Take the cube root of a quantity (only one root will be calculated)
  static inline typename GetQuantityToPowerType<1,3>::AsFloatingPointType cbrt( const QuantityType& quantity )
  { return GetQuantityToPowerType<1,3>::AsFloatingPointType::from_value( RawQuantityTraits::cbrt( quantity.value() ) ); }

  //! Take a quantity to the desired rational power
  template<boost::units::integer_type N, boost::units::integer_type D>
  static typename GetQuantityToPowerType<N,D>::AsFloatingPointType rpow( const QuantityType& quantity )
  { return GetQuantityToPowerType<N,D>::AsFloatingPointType::from_value( RawQuantityTraits::template rpow<N,D>( quantity.value() ) ); }

  //! Initialize a quantity (potentially dangerous!)
  static inline QuantityType initializeQuantity( const RawType& raw_quantity ) noexcept
  { return QuantityType::from_value( raw_quantity ); }

  //! Get the raw value of a quantity
  static inline const RawType& getRawQuantity( const QuantityType& quantity ) noexcept
  { return quantity.value(); }

  //! Set the value of a quantity (potentially dangerous!)
  static inline void setQuantity( QuantityType& quantity,
				  const RawType& raw_quantity ) noexcept
  { quantity = QuantityType::from_value( raw_quantity ); }
};

} // end Utility namespace

#endif // end UTILITY_MEASUREMENT_DEF_HPP

//---------------------------------------------------------------------------//
// end Utility_Measurement_def.hpp
//---------------------------------------------------------------------------//


/*!
  \author      Robert I A Patterson
  \author      Sashikumaar Ganesan

  Copyright (C) 2011 Robert I A Patterson & Sashikumaar Ganesan

  \file moonmd_interface.cpp
  \brief Interface between continuum flow solver and stochastic particle
         population balance solver

  Licence:

    This file is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public License
    as published by the Free Software Foundation; either version 2
    of the License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

  Contact:
    Dr. Robert Patterson
    Weierstrass Institute
    Mohrenstrasse 39
    10117 Berlin
    Germany
    riap@cantab.net
*/

#include "moonmd_interface.h"

#include "geometry1d.h"
#include "settings_io.h"
#include "reactor1d.h"
#include "reset_chemistry.h"
#include "pred_corr_solver.h"
#include "simulator.h"

#include "mops_settings_io.h"
#include "mops_mechanism.h"

#include "swp_model_stats.h"
#include "swp_mech_parser.h"

#include "linear_interpolator.hpp"

#include <boost/filesystem/operations.hpp>

#include <memory>
#include <iostream>
#include <stdexcept>
#include <limits>

/*!
 * Function private to this file
 *
 * Work out how long a particle should take to travel through each cell and return the shortest residence
 * time.  A mid point velocity integration is currently used.  This function is useful in determining the number of
 * time splitting steps which the solver must make between particle processes such as coagulation and inception
 * and particle transport.
 *
 *\param[in]         geom                Geometry defining the cells
 *\param[in]         solution_length     Communication is by vectors which must all be at least this long
 *\param[in]         solution_nodes      Distances from start of reactor (in m) at which velocity values are specified
 *\param[in]         velocity            Flow velocity in \f$\mathrm{ms}^{-1}\f$
 */
double minimumResidenceTime(const Geometry::Geometry1d& geom, const size_t solution_length,
                                 const double solution_nodes[], const double velocity[])
{
    const Utils::LinearInterpolator<double, double> velocityProfile(std::vector<double>(solution_nodes, solution_nodes + solution_length),
                                                                    std::vector<double>(velocity,       velocity + solution_length ));

    double minTime = std::numeric_limits<double>::max();
    for(size_t i = 0; i < geom.numCells(); ++i) {
        // In the 1d case cellVolume == length of cell
        double residenceTime = std::abs(geom.cellVolume(i) / velocityProfile.interpolate(geom.cellCentre(i)));
        minTime = residenceTime < minTime ? residenceTime : minTime;
    }
    return minTime;
}

/*!
 *\param[in]    chemfile     Path to file listing chemical species to consider
 *\param[in]    thermfile    Path to thermodynamic data file
 *\param[in]    settfile     Path to file specifying control parameters for Brush
 *\param[in]    swpfile      Path to XML file specifying the particle mechanism
 *\param[in]    partsolnfile Path to XML file specifying the initial particle population
 *\param[in]    chemsolnfile Path to file specifying the initial continuum phase composition
 *\param[in]    num_grid_nodes   Number of cell boundaries specified in grid_nodes
 *\param[in]    grid_nodes   Boundaries of cells to be used in Brush specified in meters.
 *
 *\pre [grid_nodes, grid_nodes + num_grid_nodes) must be a valid range
 *
 *\return    Pointer to new brush reactor object.   Client must call delete on this pointer.
 */
Brush::MooNMDInterface::particle_reactor_pointer
 Brush::MooNMDInterface::InitialiseBrush(
    const std::string& chemfile, const std::string& thermfile, const std::string& settfile,
    const std::string& swpfile, const std::string& partsolnfile, const std::string& chemsolnfile,
    const size_t num_grid_nodes, const double grid_nodes[])
{
    std::cout << "Setting up Brush\n";

    // Look at working directory (to help debug tests)
    std::cout << "Working directory is " << boost::filesystem::current_path() << std::endl;

    // diagnostic level (could be a user input)
   const int diag = 1;

    // Geometry
    std::auto_ptr<Geometry::Geometry1d> pGeom;
    // Build the geometry object and hold a smart pointer to it
    pGeom.reset(new Geometry::Geometry1d(std::vector<double>(grid_nodes, grid_nodes + num_grid_nodes),
                                          Geometry::dirichlet, Geometry::neumann));

    // Variables to hold data read from the input file
    Mops::timevector timeIntervals;
    std::string outputFileBaseName;
    std::vector<std::pair<double, double> > maxPCounts, maxM0s;
    Sweep::Stats::IModelStats::StatBound statBound;

    {
        // Load the XML
        CamXML::Document settingsXML;
        settingsXML.Load(settfile);
        const CamXML::Element * const root = settingsXML.Root();

        // Time intervals for stepping through the solution process with output
        const CamXML::Element * node = root->GetFirstChild("timeintervals");
        if (node != NULL) {
            Mops::Settings_IO::readTimeIntervals(*node, timeIntervals);
        } else {
            throw std::runtime_error("No time intervals found");
        }

        // Maximum number of computational particles per cell
        maxPCounts = Brush::Settings_IO::readProfile(root, "pcount");

        // Maximum particle concentration
        maxM0s = Brush::Settings_IO::readProfile(root, "maxm0");

        // Output details
        node = root->GetFirstChild("output");
        if(node != NULL) {
            const CamXML::Element * const filenameNode = node->GetFirstChild("filename");
            if(filenameNode != NULL) {
                outputFileBaseName = filenameNode->Data();
            }
            else {
                throw std::runtime_error("A <filename> element must be supplied in the output section");
            }

            // See if the stats bounds were specified in the input file
            const CamXML::Element * const statsBoundNode = node->GetFirstChild("statsbound");
            if(statsBoundNode != NULL) {
                // Read the data from the file
                Sweep::PropID statsboundPropertyID;
                double statsLowerBound;
                double statsUpperBound;
                Mops::Settings_IO::ReadStatsBound(*statsBoundNode, statsboundPropertyID, statsLowerBound, statsUpperBound);

                // Adjust statBound with the newly read data
                statBound.PID = statsboundPropertyID;
                statBound.Lower = statsLowerBound;
                statBound.Upper = statsUpperBound;
            }
            // If no statsbound was specified in the input file, the default constructed value of
            // statBound will be used.
        }
        else {
            throw std::runtime_error("A <output> element must be supplied with details for the required output");
        }

    }

    //========= Load chemical mechanism ==========================
    // For fixed chemistry simulations the mech does not have to
    // contain reactions, only the species names are required.
    Mops::Mechanism mech;
    {
        if(diag > 0) {
            std::cout << "Reading chemical mechanism...\n";
        }

        Sprog::IO::MechanismParser::ReadChemkin(chemfile, mech.GasMech(), thermfile, diag);

        if (diag>0)
            mech.GasMech().WriteDiagnostics("ckmech.diag");
    }

    //========= Load particle mechanism ==========================
    {
        if(diag > 0) {
            std::cout << "Setting species on particle mechanism...\n";
        }
        mech.ParticleMech().SetSpecies(mech.GasMech().Species());
        if(diag > 0) {
            std::cout << "Species on particle mechanism are at " << mech.ParticleMech().Species() << '\n';
            std::cout << "Reading particle mechanism...\n";
        }
        Sweep::MechParser::Read(swpfile, mech.ParticleMech());
        if(diag > 0) {
            std::cout << "Read particle mechanism with " << mech.ParticleMech().ProcessCount()
                      << " processes\n";
            if(diag > 1) {
                // Get names of the processes
                std::vector<std::string> procNames;
                mech.ParticleMech().GetProcessNames(procNames);

                // Print the process names out one line at a time
                std::vector<std::string>::const_iterator it = procNames.begin();
                const std::vector<std::string>::const_iterator itEnd = procNames.end();
                while(it != itEnd) {
                    std::cout << ' ' << *it++ << '\n';
                }
            }
        }
    }

    //======== Read the initial state of the continuum phase ================
    if(diag > 0) {
        std::cout << "Reading initial continuum solution...\n";
    }
    Brush::ResetChemistry initChem(chemsolnfile, Brush::ResetChemistry::Camflow, mech.GasMech(), diag);
    if(diag > 0) {
        std::cout << "Read initial continuum solution\n";
    }

    //========= Read initial guess for particle solution ==================
    std::list<ParticlePopulationPoint1d> initialPopulationPoints;
    {
        if(diag > 0) {
            std::cout << "Reading initial particle population...\n";
        }

        // Load the XML
        CamXML::Document initialParticlesXML;
        initialParticlesXML.Load(partsolnfile);
        const CamXML::Element * const root = initialParticlesXML.Root();

        // Now get the XML for each separate population
        typedef std::vector<CamXML::Element*> xml_element_list;
        xml_element_list populations;
        root->GetChildren("population", populations);

        // Read the position and details of each population
        for(xml_element_list::const_iterator it = populations.begin();
            it != populations.end(); ++it) {

            ParticlePopulationPoint1d populationDetails;

            // Get the position
            const CamXML::Attribute *attr = (*it)->GetAttribute("x");
            if(attr) {
                populationDetails.position = atof(attr->GetValue().c_str());
            }
            else {
                throw std::runtime_error("No x (position) attribute for a <population> element.");
            }

            // Get the number density of the particles
            const CamXML::Element * const m0XML = (*it)->GetFirstChild("m0");
            if(m0XML) {
                populationDetails.m0 = atof(m0XML->Data().c_str());
            }
            else {
                throw std::runtime_error("No <m0> (number density) child element for a <population> element.");
            }

            // Read in the population
            populationDetails.particleList =
                    Mops::Settings_IO::ReadInitialParticles(**it, mech.ParticleMech());

            initialPopulationPoints.push_back(populationDetails);
        }
    }

    //========= Build the initial reactor ========================
    Reactor1d *pReactor = new Reactor1d(*pGeom, mech.GasMech(), false, mech.ParticleMech(), maxPCounts, maxM0s);

    std::cout << "Setting the initial continuum solution\n";
    pReactor->ReplaceChemistry(initChem, true);

    // Put the initial particles into the reactor
    pReactor->ReplaceParticles(initialPopulationPoints.begin(), initialPopulationPoints.end());

    return pReactor;
}

/*!
 * Simulate the particle processes from the current reactor time until t_stop
 * and calculate mean energy and mass source terms.
 *
 *\param[in,out]     reac                The reactor specifying the initial condition and particle dynamics
 *\param[in]         t_stop              Time upto which to simulate
 *\param[in]         solution_length     Communication is by vectors which must all be at least this long
 *\param[in]         num_species         Number of species for which mass concentrations are supplied
 *\param[in]         solution_nodes      Distances from start of reactor (in m) at which solution values are specified
 *\param[in]         temperature         Reactor temperature in K
 *\param[in]         velocity            Flow velocity in \f$\mathrm{ms}^{-1}\f$
 *\param[in]         mass_concs          Concentrations of species in \f$\mathrm{kg m}^{-3}\f$
 *\param[in]         path_id             Integer identifying the path so that different sequences of random numbers can be used for each path
 *\param[out]        energy_source       Energy release by particle processes in \f$\mathrm{J m}^{-3} \mathrm{s}^{-1}\f$
 *\param[out]        mass_conc_sources   Release of species into solution by particle processes in \f$\mathrm{kg m}^{-3}\mathrm{s}^{-1}\f$
 *\param[in,out]     moment_output       Stream to which textual moment data can be written during calculation.
 *
 *\pre  All the 1d arrays supplied by the caller must have length at least solution_length, this includes
 *       the output arrays.
 *
 *\pre  The arrays mass_concs and mass_conc_sources are effectively 2d arrays and must have length at least
 *       solution_length * num_species
 *
 *\return    Pointer to updated brush reactor object.
 *
 * Sign convention for source terms: Positive source terms indicate release of energy or material by the solid
 * phase.  Thus a positive energy source would indicate exothermic crystallisation and a positive mass source
 * would indicate that crystals were dissolving.
 *
 * The data in mass_concs shall be ordered such that data for species i begins at mass_concs + i * num_species,
 * that is the fastest varying index shall be the index for spatial position.  This is fairly easy to change,
 * but it is important to keep all comments and code consistent!  The data in mass_conc_sources shall be ordered
 * in the same way as that in mass_concs.
 */
Brush::MooNMDInterface::particle_reactor_pointer Brush::MooNMDInterface::RunParticlePhase(
    particle_reactor& reac, const double t_stop,
    const size_t solution_length,
    const size_t num_species,
    const double solution_nodes[],
    const double temperature[],
    const double velocity[],
    const double mass_concs[],
    const size_t path_id,
    double energy_source[],
    double mass_conc_sources[],
    std::ostream &moment_output)
{
    //========= Set the chemistry on the reactors ==========
    // First put the data in vectors, because this is what the interface to ResetChemistry expects
    const fvector solutionNodes(solution_nodes, solution_nodes + solution_length);
    const fvector temperatureVector(temperature, temperature + solution_length);
    const fvector velocityVector(velocity, velocity + solution_length);

    // Work out the total density in kg m^-3
    fvector density(solution_length, 0.0);
    // Loop over the spatial positions
    for (unsigned i = 0u; i < solution_length; ++i) {
        // Add up the contribution of all the species at current position
        for (unsigned j = 0u; j < num_species; ++j) {
            density[i] += mass_concs[i + j * solution_length];
        }
    }

    // Now calculate mass fractions (because this is what the ResetChemistry constructor requires)
    std::vector<fvector> massFracs(num_species, fvector(solution_length));
    // Loop over the species
    for (unsigned i = 0u; i < num_species; ++i) {
        // Scale by the total mass density at each point
        for (unsigned j = 0u; j < solution_length; ++j) {
            massFracs[i][j] = mass_concs[j + i * solution_length] / density[j];
        }
    }

    // Dummy vector to fill unused argument positions
    const fvector vec(solution_length);

    ResetChemistry chemInterpolator(solutionNodes, temperatureVector, density, velocityVector,
                                    vec, vec, vec, vec, vec, massFracs);

    // Put the chemistry into the reactor
    reac.ReplaceChemistry(chemInterpolator, true);

    //======== Simulate to update the particle population =======
    Brush::PredCorrSolver solver(chemInterpolator, 0, 0.0, 0.0, false, 0.0, true, true, false);

    // Need to make sure that transport is simulated on intervals that do not
    // always take particles over cell boundaries.
    double minResidence = minimumResidenceTime(reac.getGeometry(), solution_length, solution_nodes, velocity);
    unsigned numSplittings = std::ceil(10 * t_stop / minResidence);
    std::cout << "Num splittings " << numSplittings << ", min residence " << minResidence << std::endl;
    solver.solve(reac, 0.0, t_stop, numSplittings, path_id);
    Brush::Simulator::saveParticleStats(reac, t_stop, Sweep::Stats::IModelStats::StatBound(), moment_output);

    //======== Estimate the source terms to return to the client =====

    return &reac;
}


//
// Created by miguelyermo on 23/7/20.
//

#include "AccumTime.h"
#include "convexHull.hpp"
#include "handlers.h"
#include "laspec.h"
#include "pseudoInverseMoorePenrose.h"
#include "SMRF.h"

#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <functional>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <limits>
#include <map>
#include <memory>
#include <omp.h>
#include <ostream>
#include <stack>
#include <utility>
#include <vector>

//
// Created by miguelyermo on 9/7/20.
//

/*
* FILENAME :  dtm.h
* PROJECT  :  rule-based-classifier-cpp
* DESCRIPTION : Creation and handling of a dtm.
*
*
*
*
*
* AUTHOR :    Miguel Yermo        START DATE : 09:20 9/7/20
*
*/

namespace SMRF
{

constexpr unsigned g_cluster_max_size{ 128 };

void classify(std::vector<Lpoint> & points, DTMGrid & dtm)
{
	int count = 0;

	std::ofstream f(mainOptions.outputDirName + "/smrf/result.xyz");

	f << std::fixed << std::setprecision(3);

	std::cout << "Labeling points...\n";

	for (Lpoint & p : points)
	{
		double dtmZ = dtm.nnInterpolation(p);
		p.setClass(p.z() < dtmZ + MAX_HEIGHT ? GROUND : UNCLASSIFIED);
		count += p.getClass() == GROUND;
		f << p.x() << " " << p.y() << " " << p.z() << " "
			<< p.I() << " " << p.rn() << " " << p.nor() << " " << p.dir() << " " << p.edge()
			<< " " << p.getClass() << "\n";
	}
	std::cout << "Ground " << count / double(points.size()) * 100 << "%\n";
}

/* Distance from point to cell center */
double distP2C(Lpoint & p, DTMCell * cell)
{
	return sqrt((p.x() - cell->center[0]) * (p.x() - cell->center[0]) +
	            (p.y() - cell->center[1]) * (p.y() - cell->center[1]));
}

/* Ensure neighbors indices are inside grid boundaries **/
inline void DTMGrid::clampNeighIdcs(int ci, int cj, int r, int min[2], int max[2]) const
{
	min[0] = ci - r >= 0 ? ci - r : 0;
	min[1] = cj - r >= 0 ? cj - r : 0;
	max[0] = ci + r < rows ? ci + r : rows - 1;
	max[1] = cj + r < cols ? cj + r : cols - 1;
}

/** Get non-empty neighboring cells in (row,col) pairs for a given radius (starts from left-bottom) */
// TODO: Funcion muy optimizable si se asignan los vecinos al crear la malla
// (8 vecinos salvo en esquinas y bordes) Ver computeCellNeighbors()
// Tambien son necesarios los vecinos de orden superior
inline std::vector<size_t> DTMGrid::getNeighCells(int Ci, int Cj, int r)
{
	int min[2], max[2], maxNeighs = 0;

	clampNeighIdcs(Ci, Cj, r, min, max);
	maxNeighs = (2 * r + 1) * (2 * r + 1) - 1;
	std::vector<size_t> neighs;
	neighs.reserve(2 * maxNeighs);
	for (size_t i = min[0]; i <= max[0]; i++)
		for (size_t j = min[1]; j <= max[1]; j++)
			if ((i != Ci || j != Cj) && (!cells[i * cols + j].empty || cells[i * cols + j].inpainted))
			{
				neighs.push_back(i);
				neighs.push_back(j);
			}
	return neighs;
}

double DTMGrid::nnInterpolation(Lpoint& p)
{
	int    Ci = 0, Cj = 0, i = 0, j = 0, n = 0;
	double dist = 0, minDist = 0, z = 0;
	Ci                         = ci[p.id()];
	Cj                         = cj[p.id()];
	z                          = cells[Ci * cols + Cj].z;
	minDist                    = distP2C(p, &cells[Ci * cols + Cj]);
	std::vector<size_t> neighs = getNeighCells(Ci, Cj, 1);
	for (n = 0; n < neighs.size(); n += 2)
	{
		i = neighs[n];
		j = neighs[n + 1];
		if ((int) cells[i * cols + j].z == NO_DATA_VAL) continue;
		dist = distP2C(p, &cells[i * cols + j]);
		if (dist < minDist)
		{
			z       = cells[i * cols + j].z;
			minDist = dist;
		}
	}
	return z;
}

double DTMGrid::slopeInterpolation(Lpoint & p)
{
	int    Ci = 0, Cj = 0, i = 0, j = 0, n = 0;
	double dist = 0, minDist = 0, slope = 0;
	Ci                         = ci[p.id()];
	Cj                         = cj[p.id()];
	slope                      = cells[Ci * cols + Cj].slope;
	minDist                    = distP2C(p, &cells[Ci * cols + Cj]);
	std::vector<size_t> neighs = getNeighCells(Ci, Cj, 1);
	for (n = 0; n < neighs.size(); n += 2)
	{
		i = neighs[n];
		j = neighs[n + 1];
		if ((int) cells[i * cols + j].z == NO_DATA_VAL) continue;
		dist = distP2C(p, &cells[i * cols + j]);
		if (dist < minDist)
		{
			slope   = cells[i * cols + j].slope;
			minDist = dist;
		}
	}
	return slope;
}

void DTMGrid::computeDtmZ(std::vector<Lpoint> & points, Octree & octree)
{
	std::cout << "Computing dtm...\n";

	for (auto& p : points)
	{
		p.setSlopeDTM(slopeInterpolation(p));
		p.setDtmZ(nnInterpolation(p));
		// TODO: check DTM and solve the bug to avoid it
		if ((p.z() - p.getDtmZ()) < 0)
			p.setRealZ(0.01);
		else
			p.setRealZ(p.z() - p.getDtmZ());
	}
}

/** Compare pairs by value (descending order) */
static int pairCmpDesc(const void * p1, const void * p2)
{
	int v1 = ((Pair *) p1)->value;
	int v2 = ((Pair *) p2)->value;
	return v2 - v1;
}

static int cmpX(const void * a, const void * b)
{
	double v1 = ((struct Coordinate *) a)->value;
	double v2 = ((struct Coordinate *) b)->value;

	return (v1 > v2) - (v1 < v2);
}

extern DTMGrid* createGridDTM(double * min, double * max, int numPts, double cs)
{
	int       i = 0, j = 0;
	double    r    = cs / 2.0;
	DTMGrid * grid = nullptr;

	grid         = new DTMGrid;
	grid->min[0] = ceil(min[0]);
	grid->min[1] = ceil(min[1]);
	grid->rows   = ceil((floor(max[0]) - grid->min[0]) / cs);
	grid->cols   = ceil((floor(max[1]) - grid->min[1]) / cs);
	grid->size   = grid->rows * grid->cols;
	grid->ci.resize(numPts);
	grid->cj.resize(numPts);
	grid->cells.resize(grid->size);
	for (i = 0; i < grid->rows; i++)
	{
		for (j = 0; j < grid->cols; j++)
		{
			grid->cells[i * grid->cols + j].id        = i * grid->cols + j;
			grid->cells[i * grid->cols + j].obj       = false;
			grid->cells[i * grid->cols + j].empty     = true;
			grid->cells[i * grid->cols + j].inpainted = false;
			grid->cells[i * grid->cols + j].center[0] = grid->min[0] + (i * cs) + r;
			grid->cells[i * grid->cols + j].center[1] = grid->min[1] + (j * cs) + r;
			grid->cells[i * grid->cols + j].z         = std::numeric_limits<double>::max();
		}
	}
	return grid;
}

/** Get the adjacent cells for BE Surface */
std::vector<indexOfCell_t> DTMGrid::getAdjCellsBE(int ci, int cj)
{
	int min[2], max[2];

	clampNeighIdcs(ci, cj, 1, min, max);

	std::vector<indexOfCell_t> neighs{};

	for (int i = min[0]; i <= max[0]; i++)
		for (int j = min[1]; j <= max[1]; j++)
		{
			if ((i != ci || j != cj) && cells[i * cols + j].inhull)
			{
				neighs.emplace_back(i, j);
			}
		}
	return neighs;
}

/** Get the adjacent cells for Minimun Surface */
std::vector<indexOfCell_t> DTMGrid::getAdjCellsMS(int Ci, int Cj)
{
	int min[2], max[2]; // range of index for neighbours

	clampNeighIdcs(Ci, Cj, 1, min, max);
	std::vector<indexOfCell_t> neighs{};

	for (int i = min[0]; i <= max[0]; i++)
		for (int j = min[1]; j <= max[1]; j++)
			if ((i != Ci || j != Cj) && cells[i * cols + j].inhull)
				neighs.emplace_back(i, j);

	return neighs;
}

/** Check if a set have at least one cell which is not object */
bool checkFullObj(DTMGrid *grid, std::vector<indexOfCell_t>& vec)
{
	for(const auto& cell : vec)
	{
		int pos{cell.first * grid->cols + cell.second};
		if (!grid->cells[pos].obj || (grid->cells[pos].obj && grid->cells[pos].inpainted))
		{
			return false;
		}
	}
	return true;
}

/** Filter the set taking object cells */
std::vector<indexOfCell_t> checkForObjCells(DTMGrid *grid, std::vector<indexOfCell_t>& vec)
{
	std::vector<indexOfCell_t> tmp{};
	for (const auto& cell : vec)
	{
		int pos{cell.first * grid->cols + cell.second};
		if (grid->cells[pos].obj && !grid->cells[pos].inpainted)
			tmp.emplace_back(cell);
	}
	return tmp;
}

void DTMGrid::createClusters(std::vector<indexOfCell_t>& inpaintCells, std::map<indexOfCell_t, std::vector<indexOfCell_t>>& neighbourCells,
		std::vector<Cluster>& clusters, ListNeighsFn listNeighsFn, CheckNeighsFn chNeighsFn)
{
	std::vector<indexOfCell_t> remainingCells(inpaintCells);

	while (!remainingCells.empty())
	{
		Cluster cluster{};
		// stackCheckCell: cola que contén as celas baleiras adxacentes pendentes de comprobar
		// se teñen a súa vez máis celas adxacentes
		std::stack<indexOfCell_t> stackCells{};

		stackCells.emplace(remainingCells[0]);

		// introducimos a primeira cela das que quedan por facer o inpainting
		// bucle para buscar veciños
		while (!stackCells.empty())
		{
			auto& cell = stackCells.top();
			stackCells.pop();

			// evita que caia en bucle infinito: para que non volva a revisar a mesma cela
			auto begin(remainingCells.begin());
			auto end(remainingCells.end());
			if (std::find(begin, end, cell) == end)
			{
				continue;
			}

			std::vector<indexOfCell_t> neighbours;

			neighbours = neighbourCells.at(cell);

			// quitamos esa cela da lista de pendentes por ver
			std::erase(remainingCells, cell);

            // anotamos esa cela para despois usala no sistema de ecuacións
			cluster.emplace_back(cell);

            // lista de veciños do mesmo tipo da cela extraida
			auto listNeighs(listNeighsFn(this, neighbours));

			for (int pos = 0; pos < listNeighs.size(); ++pos)
			{
				auto& empty = listNeighs[pos];
				stackCells.push(empty);
			}
		}
		clusters.emplace_back(cluster);
	}
}

void DTMGrid::solveSystems(std::vector<Cluster>& clusters, std::map<indexOfCell_t, std::vector<indexOfCell_t>>& neighbourCells,
	CheckValueCellFn chValueCellFn, ListNeighsFn listNeighsFn, CheckNeighsFn chNeighsFn, ChangeCellStateFn chCellStateFn)
{
	/* resolución dos clusters */
	for (auto pos{clusters.begin()}; pos != clusters.end(); ++pos)
	{
		Cluster cluster{*pos};

		while (!cluster.empty())
		{
			int col{};
			std::vector<indexOfCell_t> columns{};
			std::stack<indexOfCell_t> stackCells{};

			stackCells.emplace(cluster[0]);

			while (!stackCells.empty() && columns.size() < g_cluster_max_size)
			{
				auto& cell = stackCells.top();
				stackCells.pop();

				// evita que caia en bucle infinito: para que non volva a revisar a mesma cela
				auto begin{cluster.begin()};
				auto end{cluster.end()};
				if (std::find(begin, end, cell) == end)
				{
					continue;
				}

				auto neighbours(neighbourCells.at(cell));
				if (chNeighsFn(this, neighbours))
				{
					if (columns.empty())
					{
						std::erase(cluster, cell);
						cluster.emplace_back(cell);
						stackCells.emplace(cluster[0]);
					}
					continue;
				}

				// quitamos esa cela da lista de pendentes por ver
				std::erase(cluster, cell);

				// anotamos esa cela para despois usala no sistema de ecuacións
				columns.emplace_back(cell);

				// lista de veciños do mesmo tipo da cela extraida
				auto listNeighs(listNeighsFn(this, neighbours));

				for (auto neigh{listNeighs.begin()}; neigh != listNeighs.end(); ++neigh)
				{
					stackCells.push(*neigh);
				}
			}

			Eigen::MatrixXd M{columns.size() * 8, columns.size()};
			M.setZero();
			Eigen::VectorXd b{columns.size() * 8};
			b.setZero();

			auto columnBegin{columns.begin()};
			auto columnEnd{columns.end()};

			int currentRow{};
			for (int posCell{}; posCell < columns.size(); ++posCell)
			{
				auto cell{columns[posCell]};
				auto local{neighbourCells.at(cell)};

				int currentCol(posCell);

				// obtemos os valores Z das celas veciñas
				for (int posNeigh = 0; posNeigh < local.size(); ++posNeigh)
				{
					bool emptyNeigh{false};
                    std::vector<indexOfCell_t>::iterator found{};
                    indexOfCell_t neighbour(local[posNeigh]);

                    emptyNeigh = chValueCellFn(this, neighbour);
					found = std::find(columnBegin, columnEnd, neighbour);

					if (emptyNeigh && found == columnEnd)
						continue;

					double z{emptyNeigh ? 0.0 : cells[neighbour.first * cols + neighbour.second].z};

					if (emptyNeigh)
					{
						M(currentRow, std::distance(columnBegin, found)) = -1.0;
					}

					M(currentRow, currentCol) = 1.0;
					b[currentRow] = z;

					++currentRow;
				}
			}

			Eigen::VectorXd x = pseudoInverse(M) * b;

			for (int posCell = 0; posCell < columns.size(); ++posCell)
			{
				auto cell{columns[posCell]};
				cells[cell.first * cols + cell.second].z = x[posCell];
				chCellStateFn(this, cell);
			}
		}
	}
}

bool isObj(DTMGrid* grid, indexOfCell_t cell)
{
	return grid->cells[cell.first * grid->cols + cell.second].obj;
}

void changeObjToFalse(DTMGrid* grid, indexOfCell_t& cell)
{
	grid->cells[cell.first * grid->cols + cell.second].obj = false;
	grid->cells[cell.first * grid->cols + cell.second].inpainted = true;
}

/** Inpaint Bare Earth */
void DTMGrid::inpaintBE()
{
	std::vector<indexOfCell_t> vectorOfObjCells{};

	// assign column for every cell
	for (int i = 0; i < rows; ++i)
		for (int j = 0; j < cols; ++j)
			if (cells[i * cols + j].inhull && cells[i * cols + j].obj)
			{
				vectorOfObjCells.emplace_back(std::make_pair(i, j));
			}

	const int numObj{static_cast<int>(vectorOfObjCells.size())};
	std::cout << numObj << " obj cells " << numObj * 100.0 / size << "%\n";
	std::cout << "Inpainting... " << std::flush;

	// collemos tódolas celas baleiras e almacenamos os seus veciños nunha lista
	std::map<indexOfCell_t, std::vector<indexOfCell_t>> neighbourOfObjCells{};
	// remainingCells contén as celas restantes para o inpainting (inicialmente todas)

	for (int pos = 0; pos < vectorOfObjCells.size(); ++pos)
	{
		auto cell{vectorOfObjCells.at(pos)};
		neighbourOfObjCells.emplace(cell, getAdjCellsBE(cell.first, cell.second));
	}

	auto chValueFn{ &isObj };
	auto chCellStateFn{ &changeObjToFalse };
	auto lsNeighsFn{ &checkForObjCells };
	auto chNeighsFn{ &checkFullObj};

	std::vector<Cluster> clusters{};

	createClusters(vectorOfObjCells, neighbourOfObjCells, clusters, lsNeighsFn, chNeighsFn);
	solveSystems(clusters, neighbourOfObjCells, chValueFn, lsNeighsFn, chNeighsFn, chCellStateFn);

	std::cout << numObj << " inpainted" << '\n';
}

/** Get number of valid adjacent cells for Minimum Surface */
int DTMGrid::getNumAdjMS(int Ci, int Cj)
{
	int num = 0, min[2], max[2];

	clampNeighIdcs(Ci, Cj, 1, min, max);
	for (size_t i = min[0]; i <= max[0]; i++)
		for (size_t j = min[1]; j <= max[1]; j++)
			if ((i != Ci || j != Cj) && (!cells[i * cols + j].empty || cells[i * cols + j].inpainted)) num++;
	return num;
}

bool areAdjacent(indexOfCell_t c1, indexOfCell_t c2)
{
	// first, we check if c2 is in a near row
	if (c1.first == c2.first || c1.first == c2.first + 1 || c1.first == c2.first - 1)
		// second, the same with the column
		if (c1.second == c2.second || c1.second == c2.second + 1 || c1.second == c2.second - 1)
			return true;

	return false;
}

bool checkFullEmpty(DTMGrid *grid, std::vector<indexOfCell_t> & vec)
{
	for(auto cell{vec.begin()}; cell != vec.end(); ++cell)
	{
		int pos{cell->first * grid->cols + cell->second};
		if (!grid->cells[pos].empty || (grid->cells[pos].empty && grid->cells[pos].inpainted))
		{
			return false;
		}
	}
	return true;
}

std::vector<indexOfCell_t> checkForEmptyCells(DTMGrid *grid, std::vector<indexOfCell_t> & vec)
{
	std::vector<indexOfCell_t> tmp{};

	for (auto& cell : vec)
	{
		int pos{cell.first * grid->cols + cell.second};
		if (grid->cells[pos].empty && !grid->cells[pos].inpainted)
			tmp.emplace_back(cell);
	}
	return tmp;
}

void changeInpaintedToTrue(DTMGrid *grid, indexOfCell_t& cell)
{
	grid->cells[cell.first * grid->cols + cell.second].inpainted = true;
}

bool isEmpty(DTMGrid* grid, indexOfCell_t cell)
{
	return grid->cells[cell.first * grid->cols + cell.second].empty && !grid->cells[cell.first * grid->cols + cell.second].inpainted;
}

/** Inpaint Minimum Surface */
void DTMGrid::inpaintMS(std::vector<indexOfCell_t>& vectorOfEmptyCells)
{
	const int numEmpty(vectorOfEmptyCells.size());
	std::cout << "Inpainting... " << std::flush;

	// collemos tódolas celas baleiras e almacenamos os seus veciños nunnha lista
	std::map<indexOfCell_t, std::vector<indexOfCell_t>> neighbourOfEmptyCells{};

	for (int pos = 0; pos < vectorOfEmptyCells.size(); ++pos)
	{
		auto& cell{vectorOfEmptyCells.at(pos)};
		neighbourOfEmptyCells.emplace(cell, getAdjCellsMS(cell.first, cell.second));
	}

	auto chValueFn{ &isEmpty };
	auto chCellStateFn{ &changeInpaintedToTrue };
	auto lsNeighsFn{ &checkForEmptyCells };
	auto chNeighsFn{ &checkFullEmpty };

	std::vector<Cluster> clusters{};

	createClusters(vectorOfEmptyCells, neighbourOfEmptyCells, clusters, lsNeighsFn, chNeighsFn);
	solveSystems(clusters, neighbourOfEmptyCells, chValueFn, lsNeighsFn, chNeighsFn, chCellStateFn);

	std::cout << numEmpty << " inpainted" << '\n';
}

/** Generate a minimun surface with the lowest cell points */
void DTMGrid::genMinSurface(Octree & octree, ConvexHull & hull)
{
	Lpoint center(0, 0, 0, 0);
	std::vector<indexOfCell_t> emptyCells{};

	for (int i{}; i < rows; ++i)
		for (int j{}; j < cols; ++j)
		{
			center.id(-1);
			center.setX(cells[i * cols + j].center[0]);
			center.setY(cells[i * cols + j].center[1]);
			cells[i * cols + j].inhull = hull.inHull(center);
			if (!cells[i * cols + j].inhull) continue;
			std::vector<Lpoint *> points = octree.searchNeighbors2D(center, CELL_SIZE);
			if (!points.empty())
			{
				for (int k{}; k < points.size(); ++k)
				{
					ci[points[k]->id()] = i;
					cj[points[k]->id()] = j;
					if (points[k]->z() < cells[i * cols + j].z) cells[i * cols + j].z = points[k]->z();
				}
				cells[i * cols + j].empty = false;
			}
			else
			{
				emptyCells.emplace_back(std::make_pair(i, j));
			}
		}

	int numEmpty{static_cast<int>(emptyCells.size())};

	std::cout << std::setprecision(2);
	std::cout << numEmpty << " empty cells " << numEmpty * 100.0 / size << "%\n";
	AccumTime::instance().start();
	inpaintMS(emptyCells);
	AccumTime::instance().stop("Inpaint Minimum Surface");
}

/** Perform morphological opening */
DTMGrid DTMGrid::morphologicalOpening(int radius, int numPts)
{
	int i = 0, j = 0, n = 0, ni = 0, nj = 0, numNeighs = 0;

	DTMGrid eGrid = *this;

#pragma omp parallel for default(none) private(i, j, n, ni, nj, numNeighs) \
    shared(radius, eGrid)
	for (i = 0; i < rows; i++) // Erosion
		for (j = 0; j < cols; j++)
		{
#if USE_DISK
			neighs = getDiskNeighCells(grid, i, j, radius, &numNeighs);
#else
			std::vector<size_t> neighs = getNeighCells(i, j, radius);
#endif
			for (n = 0; n < neighs.size(); n += 2)
			{
				ni = neighs[n];
				nj = neighs[n + 1];
				if (cells[ni * cols + nj].z < eGrid.cells[i * cols + j].z) eGrid.cells[i * cols + j].z = cells[ni * cols + nj].z;
			}
		}

	DTMGrid oGrid = eGrid;
#pragma omp parallel for default(none) private(i, j, n, ni, nj, numNeighs) \
    shared(radius, eGrid, oGrid)
	for (i = 0; i < eGrid.rows; i++) // Dilation
		for (j = 0; j < eGrid.cols; j++)
		{
#if USE_DISK
			neighs = getDiskNeighCells(eGrid, i, j, radius, &numNeighs);
#else
			std::vector<size_t> neighs = eGrid.getNeighCells(i, j, radius);
#endif
			for (n = 0; n < neighs.size(); n += 2)
			{
				ni = neighs[n];
				nj = neighs[n + 1];
				if (eGrid.cells[ni * cols + nj].z > oGrid.cells[i * cols + j].z)
					oGrid.cells[i * cols + j].z = eGrid.cells[ni * cols + nj].z;
			}
		}

	return oGrid;
}

void getMinMaxCoordinates(std::vector<Lpoint> & points, double * min, double * max)
{
	min[0] = min[1] = min[2] = std::numeric_limits<double>::max();
	max[0] = max[1] = max[2] = -std::numeric_limits<double>::max();
	for (auto & p : points)
	{
		if (p.x() < min[0]) min[0] = p.x();
		if (p.y() < min[1]) min[1] = p.y();
		if (p.z() < min[2]) min[2] = p.z();
		if (p.x() > max[0]) max[0] = p.x();
		if (p.y() > max[1]) max[1] = p.y();
		if (p.z() > max[2]) max[2] = p.z();
	}
}

/** Calculate plane slope by average maximum technique (ArGIS)
 *  @todo Handle slope when the cell it's at the grid boundary
 */
void DTMGrid::calcSlopes()
{
	double a = 0, b = 0, c = 0, d = 0, f = 0, g = 0, h = 0, I = 0, dzdx = 0, dzdy = 0,
	       rise = 0;

	for (int i = 0; i < rows; i++)
		for (int j = 0; j < cols; j++)
			if (!cells[i * cols + j].empty || cells[i * cols + j].inpainted)
			{
				std::vector<size_t> neighs = getNeighCells(i, j, 1);
				if (neighs.size() == 16)
				{
					g                 = cells[neighs[0] * cols + neighs[1]].z;
					h                 = cells[neighs[2] * cols + neighs[3]].z;
					I                 = cells[neighs[4] * cols + neighs[5]].z;
					d                 = cells[neighs[6] * cols + neighs[7]].z;
					f                 = cells[neighs[8] * cols + neighs[9]].z;
					a                 = cells[neighs[10] * cols + neighs[11]].z;
					b                 = cells[neighs[12] * cols + neighs[13]].z;
					c                 = cells[neighs[14] * cols + neighs[15]].z;
					dzdx              = ((c + 2 * f + I) - (a + 2 * d + g)) / (8 * CELL_SIZE);
					dzdy              = ((g + 2 * h + I) - (a + 2 * b + c)) / (8 * CELL_SIZE);
					rise              = sqrt(dzdx * dzdx + dzdy * dzdy);
					cells[i * cols +j].slope = atan(rise) * 180 / M_PI;
				}
				else
				{
					// see todo
				}
			}
}

void DTMGrid::computeDTM(std::vector<Lpoint>& points, Octree & octree, double * Min,
                         double * max)
{
	AccumTime::instance().start();
	ConvexHull hull(points);
	AccumTime::instance().stop("Computing Convex Hull");

	// medición de inpaintMS no interior
	genMinSurface(octree, hull);

	AccumTime::instance().start();
	identifyCells(points.size());
	AccumTime::instance().stop("Identifying cells");

	AccumTime::instance().start();
	identifyOutliers(points.size(), Min[2], max[2]);
	AccumTime::instance().stop("Identifying outliers");

	AccumTime::instance().start();
	inpaintBE();
	AccumTime::instance().stop("Inpainting Bare Earth");
}

/** Improved Simple Morphological Filter (SMRF) (Pingel, 2013) */
DTMGrid smrf(std::vector<Lpoint> & points, Octree & octree, double min[3], double max[3])
{
	std::cout << "---- Computing DTM (SMRF) ----\n";
	DTMGrid dtm(min, max, points.size(), CELL_SIZE);
	dtm.computeDTM(points, octree, min, max);

	dtm.calcSlopes();
	dtm.computeDtmZ(points, octree);

	std::string outDir = mainOptions.outputDirName + "/smrf";
	createDirectory(outDir);
	dtm.writeToFile(outDir + "/dtm.xyz");
	dtm.createDTM(outDir + "/dtm.asc");

	classify(points, dtm);

	return dtm;
}

DTMGrid::DTMGrid(double * min, double * max, int numPts_, double cs)
{
	int    i = 0, j = 0;
	double r = cs / 2.0;

	min[0] = ceil(min[0]);
	min[1] = ceil(min[1]);
	cols   = ceil((floor(max[0]) - min[0]) / cs);
	rows   = ceil((floor(max[1]) - min[1]) / cs);
	size   = rows * cols;
	numPts = numPts_;
	ci.resize(numPts);
	cj.resize(numPts);
	cells.resize(size);

	for (i = 0; i < rows; i++)
	{
		for (j = 0; j < cols; j++)
		{
			cells[i * cols + j].id        = i * cols + j;
			cells[i * cols + j].obj       = false;
			cells[i * cols + j].empty     = true;
			cells[i * cols + j].inpainted = false;
			cells[i * cols + j].center[0] = min[0] + (j * cs) + r;
			cells[i * cols + j].center[1] = min[1] + (i * cs) + r;
			cells[i * cols + j].z         = std::numeric_limits<double>::max();
		}
	}

	std::cout << "Cols: " << cols << ", Rows: " <<  rows << "\n";
}

// Copy Constructor
DTMGrid::DTMGrid(const DTMGrid & dtm)
{
	rows  = dtm.rows;
	cols  = dtm.cols;
	size  = dtm.size;
	ci    = dtm.ci;
	cj    = dtm.cj;
	min   = dtm.min;
	cells = dtm.cells;
}

void DTMGrid::identifyOutliers(int numPts_, double minZ, double maxZ)
{
	int     i = 0, j = 0, count = 0;
	double  maxHeight = 0;
	DTMGrid oInvMinSurface;

	std::cout << "Identifying outliers... " << std::flush;

	DTMGrid invMinSurface = *this;

	for (i = 0; i < invMinSurface.rows; i++)
		for (j = 0; j < invMinSurface.cols; j++)
			invMinSurface.cells[i * cols + j].z = (maxZ + minZ) - invMinSurface.cells[i * cols + j].z;

	maxHeight      = 5 * MAX_SLOPE * 1 * CELL_SIZE;
	oInvMinSurface = invMinSurface.morphologicalOpening(1, numPts_);

	for (i = 0; i < invMinSurface.rows; i++)
		for (j = 0; j < invMinSurface.cols; j++)
			if (invMinSurface.cells[i * cols + j].z - oInvMinSurface.cells[i * cols + j].z > maxHeight)
			{
				cells[i * cols + j].obj = true;
				count++;
			}
	std::cout << count << " outliers\n";
}

void DTMGrid::identifyCells(int numPts_)
{
	int     maxWindow = 0;
	double  maxHeight = 0;

	DTMGrid lastSurface = *this;

	maxWindow = std::ceil(MAX_WINDOW_SIZE / CELL_SIZE);
	for (int window = 1; window <= maxWindow; window++)
	{
		maxHeight = MAX_SLOPE * (float) window * CELL_SIZE;

#if DEBUG
		std::cout << std::setprecision(2);
		std::cout << "SMRF w=" << window << "/" << maxWindow << " h=" << maxHeight << "...\n";
#endif

		DTMGrid thisSurface = lastSurface.morphologicalOpening(window, numPts_);
		for (int i = 0; i < thisSurface.rows; i++)
			for (int j = 0; j < thisSurface.cols; j++)
				if (lastSurface.cells[i * cols + j].z - thisSurface.cells[i * cols + j].z > maxHeight)
					thisSurface.cells[i * cols + j].obj = true;

		lastSurface = thisSurface;
	}
	for (int i = 0; i < lastSurface.rows; i++)
		for (int j = 0; j < lastSurface.cols; j++)
			cells[i * cols + j].obj = lastSurface.cells[i * cols + j].obj;
}

} // namespace dtm


#include <iostream>
#include <fstream>
#include <sstream>
#include <regex>
#include <GBPCalc.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/program_options.hpp>

using boost::property_tree::getSortedChildren;
namespace po = boost::program_options;

void print_usage( std::string program_name, po::options_description &options_desc)
{
  std::cerr << "Usage: " << program_name << " [options] [<infile>] "<<std::endl;
  std::cerr << options_desc << std::endl;
  exit (8);
}

// read configuration file (in various formats) and return a property tree
ptree readConfig(std::string filename, std::string format = "ini")
{
  ptree configTree;
  if( format == "ini" )
  {
    ptree stage;
    boost::property_tree::read_ini( filename, stage);

    for(auto iter : stage)
      configTree.put( iter.first, iter.second.get<std::string>("") );
  }

  if( format == "json" )
  {
    boost::property_tree::read_json( filename, configTree);
  }

  if( format == "xml" )
  {
    boost::property_tree::read_xml( filename, configTree);
  }

  return configTree;
}

// utility function to convert data to strings
template<typename T>
std::string toString( T v )
{
  std::stringstream stream;
  stream << v;
  return stream.str();
}

// get a parameter from a beam based on a parameter name
std::string getBeamParam( GaussianBeam &beam, std::string name )
{

#define RETURN( pattern, param )\
  if(std::regex_match(name, std::regex(pattern, std::regex_constants::icase ) ) ) { return toString( beam.get##param().value() ); }

RETURN("^beam\\s*diameter$", Diameter);
RETURN("^diameter$", Diameter);
RETURN("^divergence$", Divergence );
RETURN("^radius\\s*of\\s*curvature$", Divergence );
RETURN("^RoC$", Divergence );

return "UNKNOWN";
}

// structure for managing data to log
struct Log
{
  std::queue<std::string> lines;
  std::string line;

  std::vector<std::string> inputNames;
  std::vector<std::string> outputNames;

  std::string filename;

  template<typename T>
  void stage(T data)
  {
    if( line != "" )
      line += " ";

    line += toString(data);
  }

  void push()
  {
    lines.push( line );
    line = "";
  }

  void write( std::ostream& out )
  {
    while( !lines.empty() )
    {
      out << lines.front() << "\n";
      lines.pop();
    }
  }

  void write( )
  {
    std::ofstream out( filename );
    write(out);
    out.close();
  }

  void readOutputs( GaussianBeam beam )
  {
    for( auto name : outputNames )
      this->stage( getBeamParam( beam, name ) );
  }

  void readInputs( ptree config )
  {
    for( auto name : inputNames )
      this->stage( config.get<std::string>( name, "UNKNOWN") );
  }

  void setFilename( std::string prefix, std::string name)
  {
    filename = prefix+"."+name+".log";
  }

};

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

  // option handler
  po::options_description options("Supported Options");
  options.add_options()
      ("help,h"                                                                           , "print help message")
      ("verbose,v"                , po::value<int>()->default_value(0)                    , "the verbose level (-v -> verbose level 1, -vv -> verbose level 2, etc)" )
      ("debug,d"                  , po::value<int>()->default_value(0)                    , "the debug level (-d -> debug level 1, -dd -> debug level 2, etc)" )
      ("config,m"                 , po::value<std::string>()->default_value("gbp.conf")   , "configuration file" )
      ;

  po::positional_options_description args;
  args.add("config", 1);

  po::variables_map vm;
  po::store(  po::command_line_parser(argc, argv).options(options).positional(args).run(), vm);
  po::notify(vm);

  // options (including arguments) have been parsed.

  if (argc == 1 || vm.count("help"))
  {
    print_usage( argv[0], options );
    return 1;
  }

  // application

  GBPCalc<t::centimeter> calculator;
  ptree configTree;

  // read config file.
  configTree = readConfig( vm["config"].as<std::string>(), "ini" );
  //boost::property_tree::write_json( std::cout, configTree );

  for( auto run : configTree.get_child("parametric_runs") )
  {
    ptree configTreeCopy = configTree;
    std::string x_name = run.second.get<std::string>("parameters.0.name");
    std::queue<double> x_vals;
    boost::optional<double> min = run.second.get<double>("parameters.0.min");
    boost::optional<double> max = run.second.get<double>("parameters.0.max");
    boost::optional<size_t> n   = run.second.get<size_t>("parameters.0.n");

    if(min && max && n)
    {
      double dz = (max.value() - min.value())/(n.value()-1);

      for( size_t i = 0; i < n.value(); i++)
        x_vals.push( (min.value() + i*dz) );
    }
    auto valuesConfig = run.second.get_child_optional("values");
    if(valuesConfig)
      for( auto iter: getSortedChildren( valuesConfig.value(), keyIntComp, isInt ) )
        x_vals.push( iter->second.get<double>("") );

    std::map<std::string, boost::shared_ptr<Log>> logs;
    std::string logPrefix = run.second.get<std::string>("logging.prefix", "GBP");
    for( auto logConfig : getSortedChildren( run.second.get_child("logging.loggers"), keyIntComp, isInt) )
    {
      std::string name = logConfig->second.get<std::string>("tag","data");
      logs[ name ] = boost::shared_ptr<Log>( new Log() );
      logs[ name ]->setFilename( logPrefix, name );
      logs[ name ]->stage("#");
      calculator.sig_calculatedBeam.connect( decltype(calculator.sig_calculatedBeam)::slot_type( &Log::readOutputs, logs[ name ].get(), _1 ).track(logs[name]) );
      if( logConfig->second.get_child_optional("inputs") )
      {
        for( auto data : getSortedChildren( logConfig->second.get_child("inputs"), keyIntComp, isInt) )
        {
          std::string inputName = data->second.get<std::string>("name");
          logs[ name ]->inputNames.push_back( inputName );
          logs[ name ]->stage( inputName );
        }
      }
      if( logConfig->second.get_child_optional("outputs") )
      {
        for( auto data : getSortedChildren( logConfig->second.get_child("outputs"), keyIntComp, isInt) )
        {
          std::string outputName = data->second.get<std::string>("name");
          logs[ name ]->outputNames.push_back( data->second.get<std::string>("name") );
          logs[ name ]->stage( outputName );
        }
      }
      logs[ name ]->push();
    }

    while( !x_vals.empty() )
    {
      configTreeCopy.put( x_name, x_vals.front() );

      calculator.configure( configTreeCopy );

      // make sure inputs get logged first
      for( auto log : logs )
        log.second->readInputs( configTreeCopy );

      calculator.calculate();

      // push new line onto log
      for( auto log : logs )
        log.second->push();

      x_vals.pop();
    }

    for( auto log : logs )
      log.second->write();

  }

  return 0;
}


/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: t; c-basic-offset: 4 -*- vim:set ts=4 sw=4 sts=4 noet: */
#include "cas.h"

#include <cassert>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <deque>
#include <iostream>
#include <iterator>
#include <sstream>
#include <string>

#include <boost/spirit/include/lex_lexertl.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/uuid/uuid_io.hpp>

#include "cas/dimension.h"
#include "cas/helper.h"

using namespace boost::spirit;

namespace flint {
namespace cas {

namespace {

bool IsConditional(const Compound &x)
{
	return x.keyword == "case-set";
}

bool IsConditional(const Expr &x, Compound &y)
{
	if (x.which() != kExprIsCompound) return false;
	y = boost::get<Compound>(x);
	return IsConditional(y);
}

bool IsEquation(const Compound &x)
{
	return x.keyword == "eq" && x.children.size() >= 2;
}

bool IsEquation(const Expr &x, Compound &y)
{
	if (x.which() != kExprIsCompound) return false;
	y = boost::get<Compound>(x);
	return IsEquation(y);
}

bool IsBoundVariable(const Expr &expr)
{
	if (expr.which() == kExprIsCompound) {
		const Compound &c = boost::get<Compound>(expr);
		if (c.keyword == "bvar") {
			if (c.children.size() == 1) {
				const Expr &e0 = c.children[0];
				if (e0.which() == kExprIsIdentifier) {
					const std::string &s0 = boost::get<Identifier>(e0).name;
					if (s0 == "%time")
						return true;
				}
				std::cerr << "unsupported bound variable of <diff>" << std::endl;
				return false;
			}
		}
	}
	std::cerr << "unsupported form of <diff>'s 1st argument" << std::endl;
	return false;
}

void ReportInvalidLeafOfCaseSet(const boost::uuids::uuid &uuid)
{
	std::cerr << "invalid formula found in <case-set>: " << uuid << std::endl;
}

bool TransformConditional(const Compound &c, Expr &lhs, Compound &rhs)
{
	rhs.keyword = "piecewise";
	rhs.children.clear();

	for (const auto &child : c.children) {
		assert(child.which() == kExprIsCompound);
		const Compound &cs = boost::get<Compound>(child);
		assert(cs.keyword == "case");
		Compound r;
		Compound w;
		switch (cs.children.size()) {
		case 1:
			{
				r.keyword = "otherwise";
				r.children.resize(1);

				const Expr &v(cs.children[0]);
				if (IsConditional(v, w)) {
					Compound t;
					if (!TransformConditional(w, lhs, t)) return false;
					r.children[0] = t;
				} else if (IsEquation(v, w)) {
					lhs = w.children[0];
					r.children[0] = w.children[1];
				} else {
					return false;
				}

				rhs.children.push_back(r);
			}
			break;
		case 2:
			{
				r.keyword = "piece";
				r.children.resize(2);

				const Expr &v0(cs.children[0]);
				const Expr &v1(cs.children[1]);
				r.children[1] = v0;
				if (IsConditional(v1, w)) {
					Compound t;
					if (!TransformConditional(w, lhs, t)) return false;
					r.children[0] = t;
				} else if (IsEquation(v1, w)) {
					lhs = w.children[0];
					r.children[0] = w.children[1];
				} else {
					return false;
				}

				rhs.children.push_back(r);
			}
			break;
		default:
			assert(false);
			break;
		}
	}
	return true;
}

template<typename TLexer>
struct Lexer : lex::lexer<TLexer> {

	Lexer() {
		this->self.add_pattern
			("SIGN", "[-+]")
			("EXPONENT", "[eE]{SIGN}?\\d+")
			("FLOAT", "(\".\"\\d+|\\d+\".\"\\d*){EXPONENT}?|\\d+{EXPONENT}")
			;

		case_set_ = "case-set";
		case_ = "case";
		condition_ = "condition";
		eq_ = "eq";
		delay_ = "\"$\"Delay";
		delta_time_ = "\"$\"DeltaTime";

		real = "{SIGN}?{FLOAT}";
		rational = "{SIGN}?(0|[1-9]\\d*)\"/\"[1-9]\\d*";
		integer = "{SIGN}?\\d+";
		id = "\"%\"[a-zA-Z_][a-zA-Z_0-9:]*";
		keyword = "[$]?[a-zA-Z_]\\w*";

		this->self = lex::token_def<>('\n') | '\r' | '(' | ')' | ' ';
		this->self += case_set_ | case_ | condition_ | eq_ | delay_ | delta_time_;
		this->self += real | rational | integer | id | keyword;
	}

	lex::token_def<std::string> case_set_, case_, condition_, eq_, delay_, delta_time_;
	lex::token_def<std::string> id, keyword;
	lex::token_def<int> integer;
	lex::token_def<flint::lexer::Rational> rational;
	lex::token_def<flint::lexer::Real> real;
};

void RewriteDelayParam(Compound &x, const Expr &expr)
{
	Compound c;
	c.keyword = "minus";
	c.children.push_back(Identifier("%time"));
	c.children.push_back(expr);
	x.children.push_back(c);
}

template<typename TIterator>
struct Grammar : qi::grammar<TIterator, Compound()> {

	template<typename TTokenDef>
	Grammar(TTokenDef const &td)
	: Grammar::base_type(statement)
	{
		using boost::phoenix::at_c;
		using boost::phoenix::push_back;
		using boost::phoenix::val;

		statement = '(' >> bare_statement >> ')';

		bare_statement = equation | conditional;

		equation = td.eq_ [at_c<0>(_val) = _1]
			>> ' ' >> expr [push_back(at_c<1>(_val), _1)]
			>> ' ' >> expr [push_back(at_c<1>(_val), _1)];

		conditional = td.case_set_ [at_c<0>(_val) = _1]
			>> cseq [at_c<1>(_val) = _1];

		cseq = +(' ' >> celem);

		celem = '(' >> cexp >> ')';

		cexp = conditioned_case | unconditioned_case;

		conditioned_case = td.case_ [at_c<0>(_val) = _1]
			>> ' ' >> '(' >> td.condition_
			>> ' ' >> expr [push_back(at_c<1>(_val), _1)]
			>> ')'
			>> ' '
			>> statement [push_back(at_c<1>(_val), _1)];

		unconditioned_case = td.case_ [at_c<0>(_val) = _1]
			>> ' '
			>> statement [push_back(at_c<1>(_val), _1)];

		expr = comp_expr
			| td.real
			| td.rational
			| td.integer
			| td.id
			| td.keyword;

		comp_expr = '(' >> bare_expr >> ')';

		bare_expr = delay_expr | delta_time_expr | eq_expr | general_expr;

		delay_expr = td.delay_ [at_c<0>(_val) = val("$lookback")]
			>> ' ' >> expr [push_back(at_c<1>(_val), _1)]
			>> ' ' >> expr [bind(&RewriteDelayParam, _val, _1)];

		delta_time_expr = td.delta_time_ >> ' ' >> td.id [bind(&RewriteDeltaTime, _val, _1)];

		eq_expr = td.eq_ [at_c<0>(_val) = _1]
			>> seq1 [at_c<1>(_val) = _1];

		general_expr = td.keyword [at_c<0>(_val) = _1]
			>> seq0 [at_c<1>(_val) = _1];

		seq0 = *rest;

		seq1 = +rest;

		rest = ' ' >> expr;
	}

	qi::rule<TIterator, Expr()> expr, rest;
	qi::rule<TIterator, Compound()> statement, bare_statement, equation,
		conditional, celem, cexp, conditioned_case, unconditioned_case,
		comp_expr, bare_expr, delay_expr, delta_time_expr, eq_expr, general_expr;
	qi::rule<TIterator, std::deque<Expr>()> cseq, seq0, seq1;
};

/*
 * This class creates and keeps both tokens and grammar objects which
 * construction is expensive in terms of performance.
 */
class Parser {
public:
	typedef const char *base_iterator_type;
	typedef lex::lexertl::token<base_iterator_type> token_type;
	typedef lex::lexertl::lexer<token_type> lexer_type;
	typedef Lexer<lexer_type> RealLexer;
	typedef Grammar<RealLexer::iterator_type> RealGrammar;

	explicit Parser(System *output)
		: tokens_()
		, grammar_(tokens_)
		, da_()
		, output_(output)
	{
	}

	bool Load(sqlite3 *db) {
		return da_.Load(db);
	}

	int Parse(const boost::uuids::uuid &uuid, const char *math) {
		base_iterator_type it = math;
		base_iterator_type eit = math + std::strlen(math);
		Compound statement;
		bool r = lex::tokenize_and_parse(it, eit, tokens_, grammar_, statement);
		if (!r || it != eit) {
			std::cerr << "failed to parse equation: " << it << std::endl;
			return 1;
		}
		return ProcessUuidAndStatement(uuid, statement);
	}

private:
	int ProcessUuidAndStatement(const boost::uuids::uuid &uuid, Compound &statement)
	{
		Expr lhs, rhs;
		if (IsConditional(statement)) {
			Compound c;
			if (!TransformConditional(statement, lhs, c)) {
				ReportInvalidLeafOfCaseSet(uuid);
				return 1;
			}
			rhs = c;
		} else if (IsEquation(statement)) {
			lhs = statement.children[0];
			rhs = statement.children[1];
		} else {
			assert(false);
			return 1;
		}
		int lhs_col, lhs_row;
		if (!da_.Analyse(uuid, &lhs, &lhs_col, &lhs_row))
			return 1;
		int rhs_col, rhs_row;
		if (!da_.Analyse(uuid, &rhs, &rhs_col, &rhs_row))
			return 1;
		if (lhs_col != rhs_col) {
			std::cerr << "col mismatch between RHS and LHS" << std::endl;
			return 1;
		}
		if (lhs_row != rhs_row) {
			std::cerr << "row mismatch between RHS and LHS" << std::endl;
			return 1;
		}
		if (lhs.which() == kExprIsCompound) {
			const Compound &c = boost::get<Compound>(lhs);
			return AcceptCompound(uuid, c, lhs_col, lhs_row, rhs) ? 0 : 1;
		} else if (lhs.which() == kExprIsIdentifier) {
			output_->Add(uuid, Def(boost::get<Identifier>(lhs).name, lhs_col, lhs_row, rhs));
		} else {
			std::cerr << "unsupported form of equation" << std::endl; // TODO
			return 1;
		}
		return 0;
	}

	bool AcceptCompound(const boost::uuids::uuid &uuid, const Compound &c,
						int col, int row,
						Expr rhs, Expr mass = 1)
	{
		if (c.keyword == "diff") {
			assert(c.children.size() == 2);
			if (!IsBoundVariable(c.children[0]))
				return false;
			const Expr &e1 = c.children[1];
			if (e1.which() == kExprIsIdentifier) {
				output_->Add(uuid, Ode(boost::get<Identifier>(e1).name,
									   col, row, rhs, mass));
				return true;
			}
		} else if (c.keyword == "times") {
			if (c.children.size() == 2) {
				const Expr &e1 = c.children[1];
				if (e1.which() == kExprIsCompound)
					return AcceptCompound(uuid, boost::get<Compound>(e1),
										  col, row, rhs, c.children[0]);
			}
		}
		std::cerr << "unsupported form of equation's LHS" << std::endl; // TODO
		return false;
	}

	RealLexer tokens_;
	RealGrammar grammar_;
	DimensionAnalyzer da_;
	System *output_;
};

int Process(void *data, int argc, char **argv, char **names)
{
	Parser *parser = static_cast<Parser *>(data);
	(void)names;
	assert(argc == 2);
	assert(argv[0]);
	boost::uuids::uuid u;
	std::memcpy(&u, argv[0], u.size());
	return parser->Parse(u, argv[1]);
}

}

bool AnnotateEquations(sqlite3 *db, const char *input, System *output)
{
	std::unique_ptr<Parser> parser(new Parser(output));
	if (!parser->Load(db))
		return false;

	std::ostringstream oss;
	oss << "SELECT * FROM " << input;
	std::string query = oss.str();

	char *em;
	int e = sqlite3_exec(db, query.c_str(), Process, parser.get(), &em);
	if (e != SQLITE_OK) {
		if (e != SQLITE_ABORT)
			std::cerr << "failed to select " << input
				 << ": " << e << ": " << em << std::endl;
		sqlite3_free(em);
		return false;
	}
	return true;
}

}
}