/*
 * Software License Agreement (New BSD License)
 *
 * Copyright (c) 2013, Keith Leung, Felipe Inostroza
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Advanced Mining Technology Center (AMTC), the
 *       Universidad de Chile, nor the names of its contributors may be
 *       used to endorse or promote products derived from this software without
 *       specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE AMTC, UNIVERSIDAD DE CHILE, OR THE COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef PARTICLE_FILTER_HPP
#define PARTICLE_FILTER_HPP

#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition.hpp>
#include "Particle.hpp"
#include "ProcessModel.hpp"
#include "MeasurementModel.hpp"
#include <vector>

/**
 * \class ParticleFilter
 * \brief A class containing functions for implementing the particle filter
 * \tparam ProcessModel class for the process model for propagating particles
 * \tparam MeasurementModel class for the measurement model for updateing particles
 * \author Keith Leung
 */
template< class ProcessModel, class MeasurementModel>
class ParticleFilter
{
public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  typedef typename ProcessModel::TState TPose;
  typedef typename ProcessModel::TInput TInput;
  typedef typename MeasurementModel::TMeasurement TMeasure;
  typedef Particle<TPose>* pParticle;
  typedef std::vector<pParticle> TParticleSet;

  /**
   * \brief Configurations for this ParticleFilter
   */
  struct Config{
    /** Number of threads to use for computations.
     *  Right now, the overheads of multithreading
     *  does not make it worth-while
     */
    unsigned int nThreadsPropagationStep_;
  }PFconfig;

  /** Defailt constructor */
  ParticleFilter();

  /**
   * Constructor
   * \param[in] n number of particles
   * \param[in] initState if not NULL, all particles will take this initial state
   */
  ParticleFilter(int n, TPose* initState = NULL);

  /** Default destructor */
  ~ParticleFilter();

  /**
   * Get the process model pointer
   * \return pointer to process model
   */
  ProcessModel* getProcessModel();

  /**
   * Get the measurement model pointer
   * \return pointer to measurement model
   */
  MeasurementModel* getMeasurementModel();

  /**
   * Set the measurements for use in importance weight calculations
   * \note The input vector gets cleared
   * \param[in] Z vector container of measurements
   */
  void setMeasurements(std::vector<TMeasure> &Z);

  /**
   * Propagate particles using the process model
   * \param[in] input to the process model
   * \param[in] dT time-step of input (not used by all process models)
   */
  void propagate( TInput &input, double const dt = 0);

  /**
   * Calculate and update importance weights for all particles;
   * Derived class needs to implement this method
   */
  virtual void importanceWeighting();

  /**
   * Get the number of particles
   * \return count
   */
  int getParticleCount();

  /**
   * Get the pointer to the particle container
   * \retrun a pointer
   */
  TParticleSet* getParticleSet(){return &particleSet_;}

  /**
   * Set the effective particle count below which we initiate resampling
   * \param[in] t threshold
   */
  void setEffectiveParticleCountThreshold(double t);

  /**
   * Get the effective particle count threshold
   * \return threshold
   */
  double getEffectiveParticleCountThreshold();

  /**
   * Particle resampling using a low variance sampling.
   * Sampling will not occur if number of effective particles is above effNParticles_t_.
   * \param[in] n number of particles in the resampled set.
   *        The default value of 0 will keep the same number of particles
   * \return true if resampling occured
   */
  bool resample( unsigned int n = 0 );

protected:

  int nParticles_; /**< number of particles */
  TParticleSet particleSet_; /**< container for particle pointers */

  ProcessModel* pProcessModel_; /**< Process model pointer */
  MeasurementModel* pMeasurementModel_; /**< Measurement model pointer */

  double effNParticles_t_; /**< Effective particle count threshold for resampling */

  std::vector<TMeasure> measurements_; /**< Container for measurements to use for update of particle weight */

  /**
   * Normalize particle weights so that they sum to 1
   */
  void normalizeWeights();

};

////////// Implementation //////////

template< class ProcessModel, class MeasurementModel>
ParticleFilter<ProcessModel, MeasurementModel>::
ParticleFilter(){
  nParticles_ = 0;
  pProcessModel_ =  new ProcessModel;
  pMeasurementModel_ =  new MeasurementModel;
}

template< class ProcessModel, class MeasurementModel>
ParticleFilter<ProcessModel, MeasurementModel>::
ParticleFilter(int n, TPose* initState){

  // initiate particles
  nParticles_ = n;
  particleSet_.resize(nParticles_);

  bool noInitState = true;
  if(initState == NULL){
    typename TPose::Vec x0;
    x0.setZero();
    initState = new TPose(x0, 0);
  }else{
    noInitState = false;
  }

  double newParticleWeight = 1;
  for( int i = 0 ; i < nParticles_ ; i++ ){
    particleSet_[i] = new Particle<TPose>(i, *initState, newParticleWeight);
  }

  pProcessModel_ =  new ProcessModel;
  pMeasurementModel_ =  new MeasurementModel;

  effNParticles_t_ = double(nParticles_)/4.0; // default is 1/4 of n

  // set random seed for particle resampling
  srand48((unsigned int)time(NULL));

  if( noInitState ){
    delete initState;
  }
}

template< class ProcessModel, class MeasurementModel>
ParticleFilter<ProcessModel, MeasurementModel>::~ParticleFilter(){

  for( int i = 0 ; i < nParticles_ ; i++ ){
    delete particleSet_[i];
  }
  delete pProcessModel_;
  delete pMeasurementModel_;
}

template< class ProcessModel, class MeasurementModel>
ProcessModel* ParticleFilter<ProcessModel, MeasurementModel>::
getProcessModel(){
  return pProcessModel_;
}

template< class ProcessModel, class MeasurementModel>
MeasurementModel* ParticleFilter<ProcessModel, MeasurementModel>::
getMeasurementModel(){
  return pMeasurementModel_;
}

template< class ProcessModel, class MeasurementModel>
void ParticleFilter<ProcessModel, MeasurementModel>::setMeasurements(std::vector<TMeasure> &Z){
  measurements_.swap(Z);
  Z.clear();
}

template< class ProcessModel, class MeasurementModel>
void ParticleFilter<ProcessModel, MeasurementModel>::propagate( TInput &input,
								double const dt){
  TPose x_km, x_k;
  for( int i = 0 ; i < nParticles_ ; i++ ){
    particleSet_[i]->getPose( x_km );
    pProcessModel_->sample( x_k, x_km, input, dt);
    particleSet_[i]->setPose( x_k );
  }
}

template< class ProcessModel, class MeasurementModel>
void ParticleFilter<ProcessModel, MeasurementModel>::importanceWeighting(){
  return;
}

template< class ProcessModel, class MeasurementModel>
void ParticleFilter<ProcessModel, MeasurementModel>::normalizeWeights(){

  double sum = 0;
  for( int i = 0; i < nParticles_; i++ ){
    sum += particleSet_[i]->getWeight();
  }
  for( int i = 0; i < nParticles_; i++ ){
    particleSet_[i]->setWeight( particleSet_[i]->getWeight() / sum );
  }

}

template< class ProcessModel, class MeasurementModel>
int ParticleFilter<ProcessModel, MeasurementModel>::getParticleCount(){
  return nParticles_;
}

template< class ProcessModel, class MeasurementModel>
void ParticleFilter<ProcessModel, MeasurementModel>::
setEffectiveParticleCountThreshold(double t){
  effNParticles_t_ = t;
}

template< class ProcessModel, class MeasurementModel>
double ParticleFilter<ProcessModel, MeasurementModel>::
getEffectiveParticleCountThreshold(){
  return effNParticles_t_;
}

template< class ProcessModel, class MeasurementModel>
bool ParticleFilter<ProcessModel, MeasurementModel>::resample( unsigned int n ){

  // Check effective number of particles
  double sum_of_weight_squared = 0;
  normalizeWeights(); // sum of all particle weights is now 1
  for( int i = 0; i < nParticles_; i++ ){
    double w_i = particleSet_[i]->getWeight();
    sum_of_weight_squared += (w_i * w_i); // and divide by 1
  }
  double nEffParticles_ = 1.0 / sum_of_weight_squared;
  if( nEffParticles_ > effNParticles_t_ ){
    return false; // no resampling
  }

  if( n == 0 )
    n = nParticles_; // number of particles to sample

  // Sampler settings
  double randomNum_0_to_1 = drand48();
  unsigned int idx = 0;
  const double sample_interval = 1.0 / double(n);
  const double sampler_offset = sample_interval * randomNum_0_to_1;
  double sample_point = sampler_offset;
  double cumulative_weight = particleSet_[idx]->getWeight();

  // book-keeping
  std::vector<char> flag_particle_sampled (nParticles_, 0);
  std::vector<unsigned int> sampled_idx (n, 0);

  // Sample
  for( int i = 0; i < n; i++ ){

    while( sample_point > cumulative_weight ){
      // particle[idx] not sampled
      idx++;
      cumulative_weight += particleSet_[idx]->getWeight();
    }
    // particle[idx] sampled
    sampled_idx[i] = idx;
    flag_particle_sampled[idx] = 1;
    sample_point += sample_interval;
  }

  // Do the actual data copying
  unsigned int idx_prev = 0;
  unsigned int next_unsampled_idx = 0;
  for( int i = 0; i < n; i++ ){

    bool firstTime = true;
    idx = sampled_idx[i]; // particle[idx] was sampled

    if(i > 0 && idx == idx_prev ){
      firstTime = false;
    }
    idx_prev = idx;

    // cases:
    // 1. idx < n AND idx appears for first time -> do nothing
    // 2. idx < n AND it is not the first time that idx appears -> make copy
    // 3. idx > n AND idx appears for first time -> make copy
    // 4. idx > n AND it is not first time that idx appears -> make copy

    if( idx < n && firstTime){ // case 1
      particleSet_[idx]->setParentId( particleSet_[idx]->getId() );
    }else{ // case 2, 3, 4

      if( next_unsampled_idx < nParticles_ ){
	while( flag_particle_sampled[next_unsampled_idx] == 1)
	  next_unsampled_idx++;
      }

      particleSet_[next_unsampled_idx]->setParentId( particleSet_[idx]->getId() );
      particleSet_[idx]->copyStateTo( particleSet_[next_unsampled_idx] );

      next_unsampled_idx++;
    }
  }

  // Delete all particles with idx >= n
  for( int i = n; i < nParticles_; i++ ){
    delete particleSet_[i];
  }
  nParticles_ = n;

  // Reset weight of all particles
  for( int i = 0; i < n; i++ ){
    particleSet_[i]->setWeight(1);
  }

  return true;

}

#endif


/*
 * Copyright (c) 2017 Louis Langholtz https://github.com/louis-langholtz/PlayRho
 *
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

/**
 * @file
 *
 * @brief Units file.
 *
 * @details This file establishes quantity aliases, unit constants, and associated code
 *   for the expression of physical quantities using recognizably named units of those
 *   quantities. Quantities, as used herein, are types associated with physical quantities
 *   like time and length. Conceptually a given quantity is only expressable in units that
 *   relate to that quantity. For every quantity defined herein, there's typically at least
 *   one conceptually typed unit asscociated with it.
 *
 * @note In the simplest way that the PlayRho library can be configured, there's no compiler
 *   enforcement on the usage of the units for their associated quantities beyond the usage
 *   of the Real type.
 */

#ifndef PLAYRHO_COMMON_UNITS_HPP
#define PLAYRHO_COMMON_UNITS_HPP

#include <PlayRho/Common/RealConstants.hpp>
#include <PlayRho/Common/Templates.hpp>
#include <type_traits>
#include <cmath>

// #define USE_BOOST_UNITS
#if defined(USE_BOOST_UNITS)
#include <boost/units/io.hpp>
#include <boost/units/limits.hpp>
#include <boost/units/cmath.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <boost/units/systems/si/acceleration.hpp>
#include <boost/units/systems/si/frequency.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <boost/units/systems/si/mass.hpp>
#include <boost/units/systems/si/momentum.hpp>
#include <boost/units/systems/si/inverse_mass.hpp>
#include <boost/units/systems/si/area.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/angular_momentum.hpp>
#include <boost/units/systems/si/angular_velocity.hpp>
#include <boost/units/systems/si/angular_acceleration.hpp>
#include <boost/units/systems/si/second_moment_of_area.hpp>
#include <boost/units/systems/si/surface_density.hpp>
#include <boost/units/systems/si/moment_of_inertia.hpp>
#include <boost/units/systems/si/inverse_moment_of_inertia.hpp>
#include <boost/units/systems/si/force.hpp>
#include <boost/units/systems/si/torque.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#endif // defined(USE_BOOST_UNITS)

// Define quantity and unit related macros to abstract away C-preprocessor definitions
#if defined(USE_BOOST_UNITS)
#define PLAYRHO_QUANTITY(BoostDimension) boost::units::quantity<BoostDimension, Real>
#define PLAYRHO_UNIT(Quantity, BoostUnit) Quantity{BoostUnit * Real{1}}
#define PLAYRHO_DERIVED_UNIT(Quantity, BoostUnit, Ratio) Quantity{BoostUnit * Real{Ratio}}
#else // defined(USE_BOOST_UNITS)
#define PLAYRHO_QUANTITY(BoostDimension) Real
#define PLAYRHO_UNIT(Quantity, BoostUnit) Real{1}
#define PLAYRHO_DERIVED_UNIT(Quantity, BoostUnit, Ratio) Real{Ratio}}
#endif // defined(USE_BOOST_UNITS)

namespace playrho
{
    /// @defgroup PhysicalQuantities Physical Quantity Types
    /// @brief Types for physical quantities.
    /// @details These are the type aliases for physical quantities like time and length
    ///   that are used by the PlayRho library.
    ///   Conceptually a given quantity is only expressable in the units that are defined
    ///   for that quantity.
    /// @see PhysicalUnits
    /// @see https://en.wikipedia.org/wiki/List_of_physical_quantities
    /// @{

    /// @brief Time quantity.
    /// @details This is the type alias for the time base quantity.
    /// @note This quantity's dimension is: time (T).
    /// @note The SI unit of time is the second.
    /// @see Second.
    /// @see https://en.wikipedia.org/wiki/Time_in_physics
    using Time = PLAYRHO_QUANTITY(boost::units::si::time);

    /// @brief Frequency quantity.
    /// @details This is the type alias for the frequency quantity. It's a derived quantity
    ///   that's the inverse of time.
    /// @note This quantity's dimension is: inverse time (T^-1).
    /// @note The SI unit of frequency is the hertz.
    /// @see Time.
    /// @see Hertz.
    /// @see https://en.wikipedia.org/wiki/Frequency
    using Frequency = PLAYRHO_QUANTITY(boost::units::si::frequency);

    /// @brief Length quantity.
    /// @details This is the type alias for the length base quantity.
    /// @note This quantity's dimension is: length (L).
    /// @note The SI unit of length is the meter.
    /// @see Meter.
    /// @see https://en.wikipedia.org/wiki/Length
    using Length = PLAYRHO_QUANTITY(boost::units::si::length);

    /// @brief Linear velocity quantity.
    /// @details This is the type alias for the linear velocity derived quantity.
    /// @note This quantity's dimensions are: length over time (L T^-1).
    /// @note The SI unit of linear velocity is meters per second.
    /// @see Length, Time.
    /// @see MeterPerSecond.
    /// @see https://en.wikipedia.org/wiki/Speed
    using LinearVelocity = PLAYRHO_QUANTITY(boost::units::si::velocity);

    /// @brief Linear acceleration quantity.
    /// @details This is the type alias for the linear acceleration derived quantity.
    /// @note This quantity's dimensions are: length over time squared (L T^-2).
    /// @note The SI unit of linear acceleration is meters per second squared.
    /// @see Length, Time, LinearVelocity.
    /// @see MeterPerSquareSecond.
    /// @see https://en.wikipedia.org/wiki/Acceleration
    using LinearAcceleration = PLAYRHO_QUANTITY(boost::units::si::acceleration);

    /// @brief Mass quantity.
    /// @details This is the type alias for the mass base quantity.
    /// @note This quantity's dimension is: mass (M).
    /// @note The SI unit of mass is the kilogram.
    /// @see Kilogram.
    /// @see https://en.wikipedia.org/wiki/Mass
    using Mass = PLAYRHO_QUANTITY(boost::units::si::mass);

    /// @brief Inverse mass quantity.
    /// @details This is the type alias for the inverse mass quantity. It's a derived quantity
    ///   that's the inverse of mass.
    /// @note This quantity's dimension is: inverse mass (M^-1).
    /// @see Mass.
    using InvMass = PLAYRHO_QUANTITY(boost::units::si::inverse_mass);

    /// @brief Area quantity.
    /// @details This is the type alias for the area quantity. It's a derived quantity.
    /// @note This quantity's dimension is: length squared (L^2).
    /// @note The SI unit of area is the square-meter.
    /// @see Length.
    /// @see SquareMeter.
    /// @see https://en.wikipedia.org/wiki/Area
    using Area = PLAYRHO_QUANTITY(boost::units::si::area);

    /// @brief Area (surface) density quantity.
    /// @details This is the type alias for the area density quantity. It's a derived quantity.
    /// @note This quantity's dimensions are: mass per area (M L^-2).
    /// @note The SI derived unit of area density is kilogram per meter-squared.
    /// @see Mass, Area.
    /// @see KilogramPerSquareMeter.
    /// @see https://en.wikipedia.org/wiki/Area_density
    using AreaDensity = PLAYRHO_QUANTITY(boost::units::si::surface_density);

    /// @brief Angle quantity.
    /// @details This is the type alias for the plane angle base quantity.
    /// @note This quantity's dimension is: plane angle (QP).
    /// @see Radian, Degree.
    using Angle = PLAYRHO_QUANTITY(boost::units::si::plane_angle);

    /// @brief Angular velocity quantity.
    /// @details This is the type alias for the plane angular velocity quantity. It's a
    ///   derived quantity.
    /// @note This quantity's dimensions are: plane angle per time (QP T^-1).
    /// @note The SI derived unit of angular velocity is the radian per second.
    /// @see Angle, Time.
    /// @see RadianPerSecond, DegreePerSecond.
    /// @see https://en.wikipedia.org/wiki/Angular_velocity
    using AngularVelocity = PLAYRHO_QUANTITY(boost::units::si::angular_velocity);

    /// @brief Angular acceleration quantity.
    /// @details This is the type alias for the angular acceleration quantity. It's a
    ///   derived quantity.
    /// @note This quantity's dimensions are: plane angle per time squared (QP T^-2).
    /// @note The SI derived unit of angular acceleration is the radian per second-squared.
    /// @see Angle, Time, AngularVelocity.
    /// @see RadianPerSquareSecond, DegreePerSquareSecond.
    /// @see https://en.wikipedia.org/wiki/Angular_acceleration
    using AngularAcceleration = PLAYRHO_QUANTITY(boost::units::si::angular_acceleration);

    /// @brief Force quantity.
    /// @details This is the type alias for the force quantity. It's a derived quantity.
    /// @note This quantity's dimensions are: length mass per time squared (L M T^-2).
    /// @note The SI derived unit of force is the newton.
    /// @see Length, Mass, Time.
    /// @see Newton.
    /// @see https://en.wikipedia.org/wiki/Force
    using Force = PLAYRHO_QUANTITY(boost::units::si::force);

    /// @brief Torque quantity.
    /// @details This is the type alias for the torque quantity. It's a derived quantity
    ///   that's a rotational force.
    /// @note This quantity's dimensions are: length-squared mass per time-squared per
    ///   angle (L^2 M T^-2 QP^-1).
    /// @note The SI derived unit of torque is the newton meter.
    /// @see Length, Mass, Time, Angle.
    /// @see NewtonMeter.
    /// @see https://en.wikipedia.org/wiki/Torque
    using Torque = PLAYRHO_QUANTITY(boost::units::si::torque);

    /// @brief Second moment of area quantity.
    /// @details This is the type alias for the second moment of area quantity. It's a
    ///   derived quantity.
    /// @note This quantity's dimensions are: length-squared-squared (L^4).
    /// @see Length.
    /// @see https://en.wikipedia.org/wiki/Second_moment_of_area
    using SecondMomentOfArea = PLAYRHO_QUANTITY(boost::units::si::second_moment_of_area);

    /// @brief Rotational inertia quantity.
    /// @details This is the type alias for the rotational inertia quantity. It's a
    ///   derived quantity that's also called the moment of inertia or angular mass.
    /// @note This quantity's dimensions are: length-squared mass per angle-squared
    ///   (L^2 M QP^-2).
    /// @note The SI derived unit of rotational inertia is the kilogram meter-squared
    ///   (kg * m^2).
    /// @see Length, Mass, Angle, InvRotInertia.
    /// @see https://en.wikipedia.org/wiki/Moment_of_inertia
    using RotInertia = PLAYRHO_QUANTITY(boost::units::si::moment_of_inertia);

    /// @brief Inverse rotational inertia quantity.
    /// @details This is the type alias for the inverse rotational inertia quantity. It's
    ///   a derived quantity.
    /// @note This quantity's dimensions are: angle-squared per length-squared per mass
    ///    (L^-2 M^-1 QP^2).
    /// @see Length, Mass, Angle, RotInertia.
    using InvRotInertia = PLAYRHO_QUANTITY(boost::units::si::inverse_moment_of_inertia);

    /// @brief Momentum quantity.
    /// @details This is the type alias for the momentum quantity. It's a derived quantity.
    /// @note This quantity's dimensions are: length mass per time (L M T^-1).
    /// @note The SI derived unit of momentum is the kilogram meter per second.
    /// @note If p is momentum, m is mass, and v is
    ///   velocity, then p = m * v.
    /// @see Length, Mass, Time.
    /// @see NewtonSecond.
    /// @see https://en.wikipedia.org/wiki/Momentum
    using Momentum = PLAYRHO_QUANTITY(boost::units::si::momentum);

    /// @brief Angular momentum quantity.
    /// @details This is the type alias for the angular momentum quantity. It's a derived
    ///   quantity.
    /// @note This quantity's dimensions are: length-squared mass per time per angle
    ///    (L^2 M T^-1 QP^-1).
    /// @note The SI derived unit of angular momentum is the kilogram meter-squared per second.
    /// @see Length, Mass, Time, Angle, Momentum.
    /// @see NewtonMeterSecond.
    /// @see https://en.wikipedia.org/wiki/Angular_momentum
    using AngularMomentum = PLAYRHO_QUANTITY(boost::units::si::angular_momentum);

    /// @}

    /// @defgroup PhysicalUnits Units For Physical Quantities
    /// @brief Units for expressing physical quantities like time and length.
    /// @details These are the unit definitions for expressing physical quantities like time
    ///   and length. Conceptually a given unit is only usable with the quantities that are
    ///   made up of the dimensions which the unit is associated with.
    /// @see PhysicalQuantities.
    /// @{

    /// @brief Second unit of time.
    /// @note This is the SI base unit of time.
    /// @see Time.
    /// @see https://en.wikipedia.org/wiki/Second
    constexpr auto Second = PLAYRHO_UNIT(Time, boost::units::si::second);

    /// @brief Square second unit.
    /// @see Second
    constexpr auto SquareSecond = Second * Second;

    /// @brief Hertz unit of Frequency.
    /// @details Represents the hertz unit of frequency (Hz).
    /// @see Frequency.
    /// @see https://en.wikipedia.org/wiki/Hertz
    constexpr auto Hertz = PLAYRHO_UNIT(Frequency, boost::units::si::hertz);

    /// @brief Meter unit of Length.
    /// @details A unit of the length quantity.
    /// @note This is the SI base unit of length.
    /// @see Length.
    /// @see https://en.wikipedia.org/wiki/Metre
    constexpr auto Meter = PLAYRHO_UNIT(Length, boost::units::si::meter);

    /// @brief Meter per second unit of linear velocity.
    /// @see LinearVelocity.
    constexpr auto MeterPerSecond = PLAYRHO_UNIT(LinearVelocity,
        boost::units::si::meter_per_second);

    /// @brief Meter per square second unit of linear acceleration.
    /// @see LinearAcceleration.
    constexpr auto MeterPerSquareSecond = PLAYRHO_UNIT(LinearAcceleration,
        boost::units::si::meter_per_second_squared);

    /// @brief Kilogram unit of mass.
    /// @note This is the SI base unit of mass.
    /// @see Mass.
    /// @see https://en.wikipedia.org/wiki/Kilogram
    constexpr auto Kilogram = PLAYRHO_UNIT(Mass, boost::units::si::kilogram);

    /// @brief Square meter unit of area.
    /// @see Area.
    constexpr auto SquareMeter = PLAYRHO_UNIT(Area, boost::units::si::square_meter);

    /// @brief Cubic meter unit of volume.
    constexpr auto CubicMeter = Meter * Meter * Meter;

    /// @brief Kilogram per square meter unit of area density.
    /// @see AreaDensity.
    constexpr auto KilogramPerSquareMeter = PLAYRHO_UNIT(AreaDensity,
        boost::units::si::kilogram_per_square_meter);

    /// @brief Radian unit of angle.
    /// @see Angle.
    /// @see Degree.
    constexpr auto Radian = PLAYRHO_UNIT(Angle, boost::units::si::radian);

    /// @brief Degree unit of angle quantity.
    /// @see Angle.
    /// @see Radian.
    constexpr auto Degree = Angle{Radian * Pi / Real{180}};

    /// @brief Square radian unit type.
    /// @see Angle.
    /// @see Radian.
    constexpr auto SquareRadian = Radian * Radian;

    /// @brief Radian per second unit of angular velocity.
    /// @see AngularVelocity.
    /// @see Radian, Second.
    constexpr auto RadianPerSecond = PLAYRHO_UNIT(AngularVelocity,
        boost::units::si::radian_per_second);

    /// @brief Degree per second unit of angular velocity.
    /// @see AngularVelocity.
    /// @see Degree, Second.
    constexpr auto DegreePerSecond = AngularVelocity{RadianPerSecond * Degree / Radian};

    /// @brief Radian per square second unit of angular acceleration.
    /// @see AngularAcceleration.
    /// @see Radian, Second.
    constexpr auto RadianPerSquareSecond = Radian / (Second * Second);

    /// @brief Degree per square second unit of angular acceleration.
    /// @see AngularAcceleration.
    /// @see Degree, Second.
    constexpr auto DegreePerSquareSecond = Degree / (Second * Second);

    /// @brief Newton unit of force.
    /// @see Force.
    constexpr auto Newton = PLAYRHO_UNIT(Force, boost::units::si::newton);

    /// @brief Newton meter unit of torque.
    /// @see Torque.
    /// @see Newton, Meter.
    constexpr auto NewtonMeter = PLAYRHO_UNIT(Torque, boost::units::si::newton_meter);

    /// @brief Newton second unit of momentum.
    /// @see Momentum.
    /// @see Newton, Second.
    constexpr auto NewtonSecond = Newton * Second;

    /// @brief Newton meter second unit of angular momentum.
    /// @see AngularMomentum.
    /// @see Newton, Meter, Second.
    constexpr auto NewtonMeterSecond = NewtonMeter * Second;

    /// @brief Revolutions per minute units of angular velocity.
    /// @see AngularVelocity, Time
    /// @see Minute.
    constexpr auto RevolutionsPerMinute = 2 * Pi * Radian / (Real{60} * Second);

    /// @}

    /// @defgroup Unitsymbols Literals For Unit Symbols
    /// @brief User defined literals for more conveniently setting the value of physical
    ///   quantities.
    /// @see PhysicalQuantities
    /// @see PhysicalUnits
    /// @{

    /// @brief SI unit symbol for a gram unit of Mass.
    /// @see https://en.wikipedia.org/wiki/Gram
    constexpr Mass operator"" _g(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * (Kilogram / Kilo);
    }

    /// @brief SI unit symbol for a gram unit of Mass.
    /// @see https://en.wikipedia.org/wiki/Gram
    constexpr Mass operator"" _g(long double v) noexcept
    {
        return static_cast<Real>(v) * (Kilogram / Kilo);
    }

    /// @brief SI unit symbol for a kilogram unit of Mass.
    /// @see Kilogram
    /// @see https://en.wikipedia.org/wiki/Kilogram
    constexpr Mass operator"" _kg(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Kilogram;
    }

    /// @brief SI unit symbol for a kilogram unit of Mass.
    /// @see Kilogram
    /// @see https://en.wikipedia.org/wiki/Kilogram
    constexpr Mass operator"" _kg(long double v) noexcept
    {
        return static_cast<Real>(v) * Kilogram;
    }

    /// @brief SI unit symbol for a petagram unit of Mass.
    /// @see https://en.wikipedia.org/wiki/Orders_of_magnitude_(mass)
    constexpr Mass operator"" _Pg(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Peta * (Kilogram / Kilo);
    }

    /// @brief SI unit symbol for a petagram unit of Mass.
    /// @see https://en.wikipedia.org/wiki/Orders_of_magnitude_(mass)
    constexpr Mass operator"" _Pg(long double v) noexcept
    {
        return static_cast<Real>(v) * Peta * (Kilogram / Kilo);
    }

    /// @brief SI unit symbol for a yottagram unit of Mass.
    /// @see https://en.wikipedia.org/wiki/Orders_of_magnitude_(mass)
    constexpr Mass operator"" _Yg(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Yotta * (Kilogram / Kilo);
    }

    /// @brief SI unit symbol for a yottagram unit of Mass.
    /// @see https://en.wikipedia.org/wiki/Orders_of_magnitude_(mass)
    constexpr Mass operator"" _Yg(long double v) noexcept
    {
        return static_cast<Real>(v) * Yotta * (Kilogram / Kilo);
    }

    /// @brief SI unit symbol for a meter of Length.
    /// @see Meter
    /// @see https://en.wikipedia.org/wiki/Metre
    constexpr Length operator"" _m(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Meter;
    }

    /// @brief SI unit symbol for a meter of Length.
    /// @see Meter
    /// @see https://en.wikipedia.org/wiki/Metre
    constexpr Length operator"" _m(long double v) noexcept
    {
        return static_cast<Real>(v) * Meter;
    }

    /// @brief SI unit symbol for a decimeter of Length.
    /// @see https://en.wikipedia.org/wiki/Decimetre
    constexpr Length operator"" _dm(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Deci * Meter;
    }

    /// @brief SI unit symbol for a decimeter of Length.
    /// @see https://en.wikipedia.org/wiki/Decimetre
    constexpr Length operator"" _dm(long double v) noexcept
    {
        return static_cast<Real>(v) * Deci * Meter;
    }

    /// @brief SI unit symbol for a centimeter of Length.
    /// @see https://en.wikipedia.org/wiki/Centimetre
    constexpr Length operator"" _cm(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Centi * Meter;
    }

    /// @brief SI unit symbol for a centimeter of Length.
    /// @see https://en.wikipedia.org/wiki/Centimetre
    constexpr Length operator"" _cm(long double v) noexcept
    {
        return static_cast<Real>(v) * Centi * Meter;
    }

    /// @brief SI unit symbol for a gigameter unit of Length.
    /// @see https://en.wikipedia.org/wiki/Gigametre
    constexpr Length operator"" _Gm (unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Giga * Meter;
    }

    /// @brief SI unit symbol for a gigameter unit of Length.
    /// @see https://en.wikipedia.org/wiki/Gigametre
    constexpr Length operator"" _Gm (long double v) noexcept
    {
        return static_cast<Real>(v) * Giga * Meter;
    }

    /// @brief SI unit symbol for a megameter unit of Length.
    /// @see https://en.wikipedia.org/wiki/Megametre
    constexpr Length operator"" _Mm (unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Mega * Meter;
    }

    /// @brief SI unit symbol for a megameter unit of Length.
    /// @see https://en.wikipedia.org/wiki/Megametre
    constexpr Length operator"" _Mm (long double v) noexcept
    {
        return static_cast<Real>(v) * Mega * Meter;
    }

    /// @brief SI symbol for a kilometer unit of Length.
    /// @see https://en.wikipedia.org/wiki/Kilometre
    constexpr Length operator"" _km (unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Kilo * Meter;
    }

    /// @brief SI symbol for a kilometer unit of Length.
    /// @see https://en.wikipedia.org/wiki/Kilometre
    constexpr Length operator"" _km (long double v) noexcept
    {
        return static_cast<Real>(v) * Kilo * Meter;
    }

    /// @brief SI symbol for a second unit of Time.
    /// @see Second
    /// @see https://en.wikipedia.org/wiki/Second
    constexpr Time operator"" _s(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Second;
    }

    /// @brief SI symbol for a second unit of Time.
    /// @see Second
    /// @see https://en.wikipedia.org/wiki/Second
    constexpr Time operator"" _s(long double v) noexcept
    {
        return static_cast<Real>(v) * Second;
    }

    /// @brief SI symbol for a minute unit of Time.
    /// @see https://en.wikipedia.org/wiki/Minute
    constexpr Time operator"" _min(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * 60 * Second;
    }

    /// @brief SI symbol for a minute unit of Time.
    /// @see https://en.wikipedia.org/wiki/Minute
    constexpr Time operator"" _min(long double v) noexcept
    {
        return static_cast<Real>(v) * 60 * Second;
    }

    /// @brief Symbol for an hour unit of Time.
    /// @see https://en.wikipedia.org/wiki/Hour
    constexpr Time operator"" _h(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * 60 * 60 * Second;
    }

    /// @brief Symbol for an hour unit of Time.
    /// @see https://en.wikipedia.org/wiki/Hour
    constexpr Time operator"" _h(long double v) noexcept
    {
        return static_cast<Real>(v) * 60 * 60 * Second;
    }

    /// @brief Symbol for a day unit of Time.
    /// @see https://en.wikipedia.org/wiki/Day
    constexpr Time operator"" _d(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * 60 * 60 * 24 * Second;
    }

    /// @brief Symbol for a day unit of Time.
    /// @see https://en.wikipedia.org/wiki/Day
    constexpr Time operator"" _d(long double v) noexcept
    {
        return static_cast<Real>(v) * 60 * 60 * 24 * Second;
    }

    /// @brief SI symbol for a radian unit of Angle.
    /// @see Radian.
    /// @see https://en.wikipedia.org/wiki/Radian
    constexpr Angle operator"" _rad(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Radian;
    }

    /// @brief SI symbol for a radian unit of Angle.
    /// @see Radian.
    /// @see https://en.wikipedia.org/wiki/Radian
    constexpr Angle operator"" _rad(long double v) noexcept
    {
        return static_cast<Real>(v) * Radian;
    }

    /// @brief Abbreviation for a degree unit of Angle.
    /// @see Degree.
    /// @see https://en.wikipedia.org/wiki/Degree_(angle)
    constexpr Angle operator"" _deg(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Degree;
    }

    /// @brief Abbreviation for a degree unit of Angle.
    /// @see Degree.
    /// @see https://en.wikipedia.org/wiki/Degree_(angle)
    constexpr Angle operator"" _deg(long double v) noexcept
    {
        return static_cast<Real>(v) * Degree;
    }

    /// @brief SI symbol for a newton unit of Force.
    /// @see Newton
    /// @see https://en.wikipedia.org/wiki/Newton_(unit)
    constexpr Force operator"" _N(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Newton;
    }

    /// @brief SI symbol for a newton unit of Force.
    /// @see Newton
    /// @see https://en.wikipedia.org/wiki/Newton_(unit)
    constexpr Force operator"" _N(long double v) noexcept
    {
        return static_cast<Real>(v) * Newton;
    }

    /// @brief Abbreviation for meter squared unit of Area.
    /// @see SquareMeter
    constexpr Area operator"" _m2(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * SquareMeter;
    }

    /// @brief Abbreviation for meter squared unit of Area.
    /// @see SquareMeter
    constexpr Area operator"" _m2(long double v) noexcept
    {
        return static_cast<Real>(v) * SquareMeter;
    }

    /// @brief Abbreviation for meter per second.
    /// @see https://en.wikipedia.org/wiki/Metre_per_second
    /// @see Meter
    /// @see Second
    /// @see MeterPerSecond
    constexpr LinearVelocity operator"" _mps(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * MeterPerSecond;
    }

    /// @brief Abbreviation for meter per second.
    /// @see https://en.wikipedia.org/wiki/Metre_per_second
    /// @see Meter
    /// @see Second
    /// @see MeterPerSecond
    constexpr LinearVelocity operator"" _mps(long double v) noexcept
    {
        return static_cast<Real>(v) * MeterPerSecond;
    }

    /// @brief Abbreviation for kilometer per second.
    /// @see https://en.wikipedia.org/wiki/Metre_per_second
    /// @see Second
    constexpr LinearVelocity operator"" _kps(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Kilo * MeterPerSecond;
    }

    /// @brief Abbreviation for kilometer per second.
    /// @see https://en.wikipedia.org/wiki/Metre_per_second
    /// @see Second
    constexpr LinearVelocity operator"" _kps(long double v) noexcept
    {
        return static_cast<Real>(v) * Kilo * MeterPerSecond;
    }

    /// @brief Abbreviation for meter per second squared.
    /// @see https://en.wikipedia.org/wiki/Metre_per_second_squared
    /// @see MeterPerSquareSecond
    constexpr LinearAcceleration operator"" _mps2(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * MeterPerSquareSecond;
    }

    /// @brief Abbreviation for meter per second squared.
    /// @see https://en.wikipedia.org/wiki/Metre_per_second_squared
    /// @see MeterPerSquareSecond
    constexpr LinearAcceleration operator"" _mps2(long double v) noexcept
    {
        return static_cast<Real>(v) * MeterPerSquareSecond;
    }

    /// @brief SI symbol for a hertz unit of Frequency.
    /// @see Hertz
    /// @see https://en.wikipedia.org/wiki/Hertz
    constexpr Frequency operator"" _Hz(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * Hertz;
    }

    /// @brief SI symbol for a hertz unit of Frequency.
    /// @see Hertz
    /// @see https://en.wikipedia.org/wiki/Hertz
    constexpr Frequency operator"" _Hz(long double v) noexcept
    {
        return static_cast<Real>(v) * Hertz;
    }

    /// @brief Abbreviation for newton-meter unit of torque.
    /// @see NewtonMeter
    /// @see https://en.wikipedia.org/wiki/Newton_metre
    constexpr Torque operator"" _Nm(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * NewtonMeter;
    }

    /// @brief Abbreviation for newton-meter unit of torque.
    /// @see NewtonMeter
    /// @see https://en.wikipedia.org/wiki/Newton_metre
    constexpr Torque operator"" _Nm(long double v) noexcept
    {
        return static_cast<Real>(v) * NewtonMeter;
    }

    /// @brief SI symbol for a newton second of impulse.
    /// @see NewtonSecond
    /// @see https://en.wikipedia.org/wiki/Newton_second
    constexpr Momentum operator"" _Ns(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * NewtonSecond;
    }

    /// @brief SI symbol for a newton second of impulse.
    /// @see NewtonSecond
    /// @see https://en.wikipedia.org/wiki/Newton_second
    constexpr Momentum operator"" _Ns(long double v) noexcept
    {
        return static_cast<Real>(v) * NewtonSecond;
    }

    /// @brief Abbreviation for kilogram per square meter.
    constexpr AreaDensity operator"" _kgpm2(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * KilogramPerSquareMeter;
    }

    /// @brief Abbreviation for kilogram per square meter.
    constexpr AreaDensity operator"" _kgpm2(long double v) noexcept
    {
        return static_cast<Real>(v) * KilogramPerSquareMeter;
    }

    /// @brief Abbreviation for revolutions per minute.
    /// @see RevolutionsPerMinute
    constexpr AngularVelocity operator"" _rpm(unsigned long long int v) noexcept
    {
        return static_cast<Real>(v) * RevolutionsPerMinute;
    }

    /// @brief Abbreviation for revolutions per minute.
    /// @see RevolutionsPerMinute
    constexpr AngularVelocity operator"" _rpm(long double v) noexcept
    {
        return static_cast<Real>(v) * RevolutionsPerMinute;
    }

    /// @}

    /// @brief Strips the units off of the given value.
    constexpr Real StripUnit(const Real value)
    {
        return value;
    }

    /// @defgroup UnitConstants Physical Constants
    /// @brief Definitions of universal and Earthly physical constants.
    /// @see PhysicalQuantities
    /// @see PhysicalUnits
    /// @{

    /// @brief Earthly gravity.
    /// @details An approximation of the average acceleration of Earthly objects towards
    ///   the Earth due to the Earth's gravity.
    /// @note This constant is only appropriate for use for objects of low mass and close
    ///   distance relative to the Earth.
    /// @see https://en.wikipedia.org/wiki/Gravity_of_Earth
    constexpr auto EarthlyLinearAcceleration = Real{-9.8f} * MeterPerSquareSecond;

    /// @brief Big "G".
    /// @details Gravitational constant used in calculating the attractive force on a mass
    ///   to another mass at a given distance due to gravity.
    /// @see https://en.wikipedia.org/wiki/Gravitational_constant
    constexpr auto BigG = Real{6.67408e-11f} * CubicMeter / (Kilogram * SquareSecond);

    /// @}

#if defined(USE_BOOST_UNITS)
    using boost::units::isfinite;
    using boost::units::isnormal;
    using boost::units::cos;
    using boost::units::sin;

    // Don't use boost's hypot since it does type promotion which is problematic.
    // using boost::units::hypot;

    /// @brief Gets the hypotenuse.
    /// @note Don't use boost's hypot since it does type promotion which is problematic.
    /// @see https://en.cppreference.com/w/cpp/numeric/math/hypot
    /// @see https://en.wikipedia.org/wiki/Hypotenuse
    template<class Unit>
    inline auto
    hypot(const boost::units::quantity<Unit,Real>& x, const boost::units::quantity<Unit,Real>& y)
    {
        using std::hypot;
        return boost::units::quantity<Unit,Real>::from_value(hypot(x.value(), y.value()));
    }

    /// @brief Square roots the given value.
    /// @note Don't use boost's sqrt implementation as it promotes the quantity's given
    ///   underlying floating-point type which seems contrary in this case to the
    ///   specification of std::sqrt.
    /// @see https://en.cppreference.com/w/cpp/numeric/math/sqrt
    template<class Unit>
    inline auto
    sqrt(const boost::units::quantity<Unit,Real>& q)
    {
        using std::sqrt;
        using quantity_type = typename boost::units::root_typeof_helper<
            boost::units::quantity<Unit,Real>,
            boost::units::static_rational<2>
            >::type;
        using unit_type = typename quantity_type::unit_type;

        return boost::units::quantity<unit_type, Real>::from_value(sqrt(q.value()));
    }

    /// @brief Almost zero.
    template <class Y>
    inline auto AlmostZero(const boost::units::quantity<Y, Real> v)
    {
        return abs(v) < std::numeric_limits<boost::units::quantity<Y, Real>>::min();
    }

    /// @brief Strips the units off of the given value.
    template<class Unit,class Y>
    constexpr auto StripUnit(const boost::units::quantity<Unit, Y> source)
    {
        return source.value();
    }

    /// @brief Gets an invalid value for the Angle type.
    template <>
    constexpr Angle GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Radian;
    }

    /// @brief Gets an invalid value for the Frequency type.
    template <>
    constexpr Frequency GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Hertz;
    }

    /// @brief Gets an invalid value for the AngularVelocity type.
    template <>
    constexpr AngularVelocity GetInvalid() noexcept
    {
        return GetInvalid<Real>() * RadianPerSecond;
    }

    /// @brief Gets an invalid value for the Time type.
    template <>
    constexpr Time GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Second;
    }

    /// @brief Gets an invalid value for the Length type.
    template <>
    constexpr Length GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Meter;
    }

    /// @brief Gets an invalid value for the Mass type.
    template <>
    constexpr Mass GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Kilogram;
    }

    /// @brief Gets an invalid value for the InvMass type.
    template <>
    constexpr InvMass GetInvalid() noexcept
    {
        return GetInvalid<Real>() / Kilogram;
    }

    /// @brief Gets an invalid value for the Momentum type.
    template <>
    constexpr Momentum GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Kilogram * MeterPerSecond;
    }

    /// @brief Gets an invalid value for the Force type.
    template <>
    constexpr Force GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Newton;
    }

    /// @brief Gets an invalid value for the Torque type.
    template <>
    constexpr Torque GetInvalid() noexcept
    {
        return GetInvalid<Real>() * NewtonMeter;
    }

    /// @brief Gets an invalid value for the LinearVelocity type.
    template <>
    constexpr LinearVelocity GetInvalid() noexcept
    {
        return GetInvalid<Real>() * MeterPerSecond;
    }

    /// @brief Gets an invalid value for the LinearAcceleration type.
    template <>
    constexpr LinearAcceleration GetInvalid() noexcept
    {
        return GetInvalid<Real>() * MeterPerSquareSecond;
    }

    /// @brief Gets an invalid value for the AngularAcceleration type.
    template <>
    constexpr AngularAcceleration GetInvalid() noexcept
    {
        return GetInvalid<Real>() * RadianPerSquareSecond;
    }

    /// @brief Gets an invalid value for the RotInertia type.
    template <>
    constexpr RotInertia GetInvalid() noexcept
    {
        // RotInertia is L^2  M    QP^-2
        return GetInvalid<Real>() * SquareMeter * Kilogram / SquareRadian;
    }

#endif // defined(USE_BOOST_UNITS)

} // namespace playrho

#if defined(USE_BOOST_UNITS)
namespace boost {
namespace units {

// Define division and multiplication templated operators in boost::units namespace since
//   boost::units is the consistent namespace of operands for these and this aids with
//   argument dependent lookup (ADL).
//
// Note that while boost::units already defines division and multiplication operator support,
//   that's only for division or multiplication with the same type that the quantity is based
//   on. For example when Real is float, Length{0.0f} * 2.0f is already supported but
//   Length{0.0f} * 2 is not.

/// @brief Division operator.
///
/// @details Supports the division of a playrho::Real based boost::units::quantity
///   by any arithmetic type except playrho::Real.
/// @note This intentionally excludes the playrho::Real type since the playrho::Real
///   type is already supported and supporting it again in this template causes
///   ambiguous overload support.
///
template <class Dimension, typename X, typename = std::enable_if_t<
    playrho::IsArithmetic<X>::value && !std::is_same<X, playrho::Real>::value &&
    std::is_same<decltype(playrho::Real{} / X{}), playrho::Real>::value >
>
constexpr auto operator/ (quantity<Dimension, playrho::Real> lhs, X rhs)
{
    return lhs / playrho::Real(rhs);
}

template <class Dimension, typename X, typename = std::enable_if_t<
    playrho::IsArithmetic<X>::value && !std::is_same<X, playrho::Real>::value &&
    std::is_same<decltype(X{} / playrho::Real{}), playrho::Real>::value >
>
constexpr auto operator/ (X lhs, quantity<Dimension, playrho::Real> rhs)
{
    return playrho::Real(lhs) / rhs;
}

/// @brief Multiplication operator.
///
/// @details Supports the multiplication of a playrho::Real based boost::units::quantity
///   by any arithmetic type except playrho::Real.
/// @note This intentionally excludes the playrho::Real type since the playrho::Real
///   type is already supported and supporting it again in this template causes
///   ambiguous overload support.
///
template <class Dimension, typename X, typename = std::enable_if_t<
    playrho::IsArithmetic<X>::value && !std::is_same<X, playrho::Real>::value &&
    std::is_same<decltype(playrho::Real{} * X{}), playrho::Real>::value> >
constexpr auto operator* (quantity<Dimension, playrho::Real> lhs, X rhs)
{
    return lhs * playrho::Real(rhs);
}

/// @brief Multiplication operator.
///
/// @details Supports the multiplication of a playrho::Real based boost::units::quantity
///   by any arithmetic type except playrho::Real.
/// @note This intentionally excludes the playrho::Real type since the playrho::Real
///   type is already supported and supporting it again in this template causes
///   ambiguous overload support.
///
template <class Dimension, typename X, typename = std::enable_if_t<
    playrho::IsArithmetic<X>::value && !std::is_same<X, playrho::Real>::value &&
    std::is_same<decltype(playrho::Real{} * X{}), playrho::Real>::value> >
constexpr auto operator* (X lhs, quantity<Dimension, playrho::Real> rhs)
{
    return playrho::Real(lhs) * rhs;
}

} // namespace units
} // namespace boost
#endif // defined(USE_BOOST_UNITS)

#undef PLAYRHO_QUANTITY
#undef PLAYRHO_UNIT

#endif // PLAYRHO_COMMON_UNITS_HPP


/*
 *  Copyright (c) 2015 Evgeny Proydakov <lord.tiran@gmail.com>
 *
 *  Permission is hereby granted, free of charge, to any person obtaining a copy
 *  of this software and associated documentation files (the "Software"), to deal
 *  in the Software without restriction, including without limitation the rights
 *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *  copies of the Software, and to permit persons to whom the Software is
 *  furnished to do so, subject to the following conditions:
 *
 *  The above copyright notice and this permission notice shall be included in
 *  all copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 *  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 *  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 *  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 *  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 *  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 *  THE SOFTWARE.
 */

#include <cmath>
#include <chrono>
#include <fstream>
#include <iostream>

#include <boost/graph/graphviz.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list.hpp>

typedef float distance_t;

struct vertex_property
{
    size_t x;
    size_t y;
};

typedef boost::property<boost::edge_weight_t, distance_t> edge_property;

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, vertex_property, edge_property> grid;

typedef boost::graph_traits<grid>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<grid>::vertices_size_type vertices_size_type;

class vertex_hash;

typedef std::vector<vertex_descriptor> vertex_vector;
typedef boost::unordered_set<vertex_descriptor, vertex_hash> vertex_set;

typedef std::vector<std::string> name_vector;

// Exception thrown when the goal vertex is found
struct found_goal {};

///////////////////////////////////////////////////////////////////////////////

// A hash function for vertices.
class vertex_hash
{
public:
    vertex_hash(const grid& graph) : m_graph(graph) {}

    std::size_t operator()(const vertex_descriptor& v) const
    {
        std::size_t seed = 0;
        boost::hash_combine(seed, m_graph[v].x);
        boost::hash_combine(seed, m_graph[v].y);
        return seed;
    }

private:
    const grid& m_graph;
};

///////////////////////////////////////////////////////////////////////////////

//
// Euclidean heuristic for a grid
//
// This calculates the Euclidean distance between a vertex and a goal
// vertex.
//
class euclidean_heuristic : public boost::astar_heuristic<grid, distance_t>
{
public:
    euclidean_heuristic(const grid& graph, const vertex_descriptor& goal) : m_graph(graph), m_goal(goal) {}

    distance_t operator() (vertex_descriptor v) {
        distance_t x = static_cast<distance_t>(std::fabs(m_graph[m_goal].x - m_graph[v].x));
        distance_t y = static_cast<distance_t>(std::fabs(m_graph[m_goal].y - m_graph[v].y));
        return static_cast<distance_t>(std::sqrt(std::pow(x, 2) + std::pow(y, 2)));
    }

private:
    const grid& m_graph;
    vertex_descriptor m_goal;
};

///////////////////////////////////////////////////////////////////////////////

// Visitor that terminates when we find the goal vertex
struct astar_goal_visitor : public boost::default_astar_visitor
{
    astar_goal_visitor(const vertex_descriptor& goal) : m_goal(goal) {}

    void examine_vertex(const vertex_descriptor& u, const grid&) {
        if (u == m_goal) {
            throw found_goal();
        }
    }

private:
    vertex_descriptor m_goal;
};

///////////////////////////////////////////////////////////////////////////////

class grid_map
{
public:
    grid_map(size_t width, size_t height) : m_width(width), m_height(height)
    {
        const size_t size = width * height;
        m_name.resize(size);

        // init horizontal edge
        const distance_t horizontal_size = 1;
        for(size_t x = 0; x < width - 1; x++) {
            for(size_t y = 0; y < height; y++) {
                const size_t index = calc_vertex_index(x, y);
                boost::add_edge(index, index + 1, horizontal_size, m_grid);
            }
        }

        // init vertical edge
        const distance_t vertical_size = 1;
        for(size_t x = 0; x < width; x++) {
            for(size_t y = 0; y < height - 1; y++) {
                const size_t index = calc_vertex_index(x, y);
                boost::add_edge(index, index + width, vertical_size, m_grid);
            }
        }

        // init diagonal edge
        const distance_t diagonal_size = static_cast<distance_t>(std::sqrt(2));
        for(size_t x = 0; x < width - 1; x++) {
            for(size_t y = 0; y < height - 1; y++) {
                const size_t index = calc_vertex_index(x, y);
                boost::add_edge(index, index + width + 1, diagonal_size, m_grid);
                boost::add_edge(index + 1, index + width, diagonal_size, m_grid);
            }
        }

        // fill name and vertex descriptin
        for(size_t x = 0; x < width; x++) {
            for(size_t y = 0; y < height; y++) {
                const size_t index = calc_vertex_index(x, y);
                vertex_descriptor descriptor(index);
                m_grid[descriptor].x = x;
                m_grid[descriptor].y = y;
                std::stringstream sstream;
                sstream << "(" << x << ", " << y << ")";
                m_name[index] = sstream.str();
            }
        }
    }

    size_t calc_vertex_index(size_t x, size_t y)
    {
        return x + y * m_width;
    }

    const grid::vertex_bundled& get_vertex_property(vertex_descriptor vertex) const
    {
        return m_grid[vertex];
    }

    bool search_path(const vertex_descriptor& source, const vertex_descriptor& goal, vertex_vector& solution)
    {
        const size_t solution_assessment_size = m_width + m_height;
        vertex_hash hash(m_grid);

        // The predecessor map is a vertex-to-vertex mapping.
        typedef boost::unordered_map<vertex_descriptor, vertex_descriptor, vertex_hash> pred_map;
        pred_map predecessor(solution_assessment_size, hash);
        boost::associative_property_map<pred_map> pred_pmap(predecessor);

        // The distance map is a vertex-to-distance mapping.
        typedef boost::unordered_map<vertex_descriptor, distance_t, vertex_hash> dist_map;
        dist_map distance(solution_assessment_size, hash);
        boost::associative_property_map<dist_map> dist_pmap(distance);

        euclidean_heuristic heuristic(m_grid, goal);
        astar_goal_visitor  visitor  (goal);

        bool result = false;
        const int iters = 10;

        std::chrono::high_resolution_clock clock;
        auto start = clock.now();

        for(int i = 0; i < iters; i++) {
            solution.clear();
            predecessor.clear();
            distance.clear();
            try {
                boost::astar_search(m_grid, source, heuristic,
                                    boost::visitor(visitor).
                                    predecessor_map(pred_pmap).
                                    distance_map(dist_pmap)
                                    );
            }
            catch(const found_goal&) {
                // Walk backwards from the goal through the predecessor chain adding
                // vertices to the solution path.
                for (vertex_descriptor u = goal; u != source; u = predecessor[u]) {
                    solution.push_back(u);
                }
                solution.push_back(source);
                std::reverse(solution.begin(), solution.end());
                result = true;
            }
        }
        auto end = clock.now();

        distance_t dist = distance[goal];
        std::cout << "solve. result: " << result << " size: " << dist << " process time: "
                  << std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / iters << " us"
                  << std::endl;

        return result;
    }

    void dump_to_dot()
    {
        std::ofstream dot("graph.dot");
        boost::write_graphviz(dot, m_grid, boost::make_label_writer(&m_name[0]));
        dot.close();
    }

private:
    grid        m_grid;
    name_vector m_name;

    size_t m_width;
    size_t m_height;
};

///////////////////////////////////////////////////////////////////////////////

int main()
{
    const size_t w = 5;
    const size_t h = 5;

    grid_map map(w, h);

    vertex_descriptor source = map.calc_vertex_index(0, 0);
    vertex_descriptor goal = map.calc_vertex_index(w - 1, h - 1);

    auto property_source = map.get_vertex_property(source);
    std::cout << "search from: (" << property_source.x << ", " << property_source.y << ")";

    auto property_goal = map.get_vertex_property(goal);
    std::cout << " to: (" << property_goal.x << ", " << property_goal.y << ")" << std::endl;

    vertex_vector solution;
    map.search_path(source, goal, solution);

    map.dump_to_dot();

    return 0;
}


// Copyright (C) 2014 National ICT Australia (NICTA)
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.
// -------------------------------------------------------------------
//
// Written by Conrad Sanderson - http://conradsanderson.id.au
// Written by George Yammine

// Connector for Mex files to use Armadillo for calculation
// Version 0.5

#include <armadillo>
#include <mex.h>
#include <mat.h>
#include <cstring>

using namespace std;
using namespace arma;

// Get scalar value from Matlab/Octave
template<class Type>
inline
Type
armaGetScalar(const mxArray *matlabScalar)
  {
  if(mxGetData(matlabScalar) != NULL)
    {
    return (Type)mxGetScalar(matlabScalar);
    }
  else
    {
    mexErrMsgTxt("No data available.");
    return 0;
    }
  }

// To keep with Matlab/Octave mex functions since functions for double are usually defined in conjunction with the general functions.
inline
double
armaGetDouble(const mxArray *matlabScalar)
  {
  return armaGetScalar<double>(matlabScalar);
  }

// Get non-double real matrix from Matlab/Octave. Type should be case according to input.
// Use mxGetClassID inside main program to test for type.
template<class Type>
inline
Mat<Type>
armaGetData(const mxArray *matlabMatrix, bool copy_aux_mem = false, bool strict = true)
  {
  if(mxGetData(matlabMatrix) != NULL)
    {
    const mwSize n_dim = mxGetNumberOfDimensions(matlabMatrix);

    if(n_dim == 2)
      {
      return Mat<Type>((Type *)mxGetData(matlabMatrix), mxGetM(matlabMatrix), mxGetN(matlabMatrix), copy_aux_mem, strict);
      }
    else
      {
      mexErrMsgTxt("Number of dimensions must be 2.");
      return Mat<Type>();
      }
    }
  else
    {
    mexErrMsgTxt("No data available.");
    return Mat<Type>();
    }
  }

// Get double real matrix from Matlab/Octave.
inline
Mat<double>
armaGetPr(const mxArray *matlabMatrix, bool copy_aux_mem = false, bool strict = true)
  {
  if(mxGetData(matlabMatrix) != NULL)
    {
    const mwSize n_dim = mxGetNumberOfDimensions(matlabMatrix);

    if(n_dim == 2)
      {
      return Mat<double>(mxGetPr(matlabMatrix), mxGetM(matlabMatrix), mxGetN(matlabMatrix), copy_aux_mem, strict);
      }
    else
      {
      mexErrMsgTxt("Number of dimensions must be 2.");
      return Mat<double>();
      }
    }
  else
    {
    mexErrMsgTxt("No data available.");
    return Mat<double>();
    }
  }

// Get non-double imaginary matrix from Matlab/Octave. Type should be case according to input.
// Use mxGetClassID inside main program to test for type.
template<class Type>
inline
Mat<Type>
armaGetImagData(const mxArray *matlabMatrix, bool copy_aux_mem = false, bool strict = true)
  {
  if(mxGetImagData(matlabMatrix) != NULL)
    {
    const mwSize n_dim = mxGetNumberOfDimensions(matlabMatrix);

    if(n_dim == 2)
      {
      return Mat<Type>((Type *)mxGetImagData(matlabMatrix), mxGetM(matlabMatrix), mxGetN(matlabMatrix), copy_aux_mem, strict);
      }
    else
      {
      mexErrMsgTxt("Number of dimensions must be 2.");
      return Mat<Type>();
      }
    }
  else
    {
    mexErrMsgTxt("No data available.");
    return Mat<Type>();
    }
  }

// Get double imaginary matrix from Matlab/Octave.
inline
Mat<double>
armaGetPi(const mxArray *matlabMatrix, bool copy_aux_mem = false, bool strict = true)
  {
  if(mxGetImagData(matlabMatrix) != NULL)
    {
    const mwSize n_dim = mxGetNumberOfDimensions(matlabMatrix);

    if(n_dim == 2)
      {
      return Mat<double>(mxGetPi(matlabMatrix), mxGetM(matlabMatrix), mxGetN(matlabMatrix), copy_aux_mem, strict);
      }
    else
      {
      mexErrMsgTxt("Number of dimensions must be 2.");
      return Mat<double>();
      }
    }
  else
    {
    mexErrMsgTxt("No data available.");
    return Mat<double>();
    }
  }

// Get complex matrix from Matlab/Octave
inline
cx_mat
armaGetCx(const mxArray *matlabMatrix, bool copy_aux_mem = false, bool strict = true)
  {
  if( (mxGetPr(matlabMatrix) != NULL) && (mxGetPi(matlabMatrix) != NULL) )
    {
    return cx_mat(armaGetPr(matlabMatrix, copy_aux_mem, strict), armaGetPi(matlabMatrix, copy_aux_mem, strict));
    }
  else if( (mxGetPr(matlabMatrix) != NULL) && (mxGetPi(matlabMatrix) == NULL) )
    {
    return cx_mat(armaGetPr(matlabMatrix, copy_aux_mem, strict), zeros(mxGetM(matlabMatrix),mxGetN(matlabMatrix)));
    }
  else if( (mxGetPr(matlabMatrix) == NULL) && (mxGetPi(matlabMatrix) != NULL) )
    {
    return cx_mat(zeros(mxGetM(matlabMatrix), mxGetN(matlabMatrix)), armaGetPi(matlabMatrix, copy_aux_mem, strict));
    }
  else
    {
    mexErrMsgTxt("No data available.");
    return cx_mat();
    }
  }

// Return non-double real valued matrix to Matlab/Octave
template<class Type>
inline
void
armaSetData(mxArray *matlabMatrix, const Mat<Type>& armaMatrix)
  {
        Type *dst_pointer = (Type*)mxGetData(matlabMatrix);
  const Type *src_pointer = (Type*)armaMatrix.memptr();

  std::memcpy(dst_pointer, src_pointer, sizeof(Type)*armaMatrix.n_elem);
  }

// Return double real valued matrix to Matlab/Octave
inline
void
armaSetPr(mxArray *matlabMatrix, const Mat<double>& armaMatrix)
  {
        double *dst_pointer = mxGetPr(matlabMatrix);
  const double *src_pointer = armaMatrix.memptr();

  std::memcpy(dst_pointer, src_pointer, sizeof(double)*armaMatrix.n_elem);
  }

// Return imaginary valued matrix to Matlab/Octave.
template<class Type>
inline
void
armaSetImagData(mxArray *matlabMatrix, const Mat<Type>& armaMatrix)
  {
        Type *dst_pointer = (Type*)mxGetImagData(matlabMatrix);
  const Type *src_pointer = (Type*)armaMatrix.memptr();

  std::memcpy(dst_pointer, src_pointer, sizeof(Type)*armaMatrix.n_elem);
  }

// Return double complex valued matrix to Matlab/Octave
inline
void
armaSetPi(mxArray *matlabMatrix, const Mat<double>& armaMatrix)
  {
        double *dst_pointer = mxGetPi(matlabMatrix);
  const double *src_pointer = armaMatrix.memptr();

  std::memcpy(dst_pointer, src_pointer, sizeof(double)*armaMatrix.n_elem);
  }

// Return complex matrix to Matlab/Octave. Requires Matlab/Octave matrix to be mxCOMPLEX
inline
void
armaSetCx(mxArray *matlabMatrix, const cx_mat& armaMatrix)
  {
  armaSetPr(matlabMatrix, real(armaMatrix));
  armaSetPi(matlabMatrix, imag(armaMatrix));
  }

// Cube functions

// Get non-double real cube from Matlab/Octave. Type should be case according to input.
// Use mxGetClassID inside main program to test for type.
template<class Type>
inline
Cube<Type>
armaGetCubeData(const mxArray *matlabMatrix, bool copy_aux_mem = false, bool strict = true)
  {
  if(mxGetData(matlabMatrix) != NULL)
    {
    const mwSize n_dim = mxGetNumberOfDimensions(matlabMatrix);

    if(n_dim == 3)
      {
      const mwSize *dims = mxGetDimensions(matlabMatrix);
      return Cube<Type>((Type *)mxGetData(matlabMatrix), dims[0], dims[1], dims[2], copy_aux_mem, strict);
      }
    else
      {
      mexErrMsgTxt("Number of dimensions must be 3.");
      return Cube<Type>();
      }
    }
  else
    {
    mexErrMsgTxt("No data available.");
    return Cube<Type>();
    }
  }

// Get double cube from Matlab/Octave.
inline
Cube<double>
armaGetCubePr(const mxArray *matlabMatrix, bool copy_aux_mem = false, bool strict = true)
  {
  if(mxGetData(matlabMatrix) != NULL)
    {
    const mwSize n_dim = mxGetNumberOfDimensions(matlabMatrix);

    if(n_dim == 3)
      {
      const mwSize *dims = mxGetDimensions(matlabMatrix);
      return Cube<double>(mxGetPr(matlabMatrix), dims[0], dims[1], dims[2], copy_aux_mem, strict);
      }
    else
      {
      mexErrMsgTxt("Number of dimensions must be 3.");
      return Cube<double>();
      }
    }
  else
    {
    mexErrMsgTxt("No data available.");
    return Cube<double>();
    }
  }

// Get non-double imaginary cube from Matlab/Octave. Type should be case according to input.
// Use mxGetClassID inside main program to test for type.
template<class Type>
inline
Cube<Type>
armaGetCubeImagData(const mxArray *matlabMatrix, bool copy_aux_mem = false, bool strict = true)
  {
  if(mxGetImagData(matlabMatrix) != NULL)
    {
    const mwSize n_dim = mxGetNumberOfDimensions(matlabMatrix);

    if(n_dim == 3)
      {
      const mwSize *dims = mxGetDimensions(matlabMatrix);
      return Cube<Type>((Type *)mxGetImagData(matlabMatrix), dims[0], dims[1], dims[2], copy_aux_mem, strict);
      }
    else
      {
      mexErrMsgTxt("Number of dimensions must be 3.");
      return Cube<Type>();
      }
    }
  else
    {
    mexErrMsgTxt("No data available.");
    return Cube<Type>();
    }
  }

// Get double cube from Matlab/Octave.
inline
Cube<double>
armaGetCubePi(const mxArray *matlabMatrix, bool copy_aux_mem = false, bool strict = true)
  {
  if(mxGetImagData(matlabMatrix) != NULL)
    {
    const mwSize n_dim = mxGetNumberOfDimensions(matlabMatrix);

    if(n_dim == 3)
      {
      const mwSize *dims = mxGetDimensions(matlabMatrix);
      return Cube<double>(mxGetPi(matlabMatrix), dims[0], dims[1], dims[2], copy_aux_mem, strict);
      }
    else
      {
      mexErrMsgTxt("Number of dimensions must be 3.");
      return Cube<double>();
      }
    }
  else
    {
    mexErrMsgTxt("No data available.");
    return Cube<double>();
    }
  }

// Get complex cube from Matlab/Octave
inline
cx_cube
armaGetCubeCx(const mxArray *matlabMatrix, bool copy_aux_mem = false, bool strict = true)
  {
  if( (mxGetPr(matlabMatrix) != NULL) && (mxGetPi(matlabMatrix) != NULL) )
    {
    return cx_cube(armaGetCubePr(matlabMatrix, copy_aux_mem, strict), armaGetCubePi(matlabMatrix, copy_aux_mem, strict));
    }
  else if( (mxGetPr(matlabMatrix) != NULL) && (mxGetPi(matlabMatrix) == NULL) )
    {
    const mwSize *dims = mxGetDimensions(matlabMatrix);
    return cx_cube(armaGetCubePr(matlabMatrix, copy_aux_mem, strict), zeros(dims[0], dims[1], dims[2]));
    }
  else if( (mxGetPr(matlabMatrix) == NULL) && (mxGetPi(matlabMatrix) != NULL) )
    {
    const mwSize *dims = mxGetDimensions(matlabMatrix);
    return cx_cube(zeros(dims[0], dims[1], dims[2]), armaGetCubePi(matlabMatrix, copy_aux_mem, strict));
    }
  else
    {
    mexErrMsgTxt("No data available.");
    return cx_cube();
    }
  }

// return real valued cube to Matlab/Octave
template<class Type>
inline
void
armaSetCubeData(mxArray *matlabMatrix, const Cube<Type>& armaCube)
  {
        Type *dst_pointer = (Type*)mxGetData(matlabMatrix);
  const Type *src_pointer = (Type*)armaCube.memptr();

  std::memcpy(dst_pointer, src_pointer, sizeof(Type)*armaCube.n_elem);
  }

// Return double real valued cube to Matlab/Octave
inline
void
armaSetCubePr(mxArray *matlabMatrix, const Cube<double>& armaCube)
  {
        double *dst_pointer = mxGetPr(matlabMatrix);
  const double *src_pointer = armaCube.memptr();

  std::memcpy(dst_pointer, src_pointer, sizeof(double)*armaCube.n_elem);
  }

// Return imaginary valued cube to Matlab/Octave.
template<class Type>
inline
void
armaSetImagCubeData(mxArray *matlabMatrix, const Cube<Type>& armaCube)
  {
        Type *dst_pointer = (Type*)mxGetImagData(matlabMatrix);
  const Type *src_pointer = (Type*)armaCube.memptr();

  std::memcpy(dst_pointer, src_pointer, sizeof(Type)*armaCube.n_elem);
  }

// Return double imaginary valued matrix to Matlab/Octave
inline
void
armaSetCubePi(mxArray *matlabMatrix, const Cube<double>& armaCube)
  {
        double *dst_pointer = mxGetPi(matlabMatrix);
  const double *src_pointer = armaCube.memptr();

  std::memcpy(dst_pointer, src_pointer, sizeof(double)*armaCube.n_elem);
  }

// Return double complex cube to Matlab/Octave.
void
armaSetCubeCx(mxArray *matlabMatrix, const cx_cube& armaCube)
  {
  armaSetCubePr(matlabMatrix, real(armaCube));
  armaSetCubePi(matlabMatrix, imag(armaCube));
  }

// Sparse matrices

// Get sparse matrix from Matlab/Octave.
template<class Type>
inline
SpMat<Type>
armaGetSparseData(const mxArray *matlabMatrix, bool sort_locations = false)
  {
  if(!mxIsSparse(matlabMatrix))
    {
    mexErrMsgTxt("Matrix is not sparse.");
    return SpMat<Type>();
    }
  else
    {
    Type *pr = (Type *)mxGetData(matlabMatrix);

    if(pr == NULL)
      {
      mexErrMsgTxt("No data available.");
      return SpMat<Type>();
      }

    mwIndex *jc = mxGetJc(matlabMatrix);
    mwIndex *ir = mxGetIr(matlabMatrix);

    mwSize  m = mxGetM(matlabMatrix);
    mwSize  n = mxGetN(matlabMatrix);

    mwSize  non_zero = mxGetNzmax(matlabMatrix);

    umat locations = zeros<umat>(2,non_zero);
    Col<Type> values = zeros< Col<Type> >(non_zero);
    mwSize  row = 0;

    for(mwSize col = 0; col < n ; col++)
      {

      mwIndex starting_row_index = jc[col];
      mwIndex stopping_row_index = jc[col+1];

      if (starting_row_index == stopping_row_index)
        {
        // End of matrix when jc[col] == jc[col+1]
        continue;
        }
      else
        {
        for (mwIndex current_row_index = starting_row_index; current_row_index < stopping_row_index; current_row_index++)
          {
          values[row]=pr[row];
          locations.at(0,row)=ir[current_row_index];
          locations.at(1,row)=col;
          row++;
          }
        }
      }

    return SpMat<Type>(locations, values, m, n, sort_locations);
    }
  }

// Get double valued sparse matrix from Matlab/Octave.
inline
SpMat<double>
armaGetSparseMatrix(const mxArray *matlabMatrix, bool sort_locations = false)
  {
  if(!mxIsSparse(matlabMatrix))
    {
    mexErrMsgTxt("Matrix is not sparse.");
    return SpMat<double>();
    }
  else
    {
    double  *pr = mxGetPr(matlabMatrix);

    if(pr == NULL)
      {
      mexErrMsgTxt("No data available.");
      return SpMat<double>();
      }

    mwIndex *jc = mxGetJc(matlabMatrix);
    mwIndex *ir = mxGetIr(matlabMatrix);

    mwSize  m = mxGetM(matlabMatrix);
    mwSize  n = mxGetN(matlabMatrix);

    mwSize  non_zero = mxGetNzmax(matlabMatrix);

    umat locations = zeros<umat>(2,non_zero);
    Col<double> values = zeros< Col<double> >(non_zero);

    mwSize  row = 0;

    for(mwSize col = 0; col < n ; col++)
      {

      mwIndex starting_row_index = jc[col];
      mwIndex stopping_row_index = jc[col+1];

      if (starting_row_index == stopping_row_index)
        {
        // End of matrix when jc[col] == jc[col+1]
        continue;
        }
      else
        {
        for (mwIndex current_row_index = starting_row_index; current_row_index < stopping_row_index ; current_row_index++)
          {
          values[row]=pr[row];
          locations.at(0,row)=ir[current_row_index];
          locations.at(1,row)=col;
          row++;
          }
        }

      }
    return SpMat<double>(locations, values, m, n, sort_locations);
    }
  }

// Get imaginary sparse matrix from Matlab/Octave.
template<class Type>
inline
SpMat<Type>
armaGetSparseImagData(const mxArray *matlabMatrix, bool sort_locations = false)
  {
  if(!mxIsSparse(matlabMatrix))
    {
    mexErrMsgTxt("Matrix is not sparse.");
    return SpMat<Type>();
    }
  else
    {
    Type *pi = (Type *)mxGetImagData(matlabMatrix);

    if(pi == NULL)
      {
      mexErrMsgTxt("No data available.");
      return SpMat<Type>();
      }

    mwIndex *jc = mxGetJc(matlabMatrix);
    mwIndex *ir = mxGetIr(matlabMatrix);

    mwSize  m = mxGetM(matlabMatrix);
    mwSize  n = mxGetN(matlabMatrix);

    mwSize  non_zero = mxGetNzmax(matlabMatrix);

    umat locations = zeros<umat>(2,non_zero);
    Col<Type> values = zeros< Col<Type> >(non_zero);
    mwSize row = 0;

    for(mwSize col = 0; col < n ; col++)
      {
      mwIndex starting_row_index = jc[col];
      mwIndex stopping_row_index = jc[col+1];

      if (starting_row_index == stopping_row_index)
        {
        // End of matrix when jc[col] == jc[col+1]
        continue;
        }
      else
        {
        for (mwIndex current_row_index = starting_row_index; current_row_index < stopping_row_index; current_row_index++)
          {
          values[row]=pi[row];
          locations.at(0,row)=ir[current_row_index];
          locations.at(1,row)=col;
          row++;
          }
        }
      }

    return SpMat<Type>(locations, values, m, n, sort_locations);
    }
  }

// Get imaginary double valued sparse matrix from Matlab/Octave.
inline
SpMat<double>
armaGetSparseImagMatrix(const mxArray *matlabMatrix, bool sort_locations = false)
  {
  if(!mxIsSparse(matlabMatrix))
    {
    mexErrMsgTxt("Matrix is not sparse.");
    return SpMat<double>();
    }
  else
    {
    double  *pi = mxGetPi(matlabMatrix);

    if(pi == NULL)
      {
      mexErrMsgTxt("No data available.");
      return SpMat<double>();
      }

    mwIndex *jc = mxGetJc(matlabMatrix);
    mwIndex *ir = mxGetIr(matlabMatrix);

    mwSize  m = mxGetM(matlabMatrix);
    mwSize  n = mxGetN(matlabMatrix);

    mwSize  non_zero = mxGetNzmax(matlabMatrix);

    umat locations = zeros<umat>(2,non_zero);
    Col<double> values = zeros< Col<double> >(non_zero);

    mwSize row = 0;

    for(mwSize col = 0; col < n ; col++)
      {
      mwIndex starting_row_index = jc[col];
      mwIndex stopping_row_index = jc[col+1];

      if (starting_row_index == stopping_row_index)
        {
        // End of matrix when jc[col] == jc[col+1]
        continue;
        }
      else
        {
        for (mwIndex current_row_index = starting_row_index; current_row_index < stopping_row_index; current_row_index++)
          {
          values[row]=pi[row];
          locations.at(0,row)=ir[current_row_index];
          locations.at(1,row)=col;
          row++;
          }
        }
      }

    return SpMat<double>(locations, values, m, n, sort_locations);
    }
  }

// Return sparse matrix to matlab
inline
void
armaSetSparsePr(mxArray *matlabMatrix, const SpMat<double>& armaMatrix)
  {
  double  *sr  = mxGetPr(matlabMatrix);
  mwIndex *irs = mxGetIr(matlabMatrix);
  mwIndex *jcs = mxGetJc(matlabMatrix);

  mwSize n_nonzero = armaMatrix.n_nonzero;
  mwSize n_cols    = armaMatrix.n_cols;

  for (mwIndex j = 0; j < n_nonzero; j++)
    {
    sr[j]  = armaMatrix.values[j];
    irs[j] = armaMatrix.row_indices[j];
    }
  for (mwIndex j = 0; j <= n_cols; j++)
    {
    jcs[j] = armaMatrix.col_ptrs[j];
    }
  }

// Return sparse matrix to matlab as imaginary part
inline
void
armaSetSparsePi(mxArray *matlabMatrix, const SpMat<double>& armaMatrix)
  {
  double  *si  = mxGetPi(matlabMatrix);
  mwIndex *irs = mxGetIr(matlabMatrix);
  mwIndex *jcs = mxGetJc(matlabMatrix);

  mwSize n_nonzero = armaMatrix.n_nonzero;
  mwSize n_cols    = armaMatrix.n_cols;

  for (mwIndex j = 0; j < n_nonzero; j++)
    {
    si[j]  = armaMatrix.values[j];
    irs[j] = armaMatrix.row_indices[j];
    }
  for (mwIndex j = 0; j <= n_cols; j++)
    {
    jcs[j] = armaMatrix.col_ptrs[j];
    }
  }

// Create matlab side matrices

// Create 2-D Matlab/Octave matrix
inline
mxArray*
armaCreateMxMatrix(const mwSize n_rows, const mwSize n_cols, const mxClassID mx_type = mxDOUBLE_CLASS, const mxComplexity mx_complexity = mxREAL)
  {
  mxArray *temp = mxCreateNumericMatrix(n_rows, n_cols, mx_type, mx_complexity);

  if(temp == NULL)
    {
    mexErrMsgTxt("Could not create array.");
    return NULL;
    }
  else
    {
    return temp;
    }
  }

// Create 3-D Matlab/Octave matrix (cube)
inline
mxArray*
armaCreateMxMatrix(const mwSize n_rows, const mwSize n_cols, const mwSize n_slices, const mxClassID mx_type = mxDOUBLE_CLASS, const mxComplexity mx_complexity = mxREAL)
  {
  mwSize dims[3] = { n_rows, n_cols, n_slices };

  const mwSize n_dim = 3;

  mxArray *temp = mxCreateNumericArray(n_dim, dims, mx_type, mx_complexity);

  if(temp == NULL)
    {
    mexErrMsgTxt("Could not create array.");
    return NULL;
    }
  else
    {
    return temp;
    }
  }

inline
mxArray*
armaCreateMxSparseMatrix(const mwSize n_rows,const mwSize n_cols,const mwSize n_nonzero,const mxComplexity mx_complexity = mxREAL)
  {
  mxArray *temp = mxCreateSparse(n_rows, n_cols, n_nonzero, mx_complexity);

  if(temp == NULL)
    {
    mexErrMsgTxt("Could not create array.");
    return NULL;
    }
  else
    {
    return temp;
    }
  }

//Functions to write MAT files
inline
int
armaWriteMatToFile(const char *filename, mat &armaMatrix, const char *name)
  {
  MATFile *file;
  file = matOpen(filename,"wz");

  int result;

  if(file == NULL)
    {
    mexErrMsgTxt("Could not create MAT file.");
    return 0;
    }
  else
    {
    mxArray *temp = mxCreateDoubleMatrix(armaMatrix.n_rows, armaMatrix.n_cols, mxREAL);
    armaSetPr(temp, armaMatrix);
    result = matPutVariable(file, name, temp);
    mxDestroyArray(temp); //Cleanup after writing MAT file
    }

  matClose(file);

  return result;
  }

inline
int
armaWriteCxMatToFile(const char *filename, cx_mat &armaMatrix, const char *name)
  {
  MATFile *file;
  file = matOpen(filename,"wz");

  int result;

  if(file == NULL)
    {
    mexErrMsgTxt("Could not create MAT file.");
    return 0;
    }
  else
    {
    mxArray *temp = mxCreateDoubleMatrix(armaMatrix.n_rows, armaMatrix.n_cols, mxCOMPLEX);
    armaSetCx(temp, armaMatrix);
    result = matPutVariable(file, name, temp);
    mxDestroyArray(temp); //Cleanup after writing MAT file
    }

  matClose(file);

  return result;
  }

inline
int
armaWriteCubeToFile(const char *filename, cube &armaCube, const char *name)
  {
  MATFile *file;
  file = matOpen(filename,"wz");

  int result;

  if(file == NULL)
    {
    mexErrMsgTxt("Could not create MAT file.");
    return 0;
    }
  else
    {
    mxArray *temp = armaCreateMxMatrix(armaCube.n_rows, armaCube.n_cols, armaCube.n_slices, mxDOUBLE_CLASS, mxREAL);
    armaSetCubePr(temp, armaCube);
    result = matPutVariable(file, name, temp);
    mxDestroyArray(temp); //Cleanup after writing MAT file
    }

  matClose(file);

  return result;
  }

inline
int
armaWriteCxCubeToFile(const char *filename, cx_cube &armaCube, const char *name)
  {
  MATFile *file;
  file = matOpen(filename,"wz");

  int result;

  if(file == NULL)
    {
    mexErrMsgTxt("Could not create MAT file.");
    return 0;
    }
  else
    {
    mxArray *temp = armaCreateMxMatrix(armaCube.n_rows, armaCube.n_cols, armaCube.n_slices, mxDOUBLE_CLASS, mxCOMPLEX);
    armaSetCubeCx(temp, armaCube);
    result = matPutVariable(file, name, temp);
    mxDestroyArray(temp); //Cleanup after writing MAT file
    }

  matClose(file);

  return result;
  }

//Functions to read and write matrices and cubes in MAT file format
inline
mat
armaReadMatFromFile(const char *filename)
  {
  MATFile *file;
  file = matOpen(filename,"r");

  char buffer[64];
  const char *name;
  name = buffer;

  if(file == NULL)
    {
    mexErrMsgTxt("Could not open MAT file.");
    return mat();
    }
  else
    {
    mat tmp = armaGetPr(matGetNextVariable(file,&name));
    matClose(file);

    return tmp;
    }
  }

inline
cx_mat
armaReadCxMatFromFile(const char *filename)
  {
  MATFile *file;
  file = matOpen(filename,"r");

  char buffer[64];
  const char *name;
  name = buffer;

  if(file == NULL)
    {
    mexErrMsgTxt("Could not open MAT file.");
    return cx_mat();
    }
  else
    {
    cx_mat tmp = armaGetCx(matGetNextVariable(file, &name));
    matClose(file);

    return tmp;
    }
  }

inline
cube
armaReadCubeFromFile(const char *filename)
  {
  MATFile *file;
  file = matOpen(filename,"r");

  char buffer[64];
  const char *name;
  name = buffer;

  if(file == NULL)
    {
    mexErrMsgTxt("Could not open MAT file.");
    return cube();
    }
  else
    {
    cube tmp = armaGetCubePr(matGetNextVariable(file,&name));
    matClose(file);

    return tmp;
    }
  }

inline
cx_cube
armaReadCxCubeFromFile(const char *filename)
  {
  MATFile *file;
  file = matOpen(filename,"r");

  char buffer[64];
  const char *name;
  name = buffer;

  if(file == NULL)
    {
    mexErrMsgTxt("Could not open MAT file.");
    return cx_cube();
    }
  else
    {
    cx_cube tmp = armaGetCubeCx(matGetNextVariable(file,&name));
    matClose(file);

    return tmp;
    }
  }


/* Copyright (c) 2016 - 2020, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#ifndef THERMAL_OPERATOR_HH
#define THERMAL_OPERATOR_HH

#include <MaterialProperty.hh>
#include <ThermalOperatorBase.hh>

#include <deal.II/matrix_free/matrix_free.h>

namespace adamantine
{
/**
 * This class is the operator associated with the heat equation, i.e., vmult
 * performs \f$ dst = -\nabla k \nabla src \f$.
 */
template <int dim, int fe_degree, typename MemorySpaceType>
class ThermalOperator final : public ThermalOperatorBase<dim, MemorySpaceType>
{
public:
  ThermalOperator(MPI_Comm const &communicator,
                  std::shared_ptr<MaterialProperty<dim>> material_properties);

  /**
   * Associate the AffineConstraints<double> and the MatrixFree objects to the
   * underlying Triangulation.
   */
  void reinit(dealii::DoFHandler<dim> const &dof_handler,
              dealii::AffineConstraints<double> const &affine_constraints,
              dealii::QGaussLobatto<1> const &quad) override;

  void reinit(dealii::DoFHandler<dim> const &dof_handler,
              dealii::AffineConstraints<double> const &affine_constraints,
              dealii::QGauss<1> const &quad) override;

  /**
   * Compute the inverse of the mass matrix and update the material properties.
   */
  void compute_inverse_mass_matrix(
      dealii::DoFHandler<dim> const &dof_handler,
      dealii::AffineConstraints<double> const &affine_constraints) override;

  /**
   * Clear the MatrixFree object and resize the inverse of the mass matrix to
   * zero.
   */
  void clear();

  dealii::types::global_dof_index m() const override;

  dealii::types::global_dof_index n() const override;

  /**
   * Return a shared pointer to the inverse of the mass matrix.
   */
  std::shared_ptr<dealii::LA::distributed::Vector<double, MemorySpaceType>>
  get_inverse_mass_matrix() const override;

  /**
   * Return a shared pointer to the underlying MatrixFree object.
   */
  dealii::MatrixFree<dim, double> const &get_matrix_free() const;

  void vmult(dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
             dealii::LA::distributed::Vector<double, MemorySpaceType> const
                 &src) const override;

  void Tvmult(dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
              dealii::LA::distributed::Vector<double, MemorySpaceType> const
                  &src) const override;

  void vmult_add(dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
                 dealii::LA::distributed::Vector<double, MemorySpaceType> const
                     &src) const override;

  void Tvmult_add(dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
                  dealii::LA::distributed::Vector<double, MemorySpaceType> const
                      &src) const override;

  void
  jacobian_vmult(dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
                 dealii::LA::distributed::Vector<double, MemorySpaceType> const
                     &src) const override;

  void initialize_dof_vector(
      dealii::LA::distributed::Vector<double, MemorySpaceType> &vector)
      const override;

  /**
   * Evaluate the material properties for a given state field.
   */
  void evaluate_material_properties(
      dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> const
          &state) override;

  /**
   * Return the value of \f$ \frac{1}{\rho C_p} \f$ for a given cell.
   */
  double get_inv_rho_cp(
      typename dealii::DoFHandler<dim>::cell_iterator const &) const override;

private:
  /**
   * Apply the operator on a given set of quadrature points.
   */
  void local_apply(
      dealii::MatrixFree<dim, double> const &data,
      dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
      dealii::LA::distributed::Vector<double, MemorySpaceType> const &src,
      std::pair<unsigned int, unsigned int> const &cell_range) const;

  /**
   * MPI communicator.
   */
  MPI_Comm const &_communicator;
  /**
   * Data to configure the MatrixFree object.
   */
  typename dealii::MatrixFree<dim, double>::AdditionalData _matrix_free_data;
  /**
   * Store the \f$ \frac{1}{\rho C_p}\f$ coefficient.
   */
  dealii::Table<2, dealii::VectorizedArray<double>> _inv_rho_cp;
  /**
   * Table of thermal conductivity coefficient.
   */
  dealii::Table<2, dealii::VectorizedArray<double>> _thermal_conductivity;
  /**
   * Material properties associated with the domain.
   */
  std::shared_ptr<MaterialProperty<dim>> _material_properties;
  /**
   * Underlying MatrixFree object.
   */
  dealii::MatrixFree<dim, double> _matrix_free;
  /**
   * The inverse of the mass matrix is computed using an inexact Gauss-Lobatto
   * quadrature. This inexact quadrature makes the mass matrix and therefore
   * also its inverse, a diagonal matrix.
   */
  std::shared_ptr<dealii::LA::distributed::Vector<double, MemorySpaceType>>
      _inverse_mass_matrix;
  /**
   * Map between the cell iterator and the position in _inv_rho_cp table.
   */
  std::map<typename dealii::DoFHandler<dim>::cell_iterator,
           std::pair<unsigned int, unsigned int>>
      _cell_it_to_mf_cell_map;
};

template <int dim, int fe_degree, typename MemorySpaceType>
inline dealii::types::global_dof_index
ThermalOperator<dim, fe_degree, MemorySpaceType>::m() const
{
  return _matrix_free.get_vector_partitioner()->size();
}

template <int dim, int fe_degree, typename MemorySpaceType>
inline dealii::types::global_dof_index
ThermalOperator<dim, fe_degree, MemorySpaceType>::n() const
{
  return _matrix_free.get_vector_partitioner()->size();
}

template <int dim, int fe_degree, typename MemorySpaceType>
inline std::shared_ptr<dealii::LA::distributed::Vector<double, MemorySpaceType>>
ThermalOperator<dim, fe_degree, MemorySpaceType>::get_inverse_mass_matrix()
    const
{
  return _inverse_mass_matrix;
}

template <int dim, int fe_degree, typename MemorySpaceType>
inline dealii::MatrixFree<dim, double> const &
ThermalOperator<dim, fe_degree, MemorySpaceType>::get_matrix_free() const
{
  return _matrix_free;
}

template <int dim, int fe_degree, typename MemorySpaceType>
inline void ThermalOperator<dim, fe_degree, MemorySpaceType>::jacobian_vmult(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &dst,
    dealii::LA::distributed::Vector<double, MemorySpaceType> const &src) const
{
  vmult(dst, src);
}

template <int dim, int fe_degree, typename MemorySpaceType>
inline void
ThermalOperator<dim, fe_degree, MemorySpaceType>::initialize_dof_vector(
    dealii::LA::distributed::Vector<double, MemorySpaceType> &vector) const
{
  _matrix_free.initialize_dof_vector(vector);
}

template <int dim, int fe_degree, typename MemorySpaceType>
inline double ThermalOperator<dim, fe_degree, MemorySpaceType>::get_inv_rho_cp(
    typename dealii::DoFHandler<dim>::cell_iterator const &cell) const
{
  auto cell_comp_pair = _cell_it_to_mf_cell_map.find(cell);
  ASSERT(cell_comp_pair != _cell_it_to_mf_cell_map.end(), "Internal error");

  return _inv_rho_cp(cell_comp_pair->second.first,
                     0)[cell_comp_pair->second.second];
}

} // namespace adamantine

#endif