/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology 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
 *    COPYRIGHT HOLDER OR CONTRIBUTORS 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      101125    D. Dirkx          First version of file.
 *      110127    D. Dirkx          Finalized for code check.
 *      110206    J. Melman         Minor formatting issues. Identified multiple warnings.
 *      110207    D. Dirkx          Fixed warning problems by extending cerr comments.
 *      110905    S. Billemont      Reorganized includes.
 *                                  Moved (con/de)structors and getter/setters to header.
 *      120323    D. Dirkx          Removed set functions; moved functionality to constructor,
 *                                  removed raw pointer arrays.
 *    References
 *      Craidon, C.B. A Desription of the Langley Wireframe Geometry Standard (LaWGS) format, NASA
 *          TECHNICAL MEMORANDUM 85767.
 *
 *    Notes
 *
 */

#include <boost/shared_ptr.hpp>

#include "Tudat/Mathematics/GeometricShapes/lawgsPartGeometry.h"

namespace tudat
{
namespace geometric_shapes
{

//! Constructor from surface geometry (i.e., geometry type conversion).
void LawgsPartGeometry::setMesh( boost::shared_ptr< SingleSurfaceGeometry > originalSurface,
                                 int numberOfLinesIn, int numberOfPointsIn )
{
    // Set (temporary name) of part.
    name_ = "copied surface";

    // Set size of mesh.
    numberOfLines_ = numberOfLinesIn;
    numberOfPoints_ = numberOfPointsIn;

    // Allocate mesh points.
    meshPoints_.resize( boost::extents[ numberOfLines_ ][ numberOfPoints_ ] );

    // Set grid sizes from requested number of sample points.
    double independentVariableGridSize1 =
            ( originalSurface->getMaximumIndependentVariable( 1 ) -
              originalSurface->getMinimumIndependentVariable( 1 ) ) /
            ( static_cast< double >( numberOfLines_ - 1 ) );

    double independentVariableGridSize2 =
            ( originalSurface->getMaximumIndependentVariable( 2 ) -
              originalSurface->getMinimumIndependentVariable( 2 ) ) /
            ( static_cast< double >( numberOfPoints_ - 1 ) );

    // Declare sample point variables.
    double variable1, variable2;

    // Declare and set minimum values of independent variables.
    double minimumIndependentVariable1 = originalSurface->getMinimumIndependentVariable( 1 );
    double minimumIndependentVariable2 = originalSurface->getMinimumIndependentVariable( 2 );

    // Loop through the number of lines and points specified and sample
    // geometry at fixed intervals.
    for ( int i = 0; i < numberOfLines_; i++ )
    {
        for ( int j = 0; j < numberOfPoints_; j++ )
        {
            // Set sampling point of original geometry.
            variable1 = minimumIndependentVariable1 + i * independentVariableGridSize1;
            variable2 = minimumIndependentVariable2 + j * independentVariableGridSize2;

            // Set new mesh point.
            meshPoints_[ i ][ j ] = originalSurface->getSurfacePoint( variable1, variable2 );
        }
    }

    // Perform panel calculations for mesh.
    performPanelCalculations( );
}

//! Copy constructor.
LawgsPartGeometry::LawgsPartGeometry( const LawgsPartGeometry& partToCopy )
    : QuadrilateralMeshedSurfaceGeometry( )
{
    // Copy all properties of partToCopy to new part.
    name_ = partToCopy.name_;
    reversalOperator_ = partToCopy.reversalOperator_;
    numberOfLines_ = partToCopy.numberOfLines_;
    numberOfPoints_ = partToCopy.numberOfPoints_;
    rotationMatrix_= partToCopy.rotationMatrix_;
    scalingMatrix_ = partToCopy.scalingMatrix_;
    offset_= partToCopy.offset_;

    // Copy surface points array.
    meshPoints_ = partToCopy.meshPoints_;
}

//! Get surface point.
Eigen::VectorXd LawgsPartGeometry::getSurfacePoint( const double independentVariable1,
                                                    const double independentVariable2 )
{
    // Declare local variables denoting 'start' of panel.
    int pointIndex, lineIndex;

    // Declare local variable denoting surface point.
    Eigen::VectorXd point = Eigen::VectorXd( 3 );

    // Set local variables.
    pointIndex = static_cast< int > ( floor( independentVariable2 ) );
    lineIndex = static_cast< int > ( floor( independentVariable1 ) );

    // Start with panel centroid.
    point = panelCentroids_[ lineIndex ][ pointIndex ];

    // Move back to panel corner.
    point -= 0.5 *( meshPoints_[ lineIndex + 1 ][ pointIndex + 1 ] -
                    meshPoints_[ lineIndex ][ pointIndex ]);

    // Add contribution of 1st independent variable on panel.
    point += ( independentVariable1 - lineIndex ) *
             ( meshPoints_[ lineIndex + 1 ][ pointIndex ]
               - meshPoints_[ lineIndex ][ pointIndex ] );

    // Add contribution of 2nd independent variable on panel.
    point += ( independentVariable2 - pointIndex ) *
             ( meshPoints_[ lineIndex ][ pointIndex + 1 ] -
               meshPoints_[ lineIndex ][ pointIndex ] );

    return point;
}

//! Get surface derivative (currently not implemented).
Eigen::VectorXd LawgsPartGeometry::getSurfaceDerivative( const double u, const double v,
                                                         const int uDerivative,
                                                         const int vDerivative )
{
    std::cerr << "Surface derivative function not implemented in "
              << "LawgsPartGeometry class. Not able to return the "
              << uDerivative << ", " << vDerivative << "th derivative at point,"
              << u << ", " << v << ". Returning zero vector." << std::endl;

    return Eigen::Vector3d( 0.0, 0.0, 0.0 );
}

//! Get parameter.
double LawgsPartGeometry::getParameter( const int parameterIndex )
{
    std::cerr << "Get parameter function not implemented in LawgsPartGeometry"
              << "class, unable to retrieve parameter "<< parameterIndex
              << ". Returning zero." << std::endl;

    return 0.0;
}

//! Set parameter.
void LawgsPartGeometry::setParameter( const int parameterIndex, const double value )
{
    std::cerr << "Set parameter function not implemented in LawgsPartGeometry"
              << "class. Unable to set value of " << value << " at parameter index "
              << parameterIndex << std::endl;
}

//! Overload ostream to print class information.
std::ostream& operator<<( std::ostream& stream, LawgsPartGeometry& lawgsPartGeometry )
{
    stream << "This is a Langley Wireframe Geometry Standard surface geometry"
           << " of a single part." << std::endl;
    stream << "The number of lines ( contours ) is: "
           << lawgsPartGeometry.numberOfLines_ << std::endl;
    stream << "The number of points per line is: "
           << lawgsPartGeometry.numberOfPoints_ << std::endl;
    stream << "The part name is: " << lawgsPartGeometry.name_ << std::endl;

    // Return stream.
    return stream;
}

} // namespace geometric_shapes
} // namespace tudat


/*
 * This file belongs to the Galois project, a C++ library for exploiting
 * parallelism. The code is being released under the terms of the 3-Clause BSD
 * License (a copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

#include <limits>
#include <iostream>
#include <fstream>
#include <vector>
#include <cstring>
#include <sstream>

#include <boost/math/constants/constants.hpp>
#include <boost/iterator/transform_iterator.hpp>

#include "galois/Galois.h"
#include "galois/Atomic.h"
#include "galois/CilkInit.h"
#include "galois/Timer.h"
#include "galois/runtime/DoAllCoupled.h"
#include "galois/runtime/Profile.h"
#include "galois/substrate/CompilerSpecific.h"

#include "llvm/Support/CommandLine.h"
#include "Lonestar/BoilerPlate.h"

#include "Config.h"
#include "Point.h"
#include "Octree.h"
#include "BoundingBox.h"
#include "TreeBuildSumm.h"
#include "ForceComputation.h"

namespace bh {
const char* name = "Barnshut N-Body Simulator";
const char* desc =
    "Simulates the gravitational forces in a galactic cluster using the "
    "Barnes-Hut n-body algorithm";
const char* url = "barneshut";

namespace cll = llvm::cl;

static cll::opt<int> nbodies("n", cll::desc("Number of bodies"),
                             cll::init(10000));
static cll::opt<int> ntimesteps("steps", cll::desc("Number of steps"),
                                cll::init(1));
static cll::opt<int> seed("seed", cll::desc("Random seed"), cll::init(7));

enum TreeSummMethod {
  SERIAL,
  SERIAL_TREE,
  GALOIS_TREE,
  CILK_TREE,
  KDG_HAND,
  LEVEL_EXEC,
  SPEC,
  TWO_PHASE,
  DATA_DAG,
  RSUMM_SERIAL,
  RSUMM_CILK,
  RSUMM_GALOIS,
};

cll::opt<TreeSummMethod> treeSummOpt(
    cll::desc("Tree Summarization Method:"),
    cll::values(
        clEnumVal(SERIAL, "Serial recursive"),
        clEnumVal(SERIAL_TREE, "using data dependence DAG version of KDG"),
        clEnumVal(GALOIS_TREE, "using data dependence DAG version of KDG"),
        clEnumVal(CILK_TREE, "using cilk executor"),
        clEnumVal(KDG_HAND, "KDG based hand-implemented"),
        clEnumVal(LEVEL_EXEC, "using level-by-level executor"),
        clEnumVal(SPEC, "using speculative ordered executor"),
        clEnumVal(TWO_PHASE, "using two phase window ordered executor"),
        clEnumVal(DATA_DAG, "Generate DAG using data dependences"),
        clEnumVal(RSUMM_SERIAL, "Build Lock Free, Summarize Recursive Serial"),
        clEnumVal(RSUMM_CILK, "Build Lock Free, Summarize Recursive Cilk"),
        clEnumVal(RSUMM_GALOIS, "Build Lock Free, Summarize Recursive Galois"),
        clEnumValEnd),
    cll::init(SERIAL));

double nextDouble() { return rand() / (double)RAND_MAX; }

/**
 * Generates random input according to the Plummer model, which is more
 * realistic but perhaps not so much so according to astrophysicists
 */
template <typename BodyCont>
void generateInput(BodyCont& bodies, int nbodies, int seed) {
  typedef
      typename std::remove_pointer<typename BodyCont::value_type>::type Body_ty;

  double v, sq, scale;
  Point p;
  double PI = boost::math::constants::pi<double>();

  srand(seed);

  double rsc = (3 * PI) / 16;
  double vsc = sqrt(1.0 / rsc);

  for (int body = 0; body < nbodies; body++) {
    double r = 1.0 / sqrt(pow(nextDouble() * 0.999, -2.0 / 3.0) - 1);
    do {
      for (int i = 0; i < 3; i++)
        p[i] = nextDouble() * 2.0 - 1.0;
      sq = p.x * p.x + p.y * p.y + p.z * p.z;
    } while (sq > 1.0);
    scale = rsc * r / sqrt(sq);

    Body_ty* b = new Body_ty();
    b->mass    = 1.0 / nbodies;
    for (int i = 0; i < 3; i++)
      b->pos[i] = p[i] * scale;

    do {
      p.x = nextDouble();
      p.y = nextDouble() * 0.1;
    } while (p.y > p.x * p.x * pow(1 - p.x * p.x, 3.5));
    v = p.x * sqrt(2.0 / sqrt(1 + r * r));
    do {
      for (int i = 0; i < 3; i++)
        p[i] = nextDouble() * 2.0 - 1.0;
      sq = p.x * p.x + p.y * p.y + p.z * p.z;
    } while (sq > 1.0);
    scale = vsc * v / sqrt(sq);
    for (int i = 0; i < 3; i++)
      b->vel[i] = p[i] * scale;

    bodies.push_back(b);
  }
}

template <bool IsOn>
struct ToggleTime : public galois::StatTimer {
  ToggleTime(const char* name) : galois::StatTimer(name) {}
};

template <>
struct ToggleTime<false> {
  ToggleTime(const char* name) {}
  void start() {}
  void stop() {}
};

template <bool TrackTime, typename TB>
Point run(int nbodies, int ntimesteps, int seed, const TB& treeBuilder) {
  typedef typename TB::Base_ty B;
  typedef galois::gdeque<Body<B>*> Bodies;
  typedef galois::FixedSizeAllocator<OctreeInternal<B>> TreeAlloc;

  Config config;
  Bodies bodies;
  TreeAlloc treeAlloc;

  ToggleTime<TrackTime> t_input_gen("Time taken by input generation: ");

  t_input_gen.start();
  generateInput(bodies, nbodies, seed);
  t_input_gen.stop();

  Point ret;
  for (int step = 0; step < ntimesteps; step++) {
    typedef galois::worklists::PerSocketChunkLIFO<256> WL;

    ReducibleBox bbox;
    ToggleTime<TrackTime> t_bbox("Time taken by Bounding Box computation: ");

    // TODO: use parallel reducer here
    struct GetPos {
      typedef const Point& result_type;

      result_type operator()(const Body<B>* b) const { return b->pos; }
    };
    auto beg = boost::make_transform_iterator(bodies.begin(), GetPos());
    auto end = boost::make_transform_iterator(bodies.end(), GetPos());

    t_bbox.start();
    galois::do_all(beg, end, ReduceBoxes(bbox), galois::steal());
    t_bbox.stop();

    BoundingBox box(bbox.reduce());

    // OctreeInternal<B>* top = new OctreeInternal<B>(box);
    ToggleTime<TrackTime> t_tree_build(
        "Time taken by Octree building and summarization: ");

    t_tree_build.start();
    OctreeInternal<B>* top =
        treeBuilder(box, bodies.begin(), bodies.end(), treeAlloc);
    t_tree_build.stop();

    if (!skipVerify) {
      std::cout << "WARNING: Comparing against serially built & summarized "
                   "tree..., timing may be off"
                << std::endl;
      BuildSummarizeSeparate<BuildTreeSerial, SummarizeTreeSerial<B>>
          serialBuilder;
      OctreeInternal<B>* stop =
          serialBuilder(box, bodies.begin(), bodies.end(), treeAlloc);

      compareTrees(stop, top);
    }

    // BuildOctreeSerial<B> build;
    //
    // OctreeInternal<B>* top = build (box, bodies.begin (), bodies.end ());
    //
    // // galois::for_each(bodies.begin(), bodies.end(),
    // // BuildOctree<B>(top, box.radius()), galois::wl<WL> ());
    //
    // // reset the number of threads
    // galois::setActiveThreads(numThreads);
    //
    // ToggleTime<TrackTime> t_tree_summ ("Time taken by Tree Summarization: ");
    //
    // t_tree_summ.start ();
    // summMethod (top, bodies.begin (), bodies.end ());
    // t_tree_summ.stop ();

    if (false) { // disabling remaining phases
      ToggleTime<TrackTime> T_parallel("ParallelTime");
      T_parallel.start();

      galois::for_each(bodies.begin(), bodies.end(),
                       ComputeForces<B>(config, top, box.diameter()),
                       galois::wl<WL>());
      galois::for_each(bodies.begin(), bodies.end(), AdvanceBodies<B>(config),
                       galois::wl<WL>());
      T_parallel.stop();
    }

    ret = top->pos;

    std::cout << "Timestep " << step << ", Root's Center of Mass = " << top->pos
              << std::endl;

    // TODO: delete using TreeAlloc
    // delete top;
    destroyTree(top, treeAlloc);

    for (auto i = bodies.begin(), endi = bodies.end(); i != endi; ++i) {
      delete *i;
      *i = nullptr;
    }
  }

  return ret;
}

} // end namespace bh

int main(int argc, char** argv) {
  galois::StatManager sm;
  LonestarStart(argc, argv, bh::name, bh::desc, bh::url);

  std::cout.setf(std::ios::right | std::ios::scientific | std::ios::showpoint);

  std::cout << "configuration: " << bh::nbodies << " bodies, " << bh::ntimesteps
            << " time steps" << std::endl
            << std::endl;
  std::cout << "Num. of threads: " << numThreads << std::endl;

  bh::Point pos;
  galois::StatTimer T("total time:");

  T.start();
  switch (bh::treeSummOpt) {
  case bh::SERIAL:
    // TODO: fix template argument mistmatch between build and summarize
    pos =
        bh::run<true>(bh::nbodies, bh::ntimesteps, bh::seed,
                      bh::BuildSummarizeSeparate<bh::BuildTreeSerial,
                                                 bh::SummarizeTreeSerial<>>());
    break;

  case bh::SERIAL_TREE:
    pos =
        bh::run<true>(bh::nbodies, bh::ntimesteps, bh::seed,
                      bh::BuildSummarizeRecursive<bh::recursive::USE_SERIAL>());
    break;

  case bh::GALOIS_TREE:
    pos =
        bh::run<true>(bh::nbodies, bh::ntimesteps, bh::seed,
                      bh::BuildSummarizeRecursive<bh::recursive::USE_GALOIS>());
    break;

  case bh::CILK_TREE:
    // FIXME:      galois::CilkInit ();
    pos = bh::run<true>(bh::nbodies, bh::ntimesteps, bh::seed,
                        bh::BuildSummarizeRecursive<bh::recursive::USE_CILK>());
    break;

  case bh::LEVEL_EXEC:
    pos =
        bh::run<true>(bh::nbodies, bh::ntimesteps, bh::seed,
                      bh::BuildSummarizeSeparate<bh::BuildTreeLockFree,
                                                 bh::TreeSummarizeLevelExec>());
    break;

  case bh::KDG_HAND:
    pos = bh::run<true>(bh::nbodies, bh::ntimesteps, bh::seed,
                        bh::BuildSummarizeSeparate<bh::BuildTreeLockFree,
                                                   bh::TreeSummarizeKDGhand>());
    break;

  case bh::SPEC:
    pos = bh::run<true>(
        bh::nbodies, bh::ntimesteps, bh::seed,
        bh::BuildSummarizeSeparate<bh::BuildTreeLockFree,
                                   bh::TreeSummarizeSpeculative>());
    break;

  case bh::TWO_PHASE:
    pos =
        bh::run<true>(bh::nbodies, bh::ntimesteps, bh::seed,
                      bh::BuildSummarizeSeparate<bh::BuildTreeLockFree,
                                                 bh::TreeSummarizeTwoPhase>());
    break;

  case bh::DATA_DAG:
    pos = bh::run<true>(bh::nbodies, bh::ntimesteps, bh::seed,
                        bh::BuildSummarizeSeparate<bh::BuildTreeLockFree,
                                                   bh::TreeSummarizeDataDAG>());
    break;

  case bh::RSUMM_SERIAL:
    pos = bh::run<true>(
        bh::nbodies, bh::ntimesteps, bh::seed,
        bh::BuildLockFreeSummarizeRecursive<bh::recursive::USE_SERIAL>());
    break;

  case bh::RSUMM_CILK:
    // FIXME:      galois::CilkInit ();
    pos = bh::run<true>(
        bh::nbodies, bh::ntimesteps, bh::seed,
        bh::BuildLockFreeSummarizeRecursive<bh::recursive::USE_CILK>());
    break;

  case bh::RSUMM_GALOIS:
    pos = bh::run<true>(
        bh::nbodies, bh::ntimesteps, bh::seed,
        bh::BuildLockFreeSummarizeRecursive<bh::recursive::USE_GALOIS>());
    break;

  default:
    abort();
  }
  T.stop();

  // if (!skipVerify) {
  // std::cout << "Running serial tree summarization for verification" <<
  // std::endl; bh::Point serPos = bh::run<false> (bh::nbodies, bh::ntimesteps,
  // bh::seed, bh::SummarizeTreeSerial ());
  //
  // double EPS = 1e-9;
  // bool equal = true;
  // for (unsigned i = 0; i < 3; ++i) {
  // if (fabs (pos[i] - serPos[i]) > EPS) {
  // equal = false;
  // break;
  // }
  // }
  //
  // if (!equal) {
  // std::cerr << "!!!BAD: Results don't match with serial!!!" << std::endl;
  // abort ();
  //
  // } else {
  // std::cout << ">>> OK, results verified" << std::endl;
  // }
  //
  // }
}


#include <Eigen/StdVector>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <Eigen/Jacobi>
#include <Eigen/SVD>
#include <unsupported/Eigen/MatrixFunctions>
#include <iostream>
#include <map>
#include <vector>
#include <functional>
#include <type_traits>

#ifndef EIGEN_HELPERS_HPP
#define EIGEN_HELPERS_HPP

namespace std
{
    // http://stackoverflow.com/questions/2590677/how-do-i-combine-hash-values-in-c0x
    template <class T>
    inline void hash_combine(std::size_t& seed, const T& v)
    {
        std::hash<T> hasher;
        seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2);
    }

    template <typename T>
    struct hash<std::complex<T>>
    {
        std::size_t operator()(const std::complex<T>& val) const
        {
            return (std::hash<T>()(val.real()) ^ ((std::hash<T>()(val.imag()) << 1) >> 1));
        }
    };

    template <>
    struct hash<Eigen::Vector3d>
    {
        std::size_t operator()(const Eigen::Vector3d& vector) const
        {
            return (std::hash<double>()(vector.x()) ^ ((std::hash<double>()(vector.y()) << 1) >> 1) ^ (std::hash<double>()(vector.z()) << 1));
        }
    };

    // Hash function for Eigen vector.
    // Based on here: https://wjngkoh.wordpress.com/2015/03/04/c-hash-function-for-eigen-matrix-and-vector/
    template<typename _Scalar, int _Rows>
    struct hash<Eigen::Matrix<_Scalar, _Rows, 1>>
    {
        std::size_t operator() (const Eigen::Matrix<_Scalar, _Rows, 1>& vector) const
        {
            std::size_t hash = 0;
            for (ssize_t idx = 0; idx < vector.size(); idx++)
            {
                std::hash_combine(hash, vector(idx));
            }
            return hash;
        }
    };

    template <typename T1, typename T2>
    struct hash<std::pair<T1, T2>>
    {
        std::size_t operator()(const std::pair<T1, T2>& val) const
        {
            std::size_t seed = 0;
            std::hash_combine(seed, val.first);
            std::hash_combine(seed, val.second);
            return seed;
        }
    };

}

namespace EigenHelpers
{
    ////////////////////////////////////////////////////////////////////////////
    // Typedefs for aligned STL containers using Eigen types
    ////////////////////////////////////////////////////////////////////////////

    typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VectorVector2d;
    typedef std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> VectorVector3f;
    typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VectorVector3d;
    typedef std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> VectorVector4f;
    typedef std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>> VectorVector4d;
    typedef std::vector<Eigen::Quaternionf, Eigen::aligned_allocator<Eigen::Quaternionf>> VectorQuaternionf;
    typedef std::vector<Eigen::Quaterniond, Eigen::aligned_allocator<Eigen::Quaterniond>> VectorQuaterniond;
    typedef std::vector<Eigen::Isometry3f, Eigen::aligned_allocator<Eigen::Isometry3f>> VectorIsometry3f;
    typedef std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> VectorIsometry3d;
    typedef std::map<std::string, Eigen::Vector3f, std::less<std::string>, Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector3f>>> MapStringVector3f;
    typedef std::map<std::string, Eigen::Vector3d, std::less<std::string>, Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector3d>>> MapStringVector3d;
    typedef std::map<std::string, Eigen::Vector4f, std::less<std::string>, Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector4f>>> MapStringVector4f;
    typedef std::map<std::string, Eigen::Vector4d, std::less<std::string>, Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector4d>>> MapStringVector4d;
    typedef std::map<std::string, Eigen::Quaternionf, std::less<std::string>, Eigen::aligned_allocator<std::pair<const std::string, Eigen::Quaternionf>>> MapStringQuaternionf;
    typedef std::map<std::string, Eigen::Quaterniond, std::less<std::string>, Eigen::aligned_allocator<std::pair<const std::string, Eigen::Quaterniond>>> MapStringQuaterniond;
    typedef std::map<std::string, Eigen::Isometry3f, std::less<std::string>, Eigen::aligned_allocator<std::pair<const std::string, Eigen::Isometry3f>>> MapStringIsometry3f;
    typedef std::map<std::string, Eigen::Isometry3d, std::less<std::string>, Eigen::aligned_allocator<std::pair<const std::string, Eigen::Isometry3d>>> MapStringIsometry3d;

    inline bool Equal(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
    {
        if ((v1.x() == v2.x()) && (v1.y() == v2.y()) && (v1.z() == v2.z()))
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    inline bool CloseEnough(const double p1, const double p2, const double threshold)
    {
        const double real_threshold = std::abs(threshold);
        const double abs_delta = std::abs(p2 - p1);
        if (abs_delta <= real_threshold)
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    inline bool CloseEnough(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, const double threshold)
    {
        double real_threshold = std::fabs(threshold);
        if (std::fabs(v1.x() - v2.x()) > real_threshold)
        {
            return false;
        }
        if (std::fabs(v1.y() - v2.y()) > real_threshold)
        {
            return false;
        }
        if (std::fabs(v1.z() - v2.z()) > real_threshold)
        {
            return false;
        }
        return true;
    }

    template <typename EigenType, typename Allocator=Eigen::aligned_allocator<EigenType>>
    inline bool CloseEnough(const std::vector<EigenType, Allocator>& a, const std::vector<EigenType, Allocator>& b, const double threshold)
    {
        if (a.size() != b.size())
        {
            return false;
        }
        for (size_t idx = 0; idx < a.size(); idx++)
        {
            if (!CloseEnough(a[idx], b[idx], threshold))
            {
                return false;
            }
        }
        return true;
    }

    // Inspired by Eigen's "isApprox" function
    inline bool IsApprox(const double p1, const double p2, const double precision)
    {
        const double smallest_element = std::min(std::abs(p1), std::abs(p2));
        const double threshold = precision * smallest_element;
        return CloseEnough(p1, p2, threshold);
    }

    template <typename EigenType, typename Allocator=Eigen::aligned_allocator<EigenType>>
    inline bool IsApprox(const std::vector<EigenType, Allocator>& a, const std::vector<EigenType, Allocator>& b,
                         const typename EigenType::Scalar& precision = Eigen::NumTraits<typename EigenType::Scalar>::dummy_precision())
    {
        if (a.size() != b.size())
        {
            return false;
        }
        for (size_t idx = 0; idx < a.size(); idx++)
        {
            if (!a[idx].isApprox(b[idx], precision))
            {
                return false;
            }
        }
        return true;
    }

    template <typename Derived>
    inline Eigen::MatrixXd ClampNorm(const Eigen::MatrixBase<Derived>& item_to_clamp, const double max_norm)
    {
        assert(max_norm >= 0 && "You must pass a maximum norm that is positive");
        const double current_norm = item_to_clamp.norm();
        if (current_norm > max_norm)
        {
            return item_to_clamp * (max_norm / current_norm);
        }
        return item_to_clamp;
    }

    ////////////////////////////////////////////////////////////////////////////
    // Kinematics functions
    ////////////////////////////////////////////////////////////////////////////

    inline Eigen::Vector3d RotateVector(const Eigen::Quaterniond& quat, const Eigen::Vector3d& vec)
    {
        const Eigen::Quaterniond temp(0.0, vec.x(), vec.y(), vec.z());
        const Eigen::Quaterniond res = quat * (temp * quat.inverse());
        return Eigen::Vector3d(res.x(), res.y(), res.z());
    }

    inline Eigen::Vector3d RotateVectorReverse(const Eigen::Quaterniond& quat, const Eigen::Vector3d& vec)
    {
        const Eigen::Quaterniond temp(0.0, vec.x(), vec.y(), vec.z());
        const Eigen::Quaterniond res = quat.inverse() * (temp * quat);
        return Eigen::Vector3d(res.x(), res.y(), res.z());
    }

    inline double EnforceContinuousRevoluteBounds(const double value)
    {
        if ((value <= -M_PI) || (value > M_PI))
        {
            const double remainder = fmod(value, 2.0 * M_PI);
            if (remainder <= -M_PI)
            {
                return (remainder + (2.0 * M_PI));
            }
            else if (remainder > M_PI)
            {
                return (remainder - (2.0 * M_PI));
            }
            else
            {
                return remainder;
            }
        }
        else
        {
            return value;
        }
    }

    inline Eigen::VectorXd SafeNormal(const Eigen::VectorXd& vec)
    {
        const double norm = vec.norm();
        if (norm > std::numeric_limits<double>::epsilon())
        {
            return vec / norm;
        }
        else
        {
            return vec;
        }
    }

    inline double SquaredNorm(const std::vector<double>& vec)
    {
        double squared_norm = 0.0;
        for (size_t idx = 0; idx < vec.size(); idx++)
        {
            const double element = vec[idx];
            squared_norm += (element * element);
        }
        return squared_norm;
    }

    inline double Norm(const std::vector<double>& vec)
    {
        return std::sqrt(SquaredNorm(vec));
    }

    inline std::vector<double> Abs(const std::vector<double>& vec)
    {
        std::vector<double> absed(vec.size(), 0.0);
        for (size_t idx = 0; idx < absed.size(); idx++)
        {
            absed[idx] = std::abs(vec[idx]);
        }
        return absed;
    }

    inline std::vector<double> Multiply(const std::vector<double>& vec, const double scalar)
    {
        std::vector<double> multiplied(vec.size(), 0.0);
        for (size_t idx = 0; idx < multiplied.size(); idx++)
        {
            const double element = vec[idx];
            multiplied[idx] = element * scalar;
        }
        return multiplied;
    }

    inline std::vector<double> Multiply(const std::vector<double>& vec1, const std::vector<double>& vec2)
    {
        if (vec1.size() == vec2.size())
        {
            std::vector<double> multiplied(vec1.size(), 0.0);
            for (size_t idx = 0; idx < multiplied.size(); idx++)
            {
                const double element1 = vec1[idx];
                const double element2 = vec2[idx];
                multiplied[idx] = element1 * element2;
            }
            return multiplied;
        }
        else
        {
            return std::vector<double>();
        }
    }

    inline std::vector<double> Divide(const std::vector<double>& vec, const double scalar)
    {
        const double inv_scalar = 1.0 / scalar;
        return Multiply(vec, inv_scalar);
    }

    inline std::vector<double> Divide(const std::vector<double>& vec1, const std::vector<double>& vec2)
    {
        if (vec1.size() == vec2.size())
        {
            std::vector<double> divided(vec1.size(), 0.0);
            for (size_t idx = 0; idx < divided.size(); idx++)
            {
                const double element1 = vec1[idx];
                const double element2 = vec2[idx];
                divided[idx] = element1 / element2;
            }
            return divided;
        }
        else
        {
            return std::vector<double>();
        }
    }

    inline std::vector<double> Add(const std::vector<double>& vec, const double scalar)
    {
        std::vector<double> added(vec.size(), 0.0);
        for (size_t idx = 0; idx < added.size(); idx++)
        {
            added[idx] = vec[idx] + scalar;
        }
        return added;
    }

    inline std::vector<double> Add(const std::vector<double>& vec1, const std::vector<double>& vec2)
    {
        if (vec1.size() == vec2.size())
        {
            std::vector<double> added(vec1.size(), 0.0);
            for (size_t idx = 0; idx < added.size(); idx++)
            {
                const double element1 = vec1[idx];
                const double element2 = vec2[idx];
                added[idx] = element1 + element2;
            }
            return added;
        }
        else
        {
            return std::vector<double>();
        }
    }

    inline std::vector<double> Sub(const std::vector<double>& vec, const double scalar)
    {
        std::vector<double> subed(vec.size(), 0.0);
        for (size_t idx = 0; idx < subed.size(); idx++)
        {
            subed[idx] = vec[idx] - scalar;
        }
        return subed;
    }

    inline std::vector<double> Sub(const std::vector<double>& vec1, const std::vector<double>& vec2)
    {
        if (vec1.size() == vec2.size())
        {
            std::vector<double> subed(vec1.size(), 0.0);
            for (size_t idx = 0; idx < subed.size(); idx++)
            {
                const double element1 = vec1[idx];
                const double element2 = vec2[idx];
                subed[idx] = element1 - element2;
            }
            return subed;
        }
        else
        {
            return std::vector<double>();
        }
    }

    inline Eigen::Matrix3d Skew(const Eigen::Vector3d& vector)
    {
        Eigen::Matrix3d skewed;
        skewed << 0.0, -vector.z(), vector.y(),
                  vector.z(), 0.0, -vector.x(),
                  -vector.y(), vector.x(), 0.0;
        return skewed;
    }

    inline Eigen::Vector3d Unskew(const Eigen::Matrix3d& matrix)
    {
        const Eigen::Matrix3d matrix_symetric = (matrix - matrix.transpose()) / 2.0;
        const Eigen::Vector3d unskewed(matrix_symetric(2, 1), matrix_symetric(0, 2), matrix_symetric(1, 0));
        return unskewed;
    }

    inline Eigen::Matrix4d TwistHat(const Eigen::Matrix<double, 6, 1>& twist)
    {
        const Eigen::Vector3d trans_velocity = twist.segment<3>(0);
        const Eigen::Matrix3d hatted_rot_velocity = Skew(twist.segment<3>(3));
        Eigen::Matrix4d hatted_twist = Eigen::Matrix4d::Zero();
        hatted_twist.block<3, 3>(0, 0) = hatted_rot_velocity;
        hatted_twist.block<3, 1>(0, 3) = trans_velocity;
        return hatted_twist;
    }

    inline Eigen::Matrix<double, 6, 1> TwistUnhat(const Eigen::Matrix4d& hatted_twist)
    {
         const Eigen::Vector3d trans_velocity = hatted_twist.block<3, 1>(0, 3);
         const Eigen::Vector3d rot_velocity = Unskew(hatted_twist.block<3, 3>(0, 0));
         Eigen::Matrix<double, 6, 1> twist;
         twist.segment<3>(0) = trans_velocity;
         twist.segment<3>(3) = rot_velocity;
         return twist;
    }

    inline Eigen::Matrix<double, 6, 6> AdjointFromTransform(const Eigen::Isometry3d& transform)
    {
        const Eigen::Matrix3d rotation = transform.matrix().block<3, 3>(0, 0);
        const Eigen::Vector3d translation = transform.matrix().block<3, 1>(0, 3);
        const Eigen::Matrix3d translation_hat = Skew(translation);
        // Assemble the adjoint matrix
        Eigen::Matrix<double, 6, 6> adjoint;
        adjoint.block<3, 3>(0, 0) = rotation;
        adjoint.block<3, 3>(0, 3) = translation_hat * rotation;
        adjoint.block<3, 3>(3, 0) = Eigen::Matrix3d::Zero();
        adjoint.block<3, 3>(3, 3) = rotation;
        return adjoint;
    }

    inline Eigen::Matrix<double, 6, 1> TransformTwist(const Eigen::Isometry3d& transform, const Eigen::Matrix<double, 6, 1>& initial_twist)
    {
        return (Eigen::Matrix<double, 6, 1>)(EigenHelpers::AdjointFromTransform(transform) * initial_twist);
    }

    inline Eigen::Matrix<double, 6, 1> TwistBetweenTransforms(const Eigen::Isometry3d& start, const Eigen::Isometry3d& end)
    {
        const Eigen::Isometry3d t_diff = start.inverse() * end;
        return TwistUnhat(t_diff.matrix().log());
    }

    inline Eigen::Matrix3d ExpMatrixExact(const Eigen::Matrix3d& hatted_rot_velocity, const double delta_t)
    {
        assert(std::fabs(Unskew(hatted_rot_velocity).norm() - 1.0) < 1e-10);
        const Eigen::Matrix3d exp_matrix = Eigen::Matrix3d::Identity() + (hatted_rot_velocity * sin(delta_t)) + (hatted_rot_velocity * hatted_rot_velocity * (1.0 - cos(delta_t)));
        return exp_matrix;
    }

    inline Eigen::Isometry3d ExpTwist(const Eigen::Matrix<double, 6, 1>& twist, const double delta_t)
    {
        const Eigen::Vector3d trans_velocity = twist.segment<3>(0);
        const Eigen::Vector3d rot_velocity = twist.segment<3>(3);
        const double trans_velocity_norm = trans_velocity.norm();
        const double rot_velocity_norm = rot_velocity.norm();
        Eigen::Matrix4d raw_transform = Eigen::Matrix4d::Identity();
        if (rot_velocity_norm >= 1e-100)
        {
            const double scaled_delta_t = delta_t * rot_velocity_norm;
            const Eigen::Vector3d scaled_trans_velocity = trans_velocity / rot_velocity_norm;
            const Eigen::Vector3d scaled_rot_velocity = rot_velocity / rot_velocity_norm;
            const Eigen::Matrix3d rotation_displacement = ExpMatrixExact(Skew(scaled_rot_velocity), scaled_delta_t);
            const Eigen::Vector3d translation_displacement = ((Eigen::Matrix3d::Identity() - rotation_displacement) * scaled_rot_velocity.cross(scaled_trans_velocity)) + (scaled_rot_velocity * scaled_rot_velocity.transpose() * scaled_trans_velocity * scaled_delta_t);
            raw_transform.block<3, 3>(0, 0) = rotation_displacement;
            raw_transform.block<3, 1>(0, 3) = translation_displacement;
        }
        else
        {
            if ((trans_velocity_norm >= 1e-100) || (rot_velocity_norm == 0.0))
            {
                raw_transform.block<3, 1>(0, 3) = trans_velocity * delta_t;
            }
            else
            {
                std::cerr << "*** WARNING - YOU MAY ENCOUNTER NUMERICAL INSTABILITY IN EXPTWIST(...) WITH TRANS & ROT VELOCITY NORM < 1e-100 ***" << std::endl;
                const double scaled_delta_t = delta_t * rot_velocity_norm;
                const Eigen::Vector3d scaled_trans_velocity = trans_velocity / rot_velocity_norm;
                const Eigen::Vector3d scaled_rot_velocity = rot_velocity / rot_velocity_norm;
                const Eigen::Matrix3d rotation_displacement = ExpMatrixExact(Skew(scaled_rot_velocity), scaled_delta_t);
                const Eigen::Vector3d translation_displacement = ((Eigen::Matrix3d::Identity() - rotation_displacement) * scaled_rot_velocity.cross(scaled_trans_velocity)) + (scaled_rot_velocity * scaled_rot_velocity.transpose() * scaled_trans_velocity * scaled_delta_t);
                raw_transform.block<3, 3>(0, 0) = rotation_displacement;
                raw_transform.block<3, 1>(0, 3) = translation_displacement;
            }
        }
        Eigen::Isometry3d transform;
        transform = raw_transform;
        return transform;
    }

    ////////////////////////////////////////////////////////////////////////////
    // Interpolation functions
    ////////////////////////////////////////////////////////////////////////////

    inline double Interpolate(const double p1, const double p2, const double ratio)
    {
        // Safety check ratio
        double real_ratio = ratio;
        if (real_ratio < 0.0)
        {
            real_ratio = 0.0;
            std::cerr << "Interpolation ratio < 0.0, set to 0.0" << std::endl;
        }
        else if (real_ratio > 1.0)
        {
            real_ratio = 1.0;
            std::cerr << "Interpolation ratio > 1.0, set to 1.0" << std::endl;
        }
        // Interpolate
        // This is the numerically stable version, rather than  (p1 + (p2 - p1) * real_ratio)
        return ((p1 * (1.0 - real_ratio)) + (p2 * real_ratio));
    }

    inline double InterpolateContinuousRevolute(const double p1, const double p2, const double ratio)
    {
        // Safety check ratio
        double real_ratio = ratio;
        if (real_ratio < 0.0)
        {
            real_ratio = 0.0;
            std::cerr << "Interpolation ratio < 0.0, set to 0.0" << std::endl;
        }
        else if (real_ratio > 1.0)
        {
            real_ratio = 1.0;
            std::cerr << "Interpolation ratio > 1.0, set to 1.0" << std::endl;
        }
        // Safety check args
        const double real_p1 = EnforceContinuousRevoluteBounds(p1);
        const double real_p2 = EnforceContinuousRevoluteBounds(p2);
        // Interpolate
        double interpolated = 0.0;
        double diff = real_p2 - real_p1;
        if (std::fabs(diff) <= M_PI)
        {
            interpolated = real_p1 + diff * real_ratio;
        }
        else
        {
            if (diff > 0.0)
            {
                diff = 2.0 * M_PI - diff;
            }
            else
            {
                diff = -2.0 * M_PI - diff;
            }
            interpolated = real_p1 - diff * real_ratio;
            // Input states are within bounds, so the following check is sufficient
            if (interpolated > M_PI)
            {
                interpolated -= 2.0 * M_PI;
            }
            else
            {
                if (interpolated < -M_PI)
                {
                    interpolated += 2.0 * M_PI;
                }
            }
        }
        return interpolated;
    }

    inline std::vector<double> Interpolate(const std::vector<double>& v1, const std::vector<double>& v2, const double ratio)
    {
        // Safety check ratio
        double real_ratio = ratio;
        if (real_ratio < 0.0)
        {
            real_ratio = 0.0;
            std::cerr << "Interpolation ratio < 0.0, set to 0.0" << std::endl;
        }
        else if (real_ratio > 1.0)
        {
            real_ratio = 1.0;
            std::cerr << "Interpolation ratio > 1.0, set to 1.0" << std::endl;
        }
        // Safety check inputs
        const size_t len = v1.size();
        if (len != v2.size())
        {
            std::cerr << "Vectors to interpolate are different sizes (" << v1.size() << " versus " << v2.size() << ")" << std::endl;
            return std::vector<double>();
        }
        // Interpolate
        // This is the numerically stable version, rather than  (p1 + (p2 - p1) * real_ratio)
        std::vector<double> interped(len, 0);
        for (size_t idx = 0; idx < len; idx++)
        {
            interped[idx] = ((v1[idx] * (1.0 - real_ratio)) + (v2[idx] * real_ratio));
        }
        return interped;
    }

    inline Eigen::Quaterniond Interpolate(const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2, const double ratio)
    {
        // Safety check ratio
        double real_ratio = ratio;
        if (real_ratio < 0.0)
        {
            real_ratio = 0.0;
            std::cerr << "Interpolation ratio < 0.0, set to 0.0" << std::endl;
        }
        else if (real_ratio > 1.0)
        {
            real_ratio = 1.0;
            std::cerr << "Interpolation ratio > 1.0, set to 1.0" << std::endl;
        }
        // Interpolate
        return q1.slerp(real_ratio, q2);
    }

    inline Eigen::Vector3d Interpolate(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2, const double ratio)
    {
        // Safety check ratio
        double real_ratio = ratio;
        if (real_ratio < 0.0)
        {
            real_ratio = 0.0;
            std::cerr << "Interpolation ratio < 0.0, set to 0.0" << std::endl;
        }
        else if (real_ratio > 1.0)
        {
            real_ratio = 1.0;
            std::cerr << "Interpolation ratio > 1.0, set to 1.0" << std::endl;
        }
        // Interpolate
        // This is the numerically stable version, rather than  (p1 + (p2 - p1) * real_ratio)
        return ((v1 * (1.0 - real_ratio)) + (v2 * real_ratio));
    }

    inline Eigen::Isometry3d Interpolate(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2, const double ratio)
    {
        // Safety check ratio
        double real_ratio = ratio;
        if (real_ratio < 0.0)
        {
            real_ratio = 0.0;
            std::cerr << "Interpolation ratio < 0.0, set to 0.0" << std::endl;
        }
        else if (real_ratio > 1.0)
        {
            real_ratio = 1.0;
            std::cerr << "Interpolation ratio > 1.0, set to 1.0" << std::endl;
        }
        // Interpolate
        const Eigen::Vector3d v1 = t1.translation();
        const Eigen::Quaterniond q1(t1.rotation());
        const Eigen::Vector3d v2 = t2.translation();
        const Eigen::Quaterniond q2(t2.rotation());
        const Eigen::Vector3d vint = Interpolate(v1, v2, real_ratio);
        const Eigen::Quaterniond qint = Interpolate(q1, q2, real_ratio);
        const Eigen::Isometry3d tint = ((Eigen::Translation3d)vint) * qint;
        return tint;
    }

    ////////////////////////////////////////////////////////////////////////////
    // Distance functions
    ////////////////////////////////////////////////////////////////////////////

    inline double SquaredDistance(const Eigen::Vector2d& v1, const Eigen::Vector2d& v2)
    {
        const double xd = v2.x() - v1.x();
        const double yd = v2.y() - v1.y();
        return ((xd * xd) + (yd * yd));
    }

    inline double Distance(const Eigen::Vector2d& v1, const Eigen::Vector2d& v2)
    {
        return sqrt(SquaredDistance(v1, v2));
    }

    inline double SquaredDistance(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
    {
        const double xd = v2.x() - v1.x();
        const double yd = v2.y() - v1.y();
        const double zd = v2.z() - v1.z();
        return ((xd * xd) + (yd * yd) + (zd * zd));
    }

    inline double Distance(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2)
    {
        return sqrt(SquaredDistance(v1, v2));
    }

    inline double SquaredDistance(const Eigen::VectorXd& v1, const Eigen::VectorXd& v2)
    {
        assert(v1.size() == v2.size());
        return (v2 - v1).squaredNorm();
    }

    inline double Distance(const Eigen::VectorXd& v1, const Eigen::VectorXd& v2)
    {
        return (v2 - v1).norm();
    }

    inline double Distance(const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2)
    {
        const double dq = std::fabs((q1.w() * q2.w()) + (q1.x() * q2.x()) + (q1.y() * q2.y()) + (q1.z() * q2.z()));
        if (dq < (1.0 - std::numeric_limits<double>::epsilon()))
        {
            return acos(2.0 * (dq * dq) - 1.0);
        }
        else
        {
            return 0.0;
        }
    }

    inline double Distance(const Eigen::Isometry3d& t1, const Eigen::Isometry3d& t2, const double alpha = 0.5)
    {
        assert(alpha >= 0.0);
        assert(alpha <= 1.0);
        const Eigen::Vector3d v1 = t1.translation();
        const Eigen::Quaterniond q1(t1.rotation());
        const Eigen::Vector3d v2 = t2.translation();
        const Eigen::Quaterniond q2(t2.rotation());
        const double vdist = Distance(v1, v2) * (1.0 - alpha);
        const double qdist = Distance(q1, q2) * (alpha);
        return vdist + qdist;
    }

    inline double SquaredDistance(const std::vector<double>& p1, const std::vector<double>& p2)
    {
        if (p1.size() == p2.size())
        {
            double distance = 0.0;
            for (size_t idx = 0; idx < p1.size(); idx++)
            {
                distance += (p2[idx] - p1[idx]) * (p2[idx] - p1[idx]);
            }
            return distance;
        }
        else
        {
            return INFINITY;
        }
    }

    inline double Distance(const std::vector<double>& p1, const std::vector<double>& p2)
    {
        if (p1.size() == p2.size())
        {
            return sqrt(SquaredDistance(p1, p2));
        }
        else
        {
            return INFINITY;
        }
    }

    inline double ContinuousRevoluteSignedDistance(const double p1, const double p2)
    {
        // Safety check args
        const double real_p1 = EnforceContinuousRevoluteBounds(p1);
        const double real_p2 = EnforceContinuousRevoluteBounds(p2);
        const double raw_distance = real_p2 - real_p1;
        if ((raw_distance <= -M_PI) || (raw_distance > M_PI))
        {
            if (raw_distance <= -M_PI)
            {
                return (-(2.0 * M_PI) - raw_distance);
            }
            else if (raw_distance > M_PI)
            {
                return ((2.0 * M_PI) - raw_distance);
            }
            else
            {
                return raw_distance;
            }
        }
        else
        {
            return raw_distance;
        }
    }

    inline double ContinuousRevoluteDistance(const double p1, const double p2)
    {
        return std::fabs(ContinuousRevoluteSignedDistance(p1, p2));
    }

    inline double AddContinuousRevoluteValues(const double start, const double change)
    {
        return EnforceContinuousRevoluteBounds(start + change);
    }

    inline double GetContinuousRevoluteRange(const double start, const double end)
    {
        const double raw_range = ContinuousRevoluteSignedDistance(start, end);
        if (raw_range >= 0.0)
        {
            return raw_range;
        }
        else
        {
            return (2.0 * M_PI) + raw_range;
        }
    }

    inline bool CheckInContinuousRevoluteRange(const double start, const double range, const double val)
    {
        const double real_val = EnforceContinuousRevoluteBounds(val);
        const double real_start = EnforceContinuousRevoluteBounds(start);
        const double delta = ContinuousRevoluteSignedDistance(real_start, real_val);
        if (delta >= 0.0)
        {
            if (delta <= range)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
        else
        {
            const double real_delta = (2.0 * M_PI) + delta;
            if (real_delta <= range)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
    }

    inline bool CheckInContinuousRevoluteBounds(const double start, const double end, const double val)
    {
        const double range = GetContinuousRevoluteRange(start, end);
        return CheckInContinuousRevoluteRange(start, range, val);
    }

    // DistanceFn must match the following interface: std::function<double(const T&, const T&)>
    template<typename T, class DistanceFn, typename Alloc = std::allocator<T>>
    inline double CalculateTotalDistance(const std::vector<T, Alloc>& path, const DistanceFn& distance_fn)
    {
        double total_dist = 0;
        for (size_t path_idx = 0; path_idx + 1 < path.size(); path_idx++)
        {
            const double delta = distance_fn(path[path_idx], path[path_idx + 1]);
            total_dist += delta;
        }
        return total_dist;
    }

    inline double CalculateTotalDistance(const EigenHelpers::VectorVector3d& points)
    {
        double distance = 0;

        for (size_t idx = 1; idx < points.size(); ++idx)
        {
            const double delta = (points[idx] - points[idx - 1]).norm();
            distance += delta;
        }

        return distance;
    }

    inline std::vector<double> CalculateIndividualDistances(const EigenHelpers::VectorVector3d& points)
    {
        std::vector<double> distances(points.size());

        if (points.size() > 0)
        {
            distances[0] = 0.0;
            for (size_t idx = 1; idx < points.size(); ++idx)
            {
                distances[idx] = (points[idx] - points[idx - 1]).norm();
            }
        }

        return distances;
    }

    inline std::vector<double> CalculateCumulativeDistances(const EigenHelpers::VectorVector3d& points)
    {
        std::vector<double> distances(points.size());

        if (points.size() > 0)
        {
            distances[0] = 0.0;
            for (size_t idx = 1; idx < points.size(); ++idx)
            {
                const double delta = (points[idx] - points[idx - 1]).norm();
                distances[idx] = distances[idx - 1] + delta;
            }
        }

        return distances;
    }

    ////////////////////////////////////////////////////////////////////////////
    // Conversion functions
    ////////////////////////////////////////////////////////////////////////////

    inline Eigen::Quaterniond QuaternionFromRPY(const double R, const double P, const double Y)
    {
        const Eigen::AngleAxisd roll(R, Eigen::Vector3d::UnitX());
        const Eigen::AngleAxisd pitch(P, Eigen::Vector3d::UnitY());
        const Eigen::AngleAxisd yaw(Y, Eigen::Vector3d::UnitZ());
        const Eigen::Quaterniond quat(roll * pitch * yaw);
        return quat;
    }

    /* URDF RPY IS ACTUALLY APPLIED Y*P*R */
    inline Eigen::Quaterniond QuaternionFromUrdfRPY(const double R, const double P, const double Y)
    {
        const Eigen::AngleAxisd roll(R, Eigen::Vector3d::UnitX());
        const Eigen::AngleAxisd pitch(P, Eigen::Vector3d::UnitY());
        const Eigen::AngleAxisd yaw(Y, Eigen::Vector3d::UnitZ());
        const Eigen::Quaterniond quat(yaw * pitch * roll);
        return quat;
    }

    // Returns XYZ Euler angles
    inline Eigen::Vector3d EulerAnglesFromRotationMatrix(const Eigen::Matrix3d& rot_matrix)
    {
        const Eigen::Vector3d euler_angles = rot_matrix.eulerAngles(0, 1, 2); // Use XYZ angles
        return euler_angles;
    }

    // Returns XYZ Euler angles
    inline Eigen::Vector3d EulerAnglesFromQuaternion(const Eigen::Quaterniond& quat)
    {
        return EulerAnglesFromRotationMatrix(quat.toRotationMatrix());
    }

    // Returns XYZ Euler angles
    inline Eigen::Vector3d EulerAnglesFromIsometry3d(const Eigen::Isometry3d& trans)
    {
        return EulerAnglesFromRotationMatrix(trans.rotation());
    }

    inline Eigen::Isometry3d TransformFromRPY(const double x, const double y, const double z, const double roll, const double pitch, const double yaw)
    {
        const Eigen::Isometry3d transform = Eigen::Translation3d(x, y, z) * QuaternionFromRPY(roll, pitch, yaw);
        return transform;
    }

    inline Eigen::Isometry3d TransformFromRPY(const Eigen::Vector3d& translation, const Eigen::Vector3d& rotation)
    {
        const Eigen::Isometry3d transform = (Eigen::Translation3d)translation * QuaternionFromRPY(rotation.x(), rotation.y(), rotation.z());
        return transform;
    }

    inline Eigen::Isometry3d TransformFromRPY(const Eigen::VectorXd& components)
    {
        assert(components.size() == 6);
        const Eigen::Isometry3d transform = Eigen::Translation3d(components(0), components(1), components(2)) * QuaternionFromRPY(components(3), components(4), components(5));
        return transform;
    }

    inline Eigen::VectorXd TransformToRPY(const Eigen::Isometry3d& transform)
    {
        Eigen::VectorXd components = Eigen::VectorXd::Zero(6);
        const Eigen::Vector3d translation = transform.translation();
        const Eigen::Vector3d rotation = EulerAnglesFromRotationMatrix(transform.rotation());
        components << translation, rotation;
        return components;
    }

    inline Eigen::Vector3d StdVectorDoubleToEigenVector3d(const std::vector<double>& vector)
    {
        assert(vector.size() == 3 && "std::vector<double> source vector is not 3 elements in size");
        return Eigen::Vector3d(vector[0], vector[1], vector[2]);
    }

    inline Eigen::VectorXd StdVectorDoubleToEigenVectorXd(const std::vector<double>& vector)
    {
        Eigen::VectorXd eigen_vector(vector.size());
        for (size_t idx = 0; idx < vector.size(); idx++)
        {
            const double val = vector[idx];
            eigen_vector((ssize_t)idx) = val;
        }
        return eigen_vector;
    }

    inline std::vector<double> EigenVector3dToStdVectorDouble(const Eigen::Vector3d& point)
    {
        return std::vector<double>{point.x(), point.y(), point.z()};
    }

    inline std::vector<double> EigenVectorXdToStdVectorDouble(const Eigen::VectorXd& eigen_vector)
    {
        std::vector<double> vector((size_t)eigen_vector.size());
        for (size_t idx = 0; idx < (size_t)eigen_vector.size(); idx++)
        {
            const double val = eigen_vector[(ssize_t)idx];
            vector[idx] = val;
        }
        return vector;
    }

    // Takes <x, y, z, w> as is the ROS custom!
    inline Eigen::Quaterniond StdVectorDoubleToEigenQuaterniond(const std::vector<double>& vector)
    {
        if (vector.size() != 4)
        {
            std::cerr << "Quaterniond source vector is not 4 elements in size" << std::endl;
            assert(false);
        }
        Eigen::Quaterniond eigen_quaternion(vector[3], vector[0], vector[1], vector[2]);
        return eigen_quaternion;
    }

    // Returns <x, y, z, w> as is the ROS custom!
    inline std::vector<double> EigenQuaterniondToStdVectorDouble(const Eigen::Quaterniond& quat)
    {
        return std::vector<double>{quat.x(), quat.y(), quat.z(), quat.w()};
    }

    ////////////////////////////////////////////////////////////////////////////
    // Averaging functions
    // Numerically more stable averages taken from http://people.ds.cam.ac.uk/fanf2/hermes/doc/antiforgery/stats.pdf
    ////////////////////////////////////////////////////////////////////////////

    inline double AverageStdVectorDouble(
            const std::vector<double>& values,
            const std::vector<double>& weights = std::vector<double>())
    {
        // Get the weights
        assert(values.size() > 0);
        assert((weights.size() == values.size()) || (weights.size() == 0));
        const bool use_weights = (weights.size() != 0);
        // Find the first element with non-zero weight
        size_t starting_idx = 0;
        while (starting_idx < weights.size() && weights[starting_idx] == 0.0)
        {
            starting_idx++;
        }
        // If all weights are zero, result is undefined
        assert(starting_idx < values.size());
        // Start the recursive definition with the base case
        double average = values[starting_idx];
        const double starting_weight = use_weights ? std::abs(weights[starting_idx]) : 1.0;
        assert(starting_weight > 0.0);
        double weights_running_sum = starting_weight;
        // Do the weighted averaging on the rest of the vectors
        for (size_t idx = starting_idx + 1; idx < values.size(); idx++)
        {
            const double weight = use_weights ? std::abs(weights[idx]) : 1.0;
            weights_running_sum += weight;
            const double effective_weight = weight / weights_running_sum;
            const double prev_average = average;
            const double current = values[idx];
            average = prev_average + (effective_weight * (current - prev_average));
        }
        return average;
    }

    inline double ComputeStdDevStdVectorDouble(const std::vector<double>& values, const double mean)
    {
        assert(values.size() > 0);
        if (values.size() == 1)
        {
            return 0.0;
        }
        else
        {
            const double inv_n_minus_1 = 1.0 / (double)(values.size() - 1);
            double stddev_sum = 0.0;
            for (size_t idx = 0; idx < values.size(); idx++)
            {
                const double delta = values[idx] - mean;
                stddev_sum += (delta * delta);
            }
            return std::sqrt(stddev_sum * inv_n_minus_1);
        }
    }

    inline double ComputeStdDevStdVectorDouble(const std::vector<double>& values)
    {
        const double mean = AverageStdVectorDouble(values);
        return ComputeStdDevStdVectorDouble(values, mean);
    }

    /*
     * This function does not actually deal with the continuous revolute space correctly,
     * it just assumes a normal real Euclidean space
     */
    inline double AverageContinuousRevolute(
            const std::vector<double>& angles,
            const std::vector<double>& weights = std::vector<double>())
    {
        return AverageStdVectorDouble(angles, weights);
    }

    /**
     * This function is really only going to work well for "approximately continuous"
     *  types, i.e. floats and doubles, due to the implementation
     */
    template<typename ScalarType, int Rows, typename Allocator = std::allocator<Eigen::Matrix<ScalarType, Rows, 1>>>
    inline Eigen::Matrix<ScalarType, Rows, 1> AverageEigenVector(
            const std::vector<Eigen::Matrix<ScalarType, Rows, 1>, Allocator>& vectors,
            const std::vector<double>& weights = std::vector<double>())
    {
        // Get the weights
        assert(vectors.size() > 0);
        assert((weights.size() == vectors.size()) || (weights.size() == 0));
        const bool use_weights = (weights.size() != 0);
        // Find the first element with non-zero weight
        size_t starting_idx = 0;
        while (starting_idx < weights.size() && weights[starting_idx] == 0.0)
        {
            starting_idx++;
        }
        // If all weights are zero, result is undefined
        assert(starting_idx < vectors.size());
        // Start the recursive definition with the base case
        Eigen::Matrix<ScalarType, Rows, 1> avg_vector = vectors[starting_idx];
        const double starting_weight = use_weights ? std::abs(weights[starting_idx]) : 1.0;
        assert(starting_weight > 0.0);
        double weights_running_sum = starting_weight;
        // Do the weighted averaging on the rest of the vectors
        for (size_t idx = starting_idx + 1; idx < vectors.size(); ++idx)
        {
            const double weight = use_weights ? std::abs(weights[idx]) : 1.0;
            weights_running_sum += weight;
            const double effective_weight = weight / weights_running_sum;
            const Eigen::Matrix<ScalarType, Rows, 1> prev_avg_vector = avg_vector;
            const Eigen::Matrix<ScalarType, Rows, 1>& current = vectors[idx];
            avg_vector = prev_avg_vector + (effective_weight * (current - prev_avg_vector));
        }
        return avg_vector;
    }

    inline Eigen::Vector3d AverageEigenVector3d(
            const EigenHelpers::VectorVector3d& vectors,
            const std::vector<double>& weights = std::vector<double>())
    {
        return AverageEigenVector(vectors, weights);
    }

    inline Eigen::VectorXd AverageEigenVectorXd(
            const std::vector<Eigen::VectorXd>& vectors,
            const std::vector<double>& weights = std::vector<double>())
    {
        return AverageEigenVector(vectors, weights);
    }

    /**
     * Implementation of method described in (http://stackoverflow.com/a/27410865)
     * See paper at (http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf) for full explanation
     */
    inline Eigen::Quaterniond AverageEigenQuaterniond(
            const EigenHelpers::VectorQuaterniond& quaternions,
            const std::vector<double>& weights = std::vector<double>())
    {
        // Get the weights
        const bool use_weights = weights.size() == quaternions.size() ? true : false;
        assert(quaternions.size() > 0);
        assert((weights.size() == quaternions.size()) || (weights.size() == 0));
        // Shortcut the process if there is only 1 quaternion
        if (quaternions.size() == 1)
        {
            assert(weights.size() == 0 || weights[0] != 0.0);
            return quaternions[0];
        }
        // Build the averaging matrix
        Eigen::MatrixXd q_matrix(4, quaternions.size());
        for (size_t idx = 0; idx < quaternions.size(); idx++)
        {
            const double weight = use_weights ? std::abs(weights[idx]) : 1.0;
            const Eigen::Quaterniond& q = quaternions[idx];
            q_matrix.col((ssize_t)idx) << weight * q.w(), weight * q.x(), weight * q.y(), weight * q.z();
        }
        // Make the matrix square
        const Eigen::Matrix<double, 4, 4> qqtranspose_matrix = q_matrix * q_matrix.transpose();
        // Compute the eigenvectors and eigenvalues of the qqtranspose matrix
        const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> solver(qqtranspose_matrix);
        const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvalueType eigen_values = solver.eigenvalues();
        const Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvectorsType eigen_vectors = solver.eigenvectors();
        // Extract the eigenvector corresponding to the largest eigenvalue
        double max_eigenvalue = -INFINITY;
        int64_t max_eigenvector_index = -1;
        for (size_t idx = 0; idx < 4; idx++)
        {
            const double current_eigenvalue = eigen_values((long)idx).real();
            if (current_eigenvalue > max_eigenvalue)
            {
                max_eigenvalue = current_eigenvalue;
                max_eigenvector_index = (int64_t)idx;
            }
        }
        assert(max_eigenvector_index >= 0);
        // Note that these are already normalized!
        const Eigen::Vector4cd best_eigenvector = eigen_vectors.col((long)max_eigenvector_index);
        // Convert back into a quaternion
        const Eigen::Quaterniond average_q(best_eigenvector(0).real(), best_eigenvector(1).real(), best_eigenvector(2).real(), best_eigenvector(3).real());
        return average_q;
    }

    inline Eigen::Isometry3d AverageEigenIsometry3d(
            const EigenHelpers::VectorIsometry3d& transforms,
            const std::vector<double>& weights = std::vector<double>())
    {
        assert(transforms.size() > 0);
        assert((weights.size() == transforms.size()) || (weights.size() == 0));
        // Shortcut the process if there is only 1 transform
        if (transforms.size() == 1)
        {
            assert(weights.size() == 0 || weights[0] != 0.0);
            return transforms[0];
        }
        // Extract components
        EigenHelpers::VectorVector3d translations(transforms.size());
        EigenHelpers::VectorQuaterniond rotations(transforms.size());
        for (size_t idx = 0; idx < transforms.size(); idx++)
        {
            translations[idx] = transforms[idx].translation();
            rotations[idx] = Eigen::Quaterniond(transforms[idx].rotation());
        }
        // Average
        const Eigen::Vector3d average_translation = AverageEigenVector(translations, weights);
        const Eigen::Quaterniond average_rotation = AverageEigenQuaterniond(rotations, weights);
        // Make the average transform
        const Eigen::Isometry3d average_transform = (Eigen::Translation3d)average_translation * average_rotation;
        return average_transform;
    }

    ////////////////////////////////////////////////////////////////////////////
    // Geometry functions
    ////////////////////////////////////////////////////////////////////////////

    /**
     * @brief DistanceToLine
     * Math taken from http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
     * x = x0
     * point_on_line = x1
     * unit_vector = x2 - x1 / |x2 - x1|
     * @param point_on_line
     * @param unit_vector
     * @param x
     * @return The distance to the line, and the displacement along the line
     */
    inline std::pair<double, double> DistanceToLine(
            const Eigen::Vector3d& point_on_line,
            const Eigen::Vector3d& unit_vector,
            const Eigen::Vector3d x)
    {
        // Ensure that our input data is valid
        const auto real_unit_vector = unit_vector.normalized();
        if (!CloseEnough(unit_vector.norm(), 1.0, 1e-13))
        {
            std::cerr << "[Distance to line]: unit vector was not normalized: "
                      << unit_vector.transpose() << " Norm: " << unit_vector.norm() << std::endl;
        }

        const auto delta = x - point_on_line;
        const double displacement_along_line = real_unit_vector.dot(delta);
        const auto x_projected_onto_line = point_on_line + real_unit_vector * displacement_along_line;
        const double distance_to_line = (x_projected_onto_line - x).norm();

        // A simple neccescary (but not sufficient) check to look for math errors
        assert(IsApprox(distance_to_line * distance_to_line +
                        displacement_along_line * displacement_along_line, delta.squaredNorm(), 1e-10));

        return std::make_pair(distance_to_line, displacement_along_line);
    }

    ////////////////////////////////////////////////////////////////////////////
    // Projection/Rejection functions
    ////////////////////////////////////////////////////////////////////////////

    // Projects vector_to_project onto base_vector and returns the portion that is parallel to base_vector
    template <typename DerivedB, typename DerivedV>
    inline Eigen::Matrix<typename DerivedB::Scalar, Eigen::Dynamic, 1> VectorProjection(
            const Eigen::MatrixBase<DerivedB>& base_vector,
            const Eigen::MatrixBase<DerivedV>& vector_to_project)
    {
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(DerivedB);
        EIGEN_STATIC_ASSERT_VECTOR_ONLY(DerivedV);
        EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(DerivedB, DerivedV)
        static_assert(std::is_same<typename DerivedB::Scalar, typename DerivedV::Scalar>::value,
                      "base_vector and vector_to_project must have the same data type");
        // Perform projection
        const typename DerivedB::Scalar b_squared_norm = base_vector.squaredNorm();
        if (b_squared_norm > 0)
        {
            return (base_vector.dot(vector_to_project) / b_squared_norm) * base_vector;
        }
        else
        {
            return Eigen::Matrix<typename DerivedB::Scalar, Eigen::Dynamic, 1>::Zero(base_vector.rows());
        }
    }

    // Projects vector_to_project onto base_vector and returns the portion that is perpendicular to base_vector
    template <typename DerivedB, typename DerivedV>
    inline Eigen::Matrix<typename DerivedB::Scalar, Eigen::Dynamic, 1> VectorRejection(
            const Eigen::MatrixBase<DerivedB>& base_vector,
            const Eigen::MatrixBase<DerivedV>& vector_to_project)
    {
        // Rejection is defined in relation to projection
        return vector_to_project - VectorProjection(base_vector, vector_to_project);
    }

    ////////////////////////////////////////////////////////////////////////////
    // (Weighted) dot product, norm, and angle functions
    ////////////////////////////////////////////////////////////////////////////

    // Returns the (non-negative) angle defined by the vectors (b - a), and (b - c)
    template <typename DerivedA, typename DerivedB, typename DerivedC>
    inline double AngleDefinedByPoints(const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedB>& b, const Eigen::MatrixBase<DerivedC>& c)
    {
        // Check for potential numerical problems
        if (a.isApprox(b) || (b.isApprox(c)))
        {
            std::cerr << "Warning: Potential numerical stability problems in AngleDefinedByPoints\n";
        }

        // Do the actual math here
        const auto vec1 = (a - b).normalized();
        const auto vec2 = (c - b).normalized();
        const double cosine_raw = vec1.dot(vec2);
        const double cosine = std::max(-1.0, std::min(cosine_raw, 1.0));
        return std::acos(cosine);
    }

    inline double WeightedDotProduct(const Eigen::VectorXd& vec1, const Eigen::VectorXd& vec2, const Eigen::VectorXd& weights)
    {
        return vec1.cwiseProduct(weights).dot(vec2);
    }

    inline double WeightedSquaredNorm(const Eigen::VectorXd& vec, const Eigen::VectorXd weights)
    {
        return WeightedDotProduct(vec, vec, weights);
    }

    inline double WeightedNorm(const Eigen::VectorXd& vec, const Eigen::VectorXd& weights)
    {
        return std::sqrt(WeightedSquaredNorm(vec, weights));
    }

    inline double WeightedCosineAngleBetweenVectors(const Eigen::VectorXd& vec1, const Eigen::VectorXd& vec2, const Eigen::VectorXd& weights)
    {
        const double vec1_norm = WeightedNorm(vec1, weights);
        const double vec2_norm = WeightedNorm(vec2, weights);
        assert(vec1_norm > 0 && vec2_norm > 0);
        const double result = WeightedDotProduct(vec1, vec2, weights) / (vec1_norm * vec2_norm);
        return std::max(-1.0, std::min(result, 1.0));
    }

    inline double WeightedAngleBetweenVectors(const Eigen::VectorXd& vec1, const Eigen::VectorXd& vec2, const Eigen::VectorXd& weights)
    {
        return std::acos(WeightedCosineAngleBetweenVectors(vec1, vec2, weights));
    }

    ////////////////////////////////////////////////////////////////////////////
    // Other auxiliary functions
    ////////////////////////////////////////////////////////////////////////////

    class Hyperplane
    {
    protected:

        Eigen::VectorXd plane_origin_;
        Eigen::VectorXd plane_normal_;

    public:

        Hyperplane(const Eigen::VectorXd& origin, const Eigen::VectorXd& normal)
        {
            assert(origin.size() == normal.size());
            plane_origin_ = origin;
            plane_normal_ = normal;
        }

        Hyperplane() {}

        const Eigen::VectorXd& GetOrigin() const
        {
            return plane_origin_;
        }

        const Eigen::VectorXd& GetNormal() const
        {
            return plane_normal_;
        }

        double GetNormedDotProduct(const Eigen::VectorXd& point) const
        {
            assert(point.size() == plane_origin_.size());
            const Eigen::VectorXd check_vector = point - plane_origin_;
            const Eigen::VectorXd check_vector_normed = EigenHelpers::SafeNormal(check_vector);
            const double dot_product = check_vector_normed.dot(plane_normal_);
            return dot_product;
        }

        double GetRawDotProduct(const Eigen::VectorXd& point) const
        {
            assert(point.size() == plane_origin_.size());
            const Eigen::VectorXd check_vector = point - plane_origin_;
            const double dot_product = check_vector.dot(plane_normal_);
            return dot_product;
        }

        Eigen::VectorXd RejectVectorOntoPlane(const Eigen::VectorXd& vector) const
        {
            return VectorProjection(plane_normal_, vector);
        }

        double GetSquaredDistanceToPlane(const Eigen::VectorXd& point) const
        {
            const Eigen::VectorXd origin_to_point_vector = point - plane_origin_;
            return VectorProjection(plane_normal_, origin_to_point_vector).squaredNorm();
        }

        double GetDistanceToPlane(const Eigen::VectorXd& point) const
        {
            const Eigen::VectorXd origin_to_point_vector = point - plane_origin_;
            return VectorProjection(plane_normal_, origin_to_point_vector).norm();
        }

        Eigen::VectorXd ProjectVectorOntoPlane(const Eigen::VectorXd& vector) const
        {
            return VectorRejection(plane_normal_, vector);
        }

        Eigen::VectorXd ProjectPointOntoPlane(const Eigen::VectorXd& point) const
        {
            const Eigen::VectorXd origin_to_point_vector = point - plane_origin_;
            const Eigen::VectorXd projected_to_point_vector = VectorRejection(plane_normal_, origin_to_point_vector);
            const Eigen::VectorXd projected_point = plane_origin_ + projected_to_point_vector;
            return projected_point;
        }
    };

    /*
     * Returns a pair of <centroid point, normal vector> defining the plane
     */
    inline Hyperplane FitPlaneToPoints(const std::vector<Eigen::VectorXd>& points)
    {
        // Subtract out the centroid
        const Eigen::VectorXd centroid = EigenHelpers::AverageEigenVectorXd(points);
        Eigen::MatrixXd centered_points(centroid.size(), points.size());
        for (size_t idx = 0; idx < points.size(); idx++)
        {
            const Eigen::VectorXd& current_point = points[idx];
            centered_points.block(0, (ssize_t)idx, centroid.size(), 1) = (current_point - centroid);
        }
        // Compute SVD of the centered points
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(centered_points, Eigen::ComputeThinU | Eigen::ComputeThinV);
        // Get results of SVD
        const Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType& singular_values = svd.singularValues();
        const Eigen::JacobiSVD<Eigen::MatrixXd>::MatrixUType& u_matrix = svd.matrixU();
        // Get the left singular vector corresponding to the minimum singular value
        double minimum_singular_value = INFINITY;
        ssize_t best_singular_value_index = -1;
        for (ssize_t idx = 0; idx < singular_values.size(); idx++)
        {
            const std::complex<double> current_singular_value = singular_values(idx);
            if (current_singular_value.real() < minimum_singular_value)
            {
                minimum_singular_value = current_singular_value.real();
                best_singular_value_index = idx;
            }
        }
        assert(best_singular_value_index >= 0);
        // The corresponding left singular vector is the normal vector of the best-fit plane
        const Eigen::VectorXd best_left_singular_vector = u_matrix.col(best_singular_value_index);
        const Eigen::VectorXd normal_vector = EigenHelpers::SafeNormal(best_left_singular_vector);
        return Hyperplane(centroid, normal_vector);
    }

    inline double SuggestedRcond()
    {
        return 0.001;
    }

    // Derived from code by Yohann Solaro ( http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/01/msg00187.html )
    // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
    inline Eigen::MatrixXd Pinv(const Eigen::MatrixXd& b, const double rcond, const bool enable_flip=true)
    {
        bool flip = false;
        Eigen::MatrixXd a;
        if (enable_flip && (b.rows() < b.cols()))
        {
            a = b.transpose();
            flip = true;
        }
        else
        {
            a = b;
        }
        // SVD
        Eigen::JacobiSVD<Eigen::MatrixXd> svdA;
        svdA.compute(a, Eigen::ComputeFullU | Eigen::ComputeThinV);
        Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType vSingular = svdA.singularValues();
        // Build a diagonal matrix with the Inverted Singular values
        // The pseudo inverted singular matrix is easy to compute :
        // is formed by replacing every nonzero entry by its reciprocal (inversing).
        Eigen::VectorXd vPseudoInvertedSingular(svdA.matrixV().cols());
        for (int iRow = 0; iRow < vSingular.rows(); iRow++)
        {
            if (std::fabs(vSingular(iRow)) <= rcond) // Todo : Put epsilon in parameter
            {
                vPseudoInvertedSingular(iRow)= 0.0;
            }
            else
            {
                vPseudoInvertedSingular(iRow) = 1.0 / vSingular(iRow);
            }
        }
        // A little optimization here
        const Eigen::MatrixXd mAdjointU = svdA.matrixU().adjoint().block(0, 0, vSingular.rows(), svdA.matrixU().adjoint().cols());
        // Yes, this is ugly. This is to suppress a warning on type conversion related to Eigen operations
        #pragma GCC diagnostic push
        #pragma GCC diagnostic ignored "-Wconversion"
        // Pseudo-Inversion : V * S * U'
        const Eigen::MatrixXd a_pinv = (svdA.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU;
        #pragma GCC diagnostic pop
        // Flip back if need be
        if (flip)
        {
            return a_pinv.transpose();
        }
        else
        {
            return a_pinv;
        }
    }

    /**
     * @brief WeightedLeastSquaresSolver Solves the minimization problem min || Ax - b ||^2 for x, using weights w in the norm
     *                                   If the problem is ill-conditioned, adds in a damping factor. This is equivalent to
     *                                   solving A^T * diag(W) * A * x = A^T * diag(W) * b for x.
     * @param A size M x N with M > N
     * @param b size M x 1
     * @param w size M x 1
     * @param damping_threshold The smallest singular value we allow in A^T * W * A before we apply damping
     * @param damping_value The damping value we apply to the main diagonal of A^T * W * A if we exceed the threshold
     * @return size N x 1
     */
    inline Eigen::VectorXd WeightedLeastSquaresSolver(const Eigen::MatrixXd& A, const Eigen::VectorXd& b, const Eigen::VectorXd& w, const double damping_threshold, const double damping_value)
    {
        // Yes, this is ugly. This is to suppress a warning on type conversion related to Eigen operations
        #pragma GCC diagnostic push
        #pragma GCC diagnostic ignored "-Wconversion"
        Eigen::MatrixXd left_side = A.transpose() * w.asDiagonal() * A;
        #pragma GCC diagnostic pop
        const double minimum_singular_value = left_side.jacobiSvd().singularValues().minCoeff();

        if (minimum_singular_value < damping_threshold)
        {
            left_side += damping_value * Eigen::MatrixXd::Identity(left_side.rows(), left_side.cols());
        }

        // With the damping we can assume that the left side is positive definite, so use LLT to solve this
        return left_side.llt().solve(A.transpose() * w.cwiseProduct(b));
    }
}

#endif // EIGEN_HELPERS_HPP


/**
 * @file EmbedPDBQTMols.cpp
 * @author Louai KASSA BAGHDOUCHE
 * @brief A C++ code to generate conformers and their USRCAT Features from multiple pdbqt files, by fetching the SMILES and ID
 * from PDBQT and generate SMI files
 * @date 2022-03-29
 *
 * @copyright Copyright (c) 2022
 *
 */

#include <iostream>
#include <string>
#include <thread>
#include <array>
#include <vector>
#include <mutex>
#include <GraphMol/FileParsers/MolSupplier.h>
#include <GraphMol/DistGeomHelpers/Embedder.h>
#include <GraphMol/FileParsers/MolWriters.h>
#include <GraphMol/SmilesParse/SmilesParse.h>
#include <GraphMol/Descriptors/MolDescriptors.h>
#include <GraphMol/Substruct/SubstructMatch.h>
#include <boost/filesystem/fstream.hpp>
#include <boost/filesystem/operations.hpp>

using namespace std;
using namespace std::chrono;
using namespace RDGeom;
using namespace RDKit;
using namespace RDKit::MolOps;
using namespace RDKit::DGeomHelpers;
using namespace RDKit::Descriptors;
using namespace boost::filesystem;
using boost_ifstream = boost::filesystem::ifstream;
using boost_ofstream = boost::filesystem::ofstream;

static const size_t s_Num_references = 4;
static const size_t s_Num_subsets = 5;
static const array<string, s_Num_subsets> s_SubsetSMARTS
{{
  "[!#1]", // heavy
  "[#6+0!$(*~[#7,#8,F]),SH0+0v2,s+0,S^3,Cl+0,Br+0,I+0]", // hydrophobic
  "[a]", // aromatic
  "[$([O,S;H1;v2]-[!$(*=[O,N,P,S])]),$([O,S;H0;v2]),$([O,S;-]),$([N&v3;H1,H2]-[!$(*=[O,N,P,S])]),$([N;v3;H0]),$([n,o,s;+0]),F]", // acceptor
  "[N!H0v3,N!H0+v4,OH+0,SH+0,nH+0]", // donor
}};

static array<unique_ptr<ROMol>, s_Num_subsets> SubsetMols;
static array<vector<int>, s_Num_subsets> subsets;
static array<Point3D, s_Num_references> references;
static array<vector<float>, s_Num_references> dista;

// Compute the Euclidien distance
template<typename T>
static inline float dist2(const T& p0, const T& p1)
{
	const auto d0 = p0[0] - p1[0];
	const auto d1 = p0[1] - p1[1];
	const auto d2 = p0[2] - p1[2];
	return d0 * d0 + d1 * d1 + d2 * d2;
}

static array<float, 60> usrcat_features(ROMol& mol, int index)
{
  array<float, 60> features;
  // Wrap SMARTS strings to ROMol objects.
  for (size_t k = 0; k < s_Num_subsets; ++k)
  {
    SubsetMols[k].reset(reinterpret_cast<ROMol*>(SmartsToMol(s_SubsetSMARTS[k])));
  }
  const auto num_points = mol.getNumHeavyAtoms();
  const auto& conformer = mol.getConformer(index);
  for (size_t k = 0; k < s_Num_subsets; ++k)
  {
    vector<vector<pair<int, int>>> matchVect;
    SubstructMatch(mol, *SubsetMols[k], matchVect);
    const auto num_matches = matchVect.size();
    auto& subset = subsets[k];
    subset.resize(num_matches);
    for (size_t j = 0; j < num_matches; ++j)
    {
      subset[j] = matchVect[j].front().second;
    }
  }
  const auto& subset0 = subsets.front();
  // assert(subset0.size() == num_points);

  for (auto& ref : references)
  {
    ref.x = ref.y = ref.z = 0;
  }
  auto& ctd = references[0];
  auto& cst = references[1];
  auto& fct = references[2];
  auto& ftf = references[3];
  for (const auto sub : subset0)
  {
    const auto& a = conformer.getAtomPos(sub);
    ctd += a;
  }
  ctd /= num_points;
  float cst_dist = numeric_limits<float>::max();
  float fct_dist = numeric_limits<float>::lowest();
  float ftf_dist = numeric_limits<float>::lowest();
  for (const auto sub : subset0)
  {
    const auto& a = conformer.getAtomPos(sub);
    const auto this_dist = dist2(a, ctd);
    if (this_dist < cst_dist)
    {
      cst = a;
      cst_dist = this_dist;
    }
    if (this_dist > fct_dist)
    {
      fct = a;
      fct_dist = this_dist;
    }
  }
  for (const auto sub : subset0)
  {
    const auto& a = conformer.getAtomPos(sub);
    const auto this_dist = dist2(a, fct);
    if (this_dist > ftf_dist)
    {
      ftf = a;
      ftf_dist = this_dist;
    }
  }
  // Precalculate the distances of heavy atoms to the reference points, given that subsets[1 to 4] are subsets of subsets[0].
  for (size_t ref = 0; ref < s_Num_references; ++ref)
  {
    const auto& reference = references[ref];
    auto& distp = dista[ref];
    distp.resize(num_points);
    for (size_t p = 0; p < num_points; ++p)
    {
      distp[subset0[p]] = sqrt(dist2(conformer.getAtomPos(subset0[p]), reference));
    }
  }
  // loop over pharmacophoric subsets and reference points.
  size_t qo = 0;
  for (const auto& subset : subsets)
  {
    const auto n = subset.size();
    for (size_t ref = 0; ref < s_Num_references; ++ref)
    {
      // Load distances from precalculated ones
      const auto& distp = dista[ref];
      vector<float> dists(n);
      for (size_t a = 0; a < n; ++a)
      {
        dists[a] = distp[subset[a]];
      }
      // Compute moments
      array<float, 3> m{};
      if (n > 2)
      {
        const auto v = 1.0 / n;
        for (size_t j = 0; j < n; ++j)
        {
          const auto d = dists[j];
          m[0] += d;
        }
        m[0] *= v;
        for (size_t j = 0; j < n; ++j)
        {
          const auto d = dists[j] - m[0];
          m[1] += d * d;
        }
        m[1] = sqrt(m[1] * v);
        for (size_t j = 0; j < n; ++j)
        {
          const auto d = dists[j] - m[0];
          m[2] += d * d * d;
        }
        m[2] = cbrt(m[2] * v);
      }
      else if (n == 2)
      {
        m[0] = 0.5 * (dists[0] + dists[1]);
        m[1] = 0.5 * fabs(dists[0] - dists[1]);
      }
      else if (n == 1)
      {
        m[0] = dists[0];
      }
      for (const auto e : m)
      {
        features[qo++] = e;
      }
    }
  }
  return features;
}

// function to remove whitespaces
static inline auto stripWhiteSpaces(string& str)
{
  str.erase(remove(str.begin(), str.end(), ' '), str.end());
}

static mutex s_Mu;

template<typename T>
static inline void write_to_binary(T& buf, boost_ofstream& ofs)
{
  // pass the ofstream object by reference, to avoid declaring it each time
  lock_guard<mutex> lock(s_Mu);
  const size_t num_bytes = sizeof(buf);
  ofs.write(reinterpret_cast<char*>(buf.data()), num_bytes);
}

int main(int argc, char* argv[])
{
  if (argc != 4)
  {
    cerr << "Usage: ./EmbedAllAndGenerateSMIL [PDBQT FOLDER] [CONFORMERS SDF] [OUTPUT SMI]" << endl;
    return 1;
  }

  // Obtain the files from the CLI argument
  const auto pdbqt_folder = argv[1];
  const auto conformers_file = argv[2];
  const auto smi_file = argv[3];

  // generate smiles.txt and realid.txt and properties
  int p = string(smi_file).find('.');
  const path only_smiles_txt = string(smi_file).substr(0, p) + "_only_smiles.txt";
  const path only_id_txt = string(smi_file).substr(0, p) + "_only_id.txt";
  p = string(conformers_file).find('.');
  const path rfprop_file = string(conformers_file).substr(0, p) + "_4properties.f32";
  const path riprop_file = string(conformers_file).substr(0, p) + "_5properties.i16";
  const path usrcatf64_file = string(conformers_file).substr(0, p) + "_usrcat.f64";

  // Inialize the output files
  boost_ofstream conf_file(conformers_file, ios::app);
  boost_ofstream smifile(smi_file, ios::app);
  boost_ofstream smilesfile(only_smiles_txt, ios::app);
  boost_ofstream id_file(only_id_txt, ios::app);
  boost_ofstream rfprop(rfprop_file, ios::binary | ios::app);
  boost_ofstream riprop(riprop_file, ios::binary | ios::app);
  boost_ofstream usrcatf64(usrcatf64_file, ios::binary | ios::app);

  // Initalize constants
  const string pdbqt_extension = ".pdbqt";

  // Initialize variables
  string line, compound, smiles, next_line;
  int pos;
  vector<thread> thread_pool;
  thread_pool.reserve(3); // thread pool with 3 threads
  array<float, 60> features;
  array<float, 4> realfprop;
  array<int16_t, 5> realiprop;
  size_t processed_ligand = 0;
  size_t all_ligands = 0;
  EmbedParameters params(srETKDGv3);
  params.randomSeed = 209;
  params.numThreads = 8;
  params.useRandomCoords = true; // this parameter is used to make the process faster
  params.maxIterations = 3; // max iterations to 1
  SDWriter writer(&conf_file);

  // Search for pdbqt files into the pdbqt folder
  for (const auto& entry : recursive_directory_iterator(pdbqt_folder))
  {
    if (entry.path().extension() == pdbqt_extension)
    {
      std::cout << "Processing molecule Number° " << processed_ligand << '/' << all_ligands << endl;
      std::cout << "Path of the molecule " << entry.path() << endl;
      boost_ifstream ifs(entry.path());
      while (getline(ifs, line))
      {
        if (line.find("Compound:") != string::npos)
        {
          pos = line.find(':');
          compound = line.substr(pos + 1);
          stripWhiteSpaces(compound);
        }
        if (line.find("SMILES:") != string::npos)
        {
          /**
           * @brief Some compounds of the REAL library strangely contain a 'q' and 'r' characters in their
           * SMILES, and other compounds have a splitted SMILES in two lines. These problems cause a Segfault error raised from the RDKit API.
           * Here is a check of the goodness of the SMILES molecule, to avoid the Segfault error
           */
          pos = line.find(':');
          smiles = line.substr(pos + 1);
          stripWhiteSpaces(smiles);
          if (getline(ifs, next_line))
          {
            if (next_line.find("REMARK") != string::npos)
            {
              std::cout << "No problem with the SMILES!" << endl;
            }
            else
            {
              cerr << "The SMILES is splited, trying to fix the SMILES" << endl;
              smiles = smiles + next_line;
              std::cout << "The fixed SMILES: " << smiles << endl;
            }
          }
          if (smiles.find('q') != string::npos || smiles.find('r') != string::npos || smiles.find('s') != string::npos)
          {
            cerr << "Incorrect format of the SMILES" << endl;
            break;
          }
          else
          {
            try
            {
              const unique_ptr<ROMol> smi_ptr(SmilesToMol(smiles));
              const unique_ptr<ROMol> mol_ptr(addHs(*smi_ptr));
              auto& mol = *mol_ptr;
              mol.setProp("_Name", compound);
              // generate conformers
              const auto confIds = EmbedMultipleConfs(mol, 4, params);
              if (confIds.empty())
              {
                cerr << "Error, in parsing molecule. Conformers not generated!" << endl;
                break;
              }
              // Check if the molecule has 4 conformers
              if (confIds.size() == 4)
              {
                // generate 4 chemical properties for the molecules
                realfprop[0] = calcExactMW(mol);
                realfprop[1] = calcClogP(mol);
                realfprop[2] = calcTPSA(mol);
                realfprop[3] = calcLabuteASA(mol);
                // add this task to the thread inside the threads vector
                thread_pool.emplace_back([&]() {
                  write_to_binary<array<float, 4>>(realfprop, rfprop);
                }); // emplace_back to avoid copying, so that we can pass the parameters directly
                //write_to_binary<array<float, 4>>(realfprop, rfprop_file);

                // generate 5 chemical properties for the molecules
                realiprop[0] = mol.getNumHeavyAtoms();
                realiprop[1] = calcNumHBD(mol);
                realiprop[2] = calcNumHBA(mol);
                realiprop[3] = calcNumRotatableBonds(mol);
                realiprop[4] = calcNumRings(mol);
                // add this task to the thread inside the threads vector
                thread_pool.emplace_back([&]() {
         	  write_to_binary<array<int16_t, 5>>(realiprop, riprop);
                });

                std::cout << confIds.size() << " Conformers of " << compound << '\t' << smiles << " are succefully generated!" << endl;
                id_file << compound << '\n';
                smilesfile << smiles << '\n';
                smifile << compound << '\t' << smiles << '\n';
                // Writing conformers in the output SDF
                for (const auto confId : confIds)
                {
                  writer.write(mol, confId);
                }

                // generate the USRCAT features for each conformer
                thread_pool.emplace_back([&]() {
                  for (int i = 0; i < 4; i++)
                  {
                    features = usrcat_features(mol, i);
                    write_to_binary<array<float, 60>>(features, usrcatf64);
                  }
                });

                /*for (int i = 0; i < 4; i++)
                {
                  features = usrcat_features(mol, i);
                  write_to_binary<array<float, 60>>(features, usrcatf64);
                }*/
                processed_ligand++;
                break;
              }
              else
              {
                std::cout << "Molecule doesn't have 4 conformers!" << endl;
                break;
              }
            }
            catch (const MolSanitizeException& e)
            {
              cerr << "Kekulization problem!" << endl;
              break;
            }
          }
        }
      }
      // join the threads
      for (thread& th : thread_pool)
      {
        th.join();
      }
      thread_pool.clear();
      all_ligands++;
    }
  }
  std::cout << "Process completed! for " << all_ligands << " compounds" << endl;
}