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

///
/// \file partial_lanczos_bidiagonalization.hpp
///
#ifndef MXPFIT_PARTIAL_LANCZOS_BIDIAGONALIZATION_HPP
#define MXPFIT_PARTIAL_LANCZOS_BIDIAGONALIZATION_HPP

#include <Eigen/Core>

namespace mxpfit
{

///
/// ### PartialLanczosBidiagonalization
///
/// \brief Low-rank approximation of a matrix by the Lanczos bidiagonalization
/// with full reorthogonalization
///
/// \tparam MatrixT Matrix type to be decomposed. We expect that `MatrixT`
/// inherits Eigen::EigenBase class.
///
/// For a given \f$m \times n\f$ matrix \f$A\f$ and prescribed accuracy
/// \f$\epsilon,\f$ this class computes the approximate decomposition such that
///
/// \f[
///   \|A - P_{k}^{} B_{k} Q_{k}^{\ast} \|_{F} < \epsilon.
/// \f]
///
/// where \f$P_{k}\f$ is a \f$ m \times k \f$ matrix with orthonormal columns,
/// and \f$ Q_{k} \f$ is a \f$m \times k\f$ matrix with orthonormal columns.
/// \f$B_{k}\f$ is a real bidiagonal matrix
///
/// #### References
///
/// 1. D. Potts and M. Tasche, "Fast ESPRIT algorithms based on partial singular
///    value decompositions", Appl. Numer. Math. **88** (2015) 31-45.
///    [DOI: http://doi.org/10.1016/j.apnum.2014.10.003]
///
template <typename MatrixT>
class PartialLanczosBidiagonalization
{
public:
    using MatrixType    = MatrixT;
    using Scalar        = typename MatrixType::Scalar;
    using RealScalar    = typename MatrixType::RealScalar;
    using Index         = Eigen::Index;
    using Vector        = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
    using RealVector    = Eigen::Matrix<RealScalar, Eigen::Dynamic, 1>;
    using Matrix        = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    using RealVectorRef = Eigen::Ref<RealVector>;
    using MatrixRef     = Eigen::Ref<Matrix>;

    PartialLanczosBidiagonalization()
        : m_matP(),
          m_matQ(),
          m_alpha(),
          m_beta(),
          m_rank(),
          m_tolerance(Eigen::NumTraits<RealScalar>::epsilon()),
          m_error(),
          m_is_initialized()

    {
    }

    PartialLanczosBidiagonalization(Index nrows, Index ncols, Index nsteps)
        : m_matP(nrows, nsteps),
          m_matQ(ncols, nsteps),
          m_alpha(nsteps),
          m_beta(nsteps),
          m_rank(),
          m_tolerance(Eigen::NumTraits<RealScalar>::epsilon()),
          m_error(),
          m_is_initialized()
    {
    }

    PartialLanczosBidiagonalization(const MatrixType& mat, Index nsteps)
        : m_matP(mat.rows(), nsteps),
          m_matQ(mat.cols(), nsteps),
          m_alpha(nsteps),
          m_beta(nsteps),
          m_rank(),
          m_tolerance(Eigen::NumTraits<RealScalar>::epsilon()),
          m_error(),
          m_is_initialized()
    {
        compute(mat, nsteps);
    }

    void compute(const MatrixType& matA, Index nsteps);

    void setTolerance(RealScalar tolerance) noexcept
    {
        m_tolerance = tolerance;
    }

    RealScalar tolerance() const noexcept
    {
        return m_tolerance;
    }

    Index rank() const noexcept
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");
        return m_rank;
    }

    RealScalar error() const noexcept
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");
        return m_error;
    }

    const Matrix& matrixP() const noexcept
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");
        return m_matP;
    }

    const Matrix& matrixQ() const noexcept
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");
        return m_matQ;
    }

    const RealVector& diagonalAlpha() const noexcept
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");
        return m_alpha;
    }

    const RealVector& superdiagonalBeta() const noexcept
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");
        return m_beta;
    }

    Matrix reconstructedMatrix() const
    {
        assert(m_is_initialized &&
               "PartialLanczosBidiagonalization is not initialized.");

        if (m_rank == Index())
        {
            return Matrix();
        }

        auto viewP = m_matP.leftCols(m_rank);
        auto viewQ = m_matQ.leftCols(m_rank);
        Matrix matB(Matrix::Zero(m_rank, m_rank));
        matB.diagonal()  = m_alpha.head(m_rank);
        matB.diagonal(1) = m_beta.head(m_rank - 1);

        return viewP * matB * viewQ.adjoint();
    }

protected:
    Matrix m_matP;
    Matrix m_matQ;
    RealVector m_alpha;
    RealVector m_beta;

    Index m_rank;
    RealScalar m_tolerance;
    RealScalar m_error;
    bool m_is_initialized;
};

template <typename MatrixT>
void PartialLanczosBidiagonalization<MatrixT>::compute(const MatrixType& matA,
                                                       Index nsteps)
{
    assert(Index() <= nsteps && nsteps <= matA.rows() && nsteps <= matA.cols());

    m_matP.resize(matA.rows(), nsteps);
    m_matQ.resize(matA.cols(), nsteps);
    m_alpha.resize(nsteps);
    m_beta.resize(nsteps - 1);

    Vector workspace(nsteps);

    auto q0 = m_matQ.col(0);
    // Set q0 as unit vector (1,0,0,....)^T plus small perturbation
    q0.setRandom();
    q0(0) = RealScalar(1) /
            Eigen::numext::sqrt(Eigen::NumTraits<RealScalar>::epsilon());
    q0.normalize(); // make q0 normalized
    auto p0       = m_matP.col(0);
    p0            = matA * q0;
    const auto a1 = p0.norm();
    if (a1 > RealScalar())
    {
        p0 *= RealScalar(1) / a1;
    }
    m_alpha(0) = a1;

    const RealScalar tol2 = m_tolerance * m_tolerance;
    RealScalar fnorm_A    = a1 * a1; // Estimation of |A|_F
    m_error               = RealScalar();
    Index irank           = 0;

    while (++irank < nsteps)
    {
        auto p1 = m_matP.col(irank - 1);
        auto p2 = m_matP.col(irank);
        auto q1 = m_matQ.col(irank - 1);
        auto q2 = m_matQ.col(irank);
        //
        // --- Recursion for right Lanczos vector
        //
        q2 = matA.adjoint() * p1 - m_alpha(irank - 1) * q1;
        // Reorthogonalization
        auto tmp   = workspace.head(irank);
        auto viewQ = m_matQ.leftCols(irank);
        tmp        = viewQ.adjoint() * q2;
        q2 -= viewQ * tmp;
        auto b1 = q2.norm();
        if (b1 > RealScalar())
        {
            q2 *= RealScalar(1) / b1;
        }
        m_beta(irank - 1) = b1;
        //
        // --- Recursion for left Lanczos vector
        //
        // p2 <-- A * q2 - beta(i) * p1
        p2 = matA * q2 - m_beta(irank - 1) * p1;
        // Reorthogonalization
        auto viewP = m_matP.leftCols(irank);
        tmp        = viewP.adjoint() * p2;
        p2 -= viewP * tmp;

        auto a2 = p2.norm();
        if (a2 > RealScalar())
        {
            p2 *= RealScalar(1) / a2;
        }
        m_alpha(irank) = a2;
        //
        // Update frobenius norm of matrix A via
        //
        // ||A||_{F}^{2} = \sum_{K=1}^{rank(A)-1}
        //       (\alpha_{K}^{2} + \beta_{K}^{2}) + \alpha_{rank(A)}}
        //
        auto t = a2 * a2 + b1 * b1;
        fnorm_A += t;
        m_error = t / fnorm_A;
        if (m_error <= tol2)
        {
            break; // converged
        }
    }

    m_rank           = irank;
    m_is_initialized = true;
}

} // namespace mxpfit

#endif /* MXPFIT_PARTIAL_LANCZOS_BIDIAGONALIZATION_HPP*/


/**
 * Copyright (c) 2020 Melown Technologies SE
 *
 * 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.
 *
 * 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.
 */
#include "estimate-point-normals.hpp"
#include "bvh.hpp"
#include "math/math.hpp"
#include "math/geometry.hpp"

#include <boost/iterator/counting_iterator.hpp>

namespace geometry {

namespace ublas = math::ublas;

Eigen::VectorXd estimateNormal(const Eigen::MatrixXd& data)
{
    using namespace Eigen;
    // calculate centroid (1 x K) of all (N) data points
    RowVectorXd centroid = data.colwise().mean();
    // mean-center sample matrix
    MatrixXd centered = data.rowwise() - centroid;
    // calculate the covariance matrix (K x K), for PCA we may ommit
    // scaling by the factor 1/(N - 1)
    MatrixXd covariance = centered.transpose() * centered;

    // calculate SVD (ordered by the magnitude of SV)
    JacobiSVD<MatrixXd> svd(covariance, ComputeFullU);
    // the normal is the last column of U, singular vector of the covariance
    // matrix corresponding to the smallest singular value
    // (i.e., the direction of the least variability in the data)
    VectorXd sgVec(svd.matrixU().col(data.cols() - 1));

    // normalization shouldn't be needed as U should be a unitary matrix.
    return sgVec;//.normalized();
}

namespace {

    class BvhDisk : public BvhPrimitive {
        math::Point3 center_;
        math::Point3 normal_;
        double radius_;

    public:
        BvhDisk() = default;

        BvhDisk(const math::Point3& center
                , const math::Point3& normal
                , const double radius
                , const std::uint32_t index)
            : center_(center)
            , normal_(normal)
            , radius_(radius) {
            assert(radius_ > 0.);
            userData = index;
        }

        bool getIntersection(const Ray& ray, IntersectionInfo& intersection) const {
            // ray-plane intersection
            const double denom = inner_prod(normal_, ray.direction());
            if (std::fabs(denom) < 1.e-6) {
                return false;
            }
            const math::Point3 diff = center_ - ray.origin();
            const double t = inner_prod(diff, normal_) / denom;
            if (t > 0.) {
                // find the distance of the intersection from the disk center
                const math::Point3 dp = ray.origin() + ray.direction() * t - center_;
                const double distSqr = inner_prod(dp, dp);
                if (distSqr < math::sqr(radius_)) {
                    intersection.object = this;
                    intersection.t = t;
                    return true;
                }
            }

            return false;
        }

        math::Extents3 getBBox() const {
            ublas::scalar_vector<double> size(3, radius_);
            return math::Extents3(center_ - size, center_ + size);
        }

        math::Point3 getCenter() const {
            return center_;
        }
    };

} // namespace

static constexpr float DX = 0.25;
static constexpr float DZ = 1.;
static math::Points3 ALL_DIRS = {
    math::Point3(0., 0., DZ),
    math::Point3(0., DX, DZ),
    math::Point3(0., -DX, DZ),
    math::Point3(DX, 0., DZ),
    math::Point3(-DX, 0., DZ),
    math::Point3(DX, DX, DZ),
    math::Point3(DX, -DX, DZ),
    math::Point3(-DX, -DX, DZ),
    math::Point3(-DX, DX, DZ),
};

void reorientNormals(const std::vector<math::Point3>& pc
                     , std::vector<math::Point3>& normals
                     , const double pointRadius)
{
    std::vector<BvhDisk> disks(pc.size());
    UTILITY_OMP(parallel for)
    for (std::uint32_t i = 0; i < pc.size(); ++i) {
        disks[i] = BvhDisk(pc[i], normals[i], pointRadius, i);
    }

    LOG(info2) << "Building BVH for " << disks.size() << " points";
    geometry::Bvh<BvhDisk> bvh(20);
    bvh.build(std::move(disks));

    // find order in z-direction
    std::vector<std::uint32_t> orderZ(pc.size());
    std::copy(boost::counting_iterator<std::uint32_t>(0)
              , boost::counting_iterator<std::uint32_t>(pc.size())
              , orderZ.begin());
    std::sort(orderZ.begin(), orderZ.end()
              , [&](std::uint32_t i1, std::uint32_t i2) { return pc[i1](2) > pc[i2](2); });

    // step 1 - determine normal orientation based on normals of already determined points,
    // assuming the topmost points (roofs) have z>0 orientation.
    LOG(info2) << "Estimating normal orientations";
    UTILITY_OMP(parallel for)
    for (std::uint32_t rankZ = 0; rankZ < orderZ.size(); ++rankZ) {
        const std::uint32_t i = orderZ[rankZ];
        int doFlip = 0;
        for (math::Point3 dir : ALL_DIRS) {
            dir = math::normalize(dir);
            // whether the ray is outward or inward with respect to current normal orientation
            const int outward = math::sgn(inner_prod(dir, normals[i]));
            const Ray ray(pc[i] + pointRadius * dir, dir);
            IntersectionInfo is;
            if (bvh.getFirstIntersection(ray, is)) {
                const std::uint32_t j = is.object->userData;
                // if this is an outward ray, flip the normal if the intersected point has
                // the same orientation as the ray (i.e. likely a backface);
                // for an inward ray, flip the normal if the intersected point is NOT a backface.
                doFlip += int(inner_prod(normals[j], dir) > 0.) * outward;
            } else {
                // no occlusion -> normal correct if this is an outward ray
                doFlip -= outward;
            }
        }

        if (doFlip > 0) {
            // more votes for flipping the normal
            normals[i] *= -1;
        }
    }

    // step 2 - flip normals which have opposite orientation than their neighbors
    LOG(info2) << "Flipping outliers";
    KdTree<math::Point3, 3> tree(pc.begin(), pc.end());
    std::vector<uint8_t> doFlip(pc.size(), false);
    std::vector<math::Points3::const_iterator> neighs;
    UTILITY_OMP(parallel for private(neighs))
    for (std::uint32_t i = 0; i < pc.size(); ++i) {
        neighs.clear();
        tree.range(pc[i], 2 * pointRadius, neighs);

        double count = 0.;
        for (auto& n : neighs) {
            const std::uint32_t j = std::uint32_t(n - pc.begin());
            count += inner_prod(normals[i], normals[j]);
        }
        if (count < 0.) {
            doFlip[i] = true;
        }
    }
    UTILITY_OMP(parallel for)
    for (std::uint32_t i = 0; i < pc.size(); ++i) {
        if (doFlip[i]) {
            normals[i] *= -1;
        }
    }
}

} // namespace geometry


#include <iostream>
#include <math.h>

#include <boost/bind/bind.hpp>
#include "boost/math/special_functions/pow.hpp"
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/tuple/tuple.hpp>

#include "Filtering.hpp"

using namespace boost::math;
using namespace boost::numeric::ublas;

#define POW(a,b) pow(a,b)

class AbstractRadarCoordinates {
public:

	RealMatrix ENU2AER(RealVector E, RealVector N, RealVector U) {
		RealMatrix AER(E.size(), 3);
		AER(0, 0) = fmod(atan2(N(0), E(0)), (2.0*constants::pi<double>()));  // azimuth
		AER(0, 1) = atan2(U(0), sqrt(pow<2>(E(0)) + pow<2>(N(0))));
		AER(0, 2) = sqrt(pow<2>(E(0)) + pow<2>(N(0)) + pow<2>(U(0)));
		return AER;
	};

	RealMatrix AER2ENU(RealVector A, RealVector E, RealVector R) {
		RealMatrix ENU(A.size(), 3);
		ENU(0, 0) = R(0) * cos(E(0)) * sin(A(0));
		ENU(0, 1) = R(0) * cos(E(0)) * cos(A(0));
		ENU(0, 2) = R(0) * sin(E(0));
		return ENU;
	};

protected:
	virtual double d1AzimuthdENU1(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d2AzimuthdENU2(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d3AzimuthdENU3(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d4AzimuthdENU4(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d5AzimuthdENU5(const RealVector E, const RealVector N, const RealVector U) = 0;

	virtual double d1ElevationdENU1(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d2ElevationdENU2(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d3ElevationdENU3(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d4ElevationdENU4(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d5ElevationdENU5(const RealVector E, const RealVector N, const RealVector U) = 0;

	virtual double d1RangedENU1(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d2RangedENU2(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d3RangedENU3(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d4RangedENU4(const RealVector E, const RealVector N, const RealVector U) = 0;
	virtual double d5RangedENU5(const RealVector E, const RealVector N, const RealVector U) = 0;

	virtual double d1EastdAER1(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d2EastdAER2(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d3EastdAER3(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d4EastdAER4(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d5EastdAER5(const RealVector A, const RealVector E, const RealVector R) = 0;

	virtual double d1NorthdAER1(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d2NorthdAER2(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d3NorthdAER3(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d4NorthdAER4(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d5NorthdAER5(const RealVector A, const RealVector E, const RealVector R) = 0;

	virtual double d1UpdAER1(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d2UpdAER2(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d3UpdAER3(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d4UpdAER4(const RealVector A, const RealVector E, const RealVector R) = 0;
	virtual double d5UpdAER5(const RealVector A, const RealVector E, const RealVector R) = 0;

};


#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>

#include <boost/make_shared.hpp>

#include "uxabiped/utils/Walking.h"
#include "uxabiped/utils/Kinematics.h"

namespace Bipedal
{

VirtualRobot::MathTools::ConvexHull2DPtr ComputeFootContact(const VirtualRobot::CollisionModelPtr& colModel)
{
    // let the feet collide with the floor and get the collision points
    VirtualRobot::MathTools::Plane plane =  VirtualRobot::MathTools::getFloorPlane();
    VirtualRobot::CollisionCheckerPtr colChecker = VirtualRobot::CollisionChecker::getGlobalCollisionChecker();
    std::vector< VirtualRobot::MathTools::ContactPoint > pointsFoot;
    std::vector< Eigen::Vector2f > points2D;

    // let the collision begin
    colChecker->getContacts(plane, colModel, pointsFoot, 5.0f);

    // project the points on the floor
    for (size_t u = 0; u < pointsFoot.size(); u++)
    {
        Eigen::Vector2f pt2d = VirtualRobot::MathTools::projectPointToPlane2D(pointsFoot[u].p, plane);
        points2D.push_back(pt2d);
    }

    // calculate the convex hulls and the appropriate centers
    VirtualRobot::MathTools::ConvexHull2DPtr hull = VirtualRobot::MathTools::createConvexHull2D(points2D);

    return hull;
}

Eigen::Vector2f CenterConvexHull(const VirtualRobot::MathTools::ConvexHull2DPtr& hull)
{
    Eigen::Vector2f center = VirtualRobot::MathTools::getConvexHullCenter(hull);

    // translate points of FootShape so, that center of convex hull is (0|0)
    for (unsigned i =  0; i < hull->vertices.size(); i++)
    {
        hull->vertices[i] -= center;
    }

    return center;
}

void TransformConvexHull(const VirtualRobot::MathTools::ConvexHull2DPtr& hull, const Eigen::Matrix3f& frame)
{
    // translate points of FootShape so, that center of convex hull is (0|0)
    for (unsigned i =  0; i < hull->vertices.size(); i++)
    {
        hull->vertices[i] = frame.block(0, 0, 2, 2) * hull->vertices[i] + frame.block(0, 2, 2, 1);
    }
}

Eigen::Matrix2f ComputeWalkingDirection(const Eigen::Vector2f& leftFootCenter, const Eigen::Vector2f& rightFootCenter)
{
    Eigen::Vector2f center = (leftFootCenter + rightFootCenter) * 0.5;
    Eigen::Vector2f centerToLeft = (leftFootCenter - center);
    centerToLeft.normalize();
    Eigen::Matrix2f rotNinety;
    rotNinety << 0, 1, -1, 0;
    Eigen::Vector2f walkingDirection = rotNinety * centerToLeft;

    // Note: The TCP coordinate system in Armar4 uses y as forward direction
    Eigen::Matrix2f pose;
    pose << -centerToLeft.x(), walkingDirection.x(), -centerToLeft.y(), walkingDirection.y();

    return pose;
}

Eigen::Vector2f computeHullContactPoint(const Eigen::Vector2f p, const VirtualRobot::MathTools::ConvexHull2DPtr& hull)
{
    double min = std::numeric_limits<double>::max();
    VirtualRobot::MathTools::Segment2D min_segment;
    for(const auto& segment : hull->segments)
    {
        double dist = VirtualRobot::MathTools::distPointSegment(hull->vertices[segment.id1],
                hull->vertices[segment.id2], p);
        if (dist < min)
        {
            min = dist;
            min_segment = segment;
        }
    }
    return VirtualRobot::MathTools::nearestPointOnSegment(hull->vertices[min_segment.id1], hull->vertices[min_segment.id2], p);
}

VirtualRobot::MathTools::ConvexHull2DPtr computeConvexHull(const VirtualRobot::RobotNodePtr& foot,
                                                           const VirtualRobot::RobotNodePtr& tcp)
{
    auto colModel = foot->getCollisionModel()->clone();
    Eigen::Matrix4f relPose = tcp->getGlobalPose().inverse() * colModel->getGlobalPose();
    colModel->setGlobalPose(relPose);
    auto hull = Bipedal::ComputeFootContact(colModel);
    Bipedal::CenterConvexHull(hull);
    return hull;
}

VirtualRobot::MathTools::ConvexHull2DPtr computeSupportPolygone(const Eigen::Matrix4f& leftFootPose,
                                                                const Eigen::Matrix4f& rightFootPose,
                                                                const VirtualRobot::MathTools::ConvexHull2DPtr& leftFootHull,
                                                                const VirtualRobot::MathTools::ConvexHull2DPtr& rightFootHull,
                                                                Bipedal::SupportPhase phase)
{
    if (phase == SUPPORT_LEFT)
        return boost::make_shared<VirtualRobot::MathTools::ConvexHull2D>(*leftFootHull);

    if (phase == SUPPORT_RIGHT)
        return boost::make_shared<VirtualRobot::MathTools::ConvexHull2D>(*rightFootHull);

    if (phase == SUPPORT_BOTH)
    {
        Eigen::Vector2f offset = rightFootPose.block(0, 3, 2, 1) - leftFootPose.block(0, 3, 2, 1);

        std::vector<Eigen::Vector2f> points;
        for (const auto& v : leftFootHull->vertices)
        {
            points.push_back(v - offset/2);
        }
        for (const auto& v : rightFootHull->vertices)
        {
            points.push_back(v + offset/2);
        }

        auto dualSupportHull = VirtualRobot::MathTools::createConvexHull2D(points);
        Bipedal::CenterConvexHull(dualSupportHull);

        return dualSupportHull;
    }

    return VirtualRobot::MathTools::ConvexHull2DPtr();
}

}


/* Copyright 2022 Zuru Tech HK Limited.
 *
 * Licensed under the Apache License, Version 2.0(the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <chrono>
#include <stdexcept>

#include <Eigen/CholmodSupport>
#include <Eigen/Dense>
#include <Eigen/KLUSupport>
#include <Eigen/Sparse>
#include <Eigen/UmfPackSupport>

#include <solvers/SparseSystem.hpp>
#include <solvers/SuiteSparseSolver.hpp>

namespace solvers {

Eigen::VectorXd SuiteSparseSolver::_cholmod_solve(const SparseSystem& system,
                                                  double& duration) const
{
    auto [A, b] = system.toEigenUpperCSR();
    Eigen::VectorXd result;
    std::chrono::time_point start = std::chrono::high_resolution_clock::now();
    switch (_method) {
        case SuiteSparseMethod::SimplicialLLT: {
            Eigen::CholmodSimplicialLLT<
                Eigen::SparseMatrix<double, Eigen::RowMajor>, Eigen::Upper>
                simplicial_llt;
            simplicial_llt.compute(A);
            result = simplicial_llt.solve(b);
            break;
        }
        case SuiteSparseMethod::SimplicialLDLT: {
            Eigen::CholmodSimplicialLDLT<
                Eigen::SparseMatrix<double, Eigen::RowMajor>, Eigen::Upper>
                simplicial_ldlt;
            simplicial_ldlt.compute(A);
            result = simplicial_ldlt.solve(b);
            break;
        }
        case SuiteSparseMethod::SupernodalLLT: {
            Eigen::CholmodSupernodalLLT<
                Eigen::SparseMatrix<double, Eigen::RowMajor>, Eigen::Upper>
                supernodal_llt;
            supernodal_llt.compute(A);
            result = supernodal_llt.solve(b);
            break;
        }
        default:
            throw std::logic_error("Invalid solving method");
            break;
    }
    std::chrono::time_point end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> time_difference = end - start;
    duration = time_difference.count();
    return result;
}

Eigen::VectorXd SuiteSparseSolver::_cholmod_gpu_solve(
    const SparseSystem& system,
    double& duration) const
{
    auto [A, b] = system.toEigenCSR();
    Eigen::SparseMatrix<double, Eigen::RowMajor, SuiteSparse_long> A_long(A);
    cholmod_sparse cholmod_A = viewAsCholmod(A_long);
    cholmod_A.stype = 1;
    cholmod_dense cholmod_b = viewAsCholmod(b);
    cholmod_dense* cholmod_x;
    auto* c = new cholmod_common();
    cholmod_factor* L;
    cholmod_l_start(c);
    c->useGPU = 1;
    switch (_method) {
        case SuiteSparseMethod::SimplicialLLT: {
            c->final_asis = 0;
            c->supernodal = CHOLMOD_SIMPLICIAL;
            c->final_ll = 1;
            break;
        }
        case SuiteSparseMethod::SimplicialLDLT: {
            c->final_asis = 1;
            c->supernodal = CHOLMOD_SIMPLICIAL;
            break;
        }
        case SuiteSparseMethod::SupernodalLLT: {
            c->final_asis = 1;
            c->supernodal = CHOLMOD_SUPERNODAL;
            break;
        }
        default:
            throw std::logic_error("Invalid solving method");
            break;
    }
    std::chrono::time_point start = std::chrono::high_resolution_clock::now();
    L = cholmod_l_analyze(&cholmod_A, c);
    cholmod_l_factorize(&cholmod_A, L, c);
    cholmod_x = cholmod_l_solve(CHOLMOD_A, L, &cholmod_b, c);
    std::chrono::time_point end = std::chrono::high_resolution_clock::now();
    Eigen::VectorXd result = Eigen::Map<Eigen::VectorXd>(
        static_cast<double*>(cholmod_x->x), static_cast<int64_t>(system.dim()));
    cholmod_l_free_factor(&L, c);
    cholmod_l_free_dense(&cholmod_x, c);
    cholmod_l_finish(c);
    std::chrono::duration<double> time_difference = end - start;
    duration = time_difference.count();
    return result;
}

Eigen::VectorXd SuiteSparseSolver::_lu_solve(const SparseSystem& system,
                                             double& duration) const
{
    auto [A, b] = system.toEigenCSC();
    Eigen::VectorXd result;

    std::chrono::time_point start = std::chrono::high_resolution_clock::now();
    if (_method == SuiteSparseMethod::LU) {
        Eigen::UmfPackLU<Eigen::SparseMatrix<double, Eigen::ColMajor>>
            umfpacklu;
        umfpacklu.compute(A);
        result = umfpacklu.solve(b);
    }
    else {
        Eigen::KLU<Eigen::SparseMatrix<double, Eigen::ColMajor>> klu;
        klu.compute(A);
        result = klu.solve(b);
    }
    std::chrono::time_point end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> time_difference = end - start;
    duration = time_difference.count();
    return result;
}

Eigen::VectorXd SuiteSparseSolver::solve(const SparseSystem& system,
                                         double& duration) const
{
    Eigen::VectorXd result;

    switch (_method) {
        case SuiteSparseMethod::SimplicialLLT:
        case SuiteSparseMethod::SimplicialLDLT:
        case SuiteSparseMethod::SupernodalLLT:
            if (_gpu) {
                result = _cholmod_gpu_solve(system, duration);
            }
            else {
                result = _cholmod_solve(system, duration);
            }
            break;
        case SuiteSparseMethod::LU:
        case SuiteSparseMethod::KLU:
            result = _lu_solve(system, duration);
            break;
        default:
            throw std::logic_error("Invalid solving method");
            break;
    }
    return result;
}

}    // namespace solvers


/**********************************************************************
align.cpp - Align two molecules or vectors of vector3

Copyright (C) 2010 by Noel M. O'Boyle

This file is part of the Open Babel project.
For more information, see <http://openbabel.org/>

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation version 2 of the License.

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

#include <openbabel/babelconfig.h>

#include <vector>
#include <climits> // UINT_MAX

#include <openbabel/math/align.h>
#include <openbabel/graphsym.h>
#include <openbabel/math/vector3.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <Eigen/LU>

using namespace std;

namespace OpenBabel
{
  OBAlign::OBAlign(bool includeH, bool symmetry) : _method(OBAlign::Kabsch)
  {
    _ready = false;
    _symmetry = symmetry;
    _includeH = includeH;
    _prefmol = 0;
  }

  OBAlign::OBAlign(const vector<vector3> &ref, const vector<vector3> &target) : _method(OBAlign::Kabsch)
  {
    SetRef(ref);
    SetTarget(target);
    _symmetry = false;
    _prefmol = 0;
  }

  OBAlign::OBAlign(const OBMol &refmol, const OBMol &targetmol, bool includeH, bool symmetry) : _method(OBAlign::Kabsch)
  {
    _symmetry = symmetry;
    _includeH = includeH;
    SetRefMol(refmol);
    SetTargetMol(targetmol);
  }

  void OBAlign::VectorsToMatrix(const vector<vector3> *pcoords, Eigen::MatrixXd &coords) {

    vector<vector3>::size_type N = pcoords->size();
    coords.resize(3, N);

    // Create a 3xN matrix of the coords
    vector<vector3>::const_iterator it;
    vector<vector3>::size_type colm;
    for (colm=0,it=pcoords->begin();colm<N;++colm,++it)
      coords.col(colm) = Eigen::Vector3d( it->AsArray() );
  }

  Eigen::Vector3d OBAlign::MoveToOrigin(Eigen::MatrixXd &coords) {

    vector<vector3>::size_type N = coords.cols();

    // Find the centroid
    Eigen::Vector3d centroid;
    centroid = coords.rowwise().sum() / N;

    // Subtract the centroids
    for (vector<vector3>::size_type i=0; i<N; ++i)
      coords.col(i) -= centroid;
    return centroid;
  }

  void OBAlign::SetRef(const vector<vector3> &ref) {
    _pref = &ref;
    VectorsToMatrix(_pref, _mref);
    _ref_centr = MoveToOrigin(_mref);

    _ready = false;
  }

  void OBAlign::SetTarget(const vector<vector3> &target) {
    _ptarget = ⌖
    VectorsToMatrix(_ptarget, _mtarget);
    _target_centr = MoveToOrigin(_mtarget);

    _ready = false;
  }

  void OBAlign::SetRefMol(const OBMol &refmol) {
    _prefmol = &refmol;

    // Set up the BitVec for the hydrogens and store the refmol coords
    _frag_atoms.Clear();
    _frag_atoms.Resize(refmol.NumAtoms() + 1);
    _refmol_coords.resize(0);
    OBAtom* atom;
    int delta = 1;
    _newidx.resize(0);

    for (unsigned int i=1; i<=refmol.NumAtoms(); ++i) {
      atom = refmol.GetAtom(i);
      if (_includeH || !atom->IsHydrogen()) {
        _frag_atoms.SetBitOn(i);
        _newidx.push_back(i - delta);
        _refmol_coords.push_back(atom->GetVector());
      }
      else {
        delta++;
        _newidx.push_back(UINT_MAX);
      }
    }
    SetRef(_refmol_coords);

    if (_symmetry) {
      FindAutomorphisms((OBMol*)&refmol, _aut, _frag_atoms);
    }
  }

  void OBAlign::SetTargetMol(const OBMol &targetmol) {
    _ptargetmol = &targetmol;
    _targetmol_coords.resize(0);
    OBAtom const *atom;
    for (unsigned int i=1; i<=targetmol.NumAtoms(); ++i) {
      atom = targetmol.GetAtom(i);
      if (_includeH || !atom->IsHydrogen())
        _targetmol_coords.push_back(atom->GetVector());
    }
    SetTarget(_targetmol_coords);
  }

  void OBAlign::SetMethod(OBAlign::AlignMethod method) {
    _method = method;
  }

/* Evaluates the Newton-Raphson correction for the Horn quartic.
   only 11 FLOPs */
  static double eval_horn_NR_corrxn(const vector<double> &c, const double x)
  {
    double x2 = x*x;
    double b = (x2 + c[2])*x;
    double a = b + c[1];

    return((a*x + c[0])/(2.0*x2*x + b + a));
  }

  /* Newton-Raphson root finding */
  static double QCProot(const vector<double> &coeff, double guess, const double delta)
  {
    int             i;
    double          oldg;
    double initialg = guess;

    for (i = 0; i < 50; ++i)
    {
        oldg = guess;
        /* guess -= (eval_horn_quart(coeff, guess) / eval_horn_quart_deriv(coeff, guess)); */
        guess -= eval_horn_NR_corrxn(coeff, guess);

        if (fabs(guess - oldg) < fabs(delta*guess))
            return(guess);
    }

    return initialg + 1.0; // Failed to converge!
  }

  vector<double> CalcQuarticCoeffs(const Eigen::Matrix3d &M)
  {
    vector<double> coeff(4);

    double          Sxx, Sxy, Sxz, Syx, Syy, Syz, Szx, Szy, Szz;
    double          Szz2, Syy2, Sxx2, Sxy2, Syz2, Sxz2, Syx2, Szy2, Szx2,
                    SyzSzymSyySzz2, Sxx2Syy2Szz2Syz2Szy2, Sxy2Sxz2Syx2Szx2,
                    SxzpSzx, SyzpSzy, SxypSyx, SyzmSzy,
                    SxzmSzx, SxymSyx, SxxpSyy, SxxmSyy;

#ifdef HAVE_EIGEN3
    Eigen::MatrixXd M_sqr = M.array().square();
#else
    Eigen::MatrixXd M_sqr = M.cwise().square();
#endif

    Sxx = M(0, 0);
    Sxy = M(1, 0);
    Sxz = M(2, 0);
    Syx = M(0, 1);
    Syy = M(1, 1);
    Syz = M(2, 1);
    Szx = M(0, 2);
    Szy = M(1, 2);
    Szz = M(2, 2);

    Sxx2 = Sxx * Sxx;
    Syy2 = Syy * Syy;
    Szz2 = Szz * Szz;

    Sxy2 = Sxy * Sxy;
    Syz2 = Syz * Syz;
    Sxz2 = Sxz * Sxz;

    Syx2 = Syx * Syx;
    Szy2 = Szy * Szy;
    Szx2 = Szx * Szx;

    SyzSzymSyySzz2 = 2.0*(Syz*Szy - Syy*Szz);
    Sxx2Syy2Szz2Syz2Szy2 = Syy2 + Szz2 - Sxx2 + Syz2 + Szy2;

    /* coeff[4] = 1.0; */
    /* coeff[3] = 0.0; */
    // coeff[2] = -2.0 * (Sxx2 + Syy2 + Szz2 + Sxy2 + Syx2 + Sxz2 + Szx2 + Syz2 + Szy2);
    coeff[2] = -2.0 * M_sqr.sum();
    coeff[1] = 8.0 * (Sxx*Syz*Szy + Syy*Szx*Sxz + Szz*Sxy*Syx - Sxx*Syy*Szz - Syz*Szx*Sxy - Szy*Syx*Sxz);

    SxzpSzx = Sxz+Szx;
    SyzpSzy = Syz+Szy;
    SxypSyx = Sxy+Syx;
    SyzmSzy = Syz-Szy;
    SxzmSzx = Sxz-Szx;
    SxymSyx = Sxy-Syx;
    SxxpSyy = Sxx+Syy;
    SxxmSyy = Sxx-Syy;
    Sxy2Sxz2Syx2Szx2 = Sxy2 + Sxz2 - Syx2 - Szx2;

    coeff[0] = Sxy2Sxz2Syx2Szx2 * Sxy2Sxz2Syx2Szx2
             + (Sxx2Syy2Szz2Syz2Szy2 + SyzSzymSyySzz2) * (Sxx2Syy2Szz2Syz2Szy2 - SyzSzymSyySzz2)
             + (-(SxzpSzx)*(SyzmSzy)+(SxymSyx)*(SxxmSyy-Szz)) * (-(SxzmSzx)*(SyzpSzy)+(SxymSyx)*(SxxmSyy+Szz))
             + (-(SxzpSzx)*(SyzpSzy)-(SxypSyx)*(SxxpSyy-Szz)) * (-(SxzmSzx)*(SyzmSzy)-(SxypSyx)*(SxxpSyy+Szz))
             + (+(SxypSyx)*(SyzpSzy)+(SxzpSzx)*(SxxmSyy+Szz)) * (-(SxymSyx)*(SyzmSzy)+(SxzpSzx)*(SxxpSyy+Szz))
             + (+(SxypSyx)*(SyzmSzy)+(SxzmSzx)*(SxxmSyy-Szz)) * (-(SxymSyx)*(SyzpSzy)+(SxzmSzx)*(SxxpSyy-Szz));

    return coeff;
  }

  void OBAlign::TheobaldAlign(const Eigen::MatrixXd &mtarget)
  {
    // M = B(t) times A (where A, B are N x 3 matrices)
    Eigen::Matrix3d M = mtarget * _mref.transpose();

    // Maximum value for lambda is (Ga + Gb) / 2
    double innerprod = mtarget.squaredNorm() + _mref.squaredNorm();

    vector<double> coeffs = CalcQuarticCoeffs(M);
    double lambdamax = QCProot(coeffs, 0.5 * innerprod, 1e-6);
    if (lambdamax > (0.5 * innerprod))
      _fail = true;
    else {
      double sqrdev = innerprod - (2.0 * lambdamax);
      _rmsd = sqrt(sqrdev / mtarget.cols());
    }
  }

  void OBAlign::SimpleAlign(const Eigen::MatrixXd &mtarget)
  {
    // Covariance matrix C = X times Y(t)
    Eigen::Matrix3d C = _mref * mtarget.transpose();

    // Singular Value Decomposition of C into USV(t)
#ifdef HAVE_EIGEN3
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(C, Eigen::ComputeFullU | Eigen::ComputeFullV);
#else
    Eigen::SVD<Eigen::Matrix3d> svd(C);
#endif

    // Prepare matrix T
    double sign = (C.determinant() > 0) ? 1. : -1.; // Sign of determinant
    Eigen::Matrix3d T = Eigen::Matrix3d::Identity();
    T(2,2) = sign;

    // Optimal rotation matrix, U, is V T U(t)
    _rotMatrix = svd.matrixV() * T * svd.matrixU().transpose();

    // Rotate target using rotMatrix
    _result = _rotMatrix.transpose() * mtarget;

    Eigen::MatrixXd deviation = _result - _mref;
#ifdef HAVE_EIGEN3
    Eigen::MatrixXd sqr = deviation.array().square();
#else
    Eigen::MatrixXd sqr = deviation.cwise().square();
#endif
    double sum = sqr.sum();
    _rmsd = sqrt( sum / sqr.cols() );

  }

  bool OBAlign::Align()
  {
    vector<vector3>::size_type N = _ptarget->size();

    if (_pref->size() != N) {
      obErrorLog.ThrowError(__FUNCTION__, "Cannot align the reference and target as they are of different size" , obError);
      return false;
    }

    if (!_symmetry || _aut.size() == 1) {
      if (_method == OBAlign::Kabsch)
        SimpleAlign(_mtarget);
      else
        TheobaldAlign(_mtarget);
    }
    else {  // Iterate over the automorphisms

      // ...for storing the results from the lowest rmsd to date
      double min_rmsd = DBL_MAX;
      Eigen::MatrixXd result, rotMatrix;

      // Try all of the symmetry-allowed permutations
      OBIsomorphismMapper::Mappings::const_iterator cit;
      Eigen::MatrixXd mtarget(_mtarget.rows(), _mtarget.cols());

      for (unsigned int k = 0; k < _aut.size(); ++k) {
        // Rearrange columns of _mtarget for this permutation
        unsigned int i=0;
        for (unsigned int j=1; j<=_prefmol->NumAtoms(); ++j) {
          if (_frag_atoms.BitIsSet(j)) {
            for (std::size_t l = 0; l < _aut[k].size(); ++l)
              if (_aut[k][l].first == j - 1) {
                mtarget.col(i) = _mtarget.col(_newidx[_aut[k][l].second]);
                break;
              }
            i++;
          }
        }
        if (_method == OBAlign::Kabsch)
          SimpleAlign(mtarget);
        else
          TheobaldAlign(mtarget);
        if (_rmsd < min_rmsd) {
          min_rmsd = _rmsd;
          result = _result;
          rotMatrix = _rotMatrix;
        }
      }

      // Restore the best answer from memory
      _rmsd = min_rmsd;
      _result = result;
      _rotMatrix = rotMatrix;
    }

    _ready = true;
    return true;
  }

  matrix3x3 OBAlign::GetRotMatrix()
  {
    if (!_ready) {
      obErrorLog.ThrowError(__FUNCTION__, "Rotation matrix not available until you call Align()" , obError);
      return matrix3x3();
    }

    // Convert Eigen::Matrix to matrix3x3
    double rot[3][3];
    for (int row=0; row<3; ++row)
       for (int col=0; col<3; ++col)
         rot[col][row] = _rotMatrix(row, col); // Return in form suitable for use in expressions like "result *= rotMatrix";
    matrix3x3 rotmat = matrix3x3(rot);

    return rotmat;
  }

  vector<vector3> OBAlign::GetAlignment() {
    vector<vector3> aligned_coords;
    if (!_ready) {
      obErrorLog.ThrowError(__FUNCTION__, "Alignment not available until you call Align()" , obError);
      return aligned_coords;
    }

    if (!_prefmol || _includeH) {
      // Add back the centroid of the reference and convert to vv3
      Eigen::Vector3d tmp;
      aligned_coords.reserve(_result.cols());
      for (int i=0; i<_result.cols(); ++i) {
        tmp = _result.col(i) + _ref_centr;
        aligned_coords.push_back(vector3(tmp(0), tmp(1), tmp(2)));
      }
    }
    else { // Need to deal with the case where hydrogens were excluded
      vector<vector3> target_coords;
      for (unsigned int i=1; i<=_ptargetmol->NumAtoms(); ++i)
        target_coords.push_back(_ptargetmol->GetAtom(i)->GetVector());
      Eigen::MatrixXd mtarget;
      VectorsToMatrix(&target_coords, mtarget);

      // Subtract the centroid of the non-H atoms
      for (vector<vector3>::size_type i=0; i<mtarget.cols(); ++i)
        mtarget.col(i) -= _target_centr;

      // Rotate
      Eigen::MatrixXd result = mtarget.transpose() * _rotMatrix;
      result.transposeInPlace();

      // Add back the centroid of the reference and convert to vv3
      Eigen::Vector3d tmp;
      aligned_coords.reserve(_result.cols());
      for (int i=0; i<result.cols(); ++i) {
        tmp = result.col(i) + _ref_centr;
        aligned_coords.push_back(vector3(tmp(0), tmp(1), tmp(2)));
      }
    }

    return aligned_coords;
  }

  bool OBAlign::UpdateCoords(OBMol* target) {
    if (!_ready) {
      obErrorLog.ThrowError(__FUNCTION__, "Alignment not available until you call Align()" , obError);
      return false;
    }

    vector<vector3> newcoords = GetAlignment();
    if (newcoords.size() != target->NumAtoms()) {
      obErrorLog.ThrowError(__FUNCTION__, "Cannot update the target molecule with the alignment coordinates as they are of different size" , obError);
      return false;
    }

    int i = 0;
    FOR_ATOMS_OF_MOL(a, *target) {
      a->SetVector(newcoords.at(i));
      i++;
    }

    return true;
  }

  double OBAlign::GetRMSD() {
    if (!_ready) {
      obErrorLog.ThrowError(__FUNCTION__, "RMSD not available until you call Align()" , obError);
      return (double) NULL;
    }

    return _rmsd;
  }

} // namespace OpenBabel

//! \file align.cpp
//! \brief Handle 3D coordinates.


//  Copyright (c) 2015 John Maddock
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
#ifndef BOOST_MATH_ELLINT_RG_HPP
#define BOOST_MATH_ELLINT_RG_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/ellint_rd.hpp>
#include <boost/math/special_functions/ellint_rf.hpp>
#include <boost/math/special_functions/pow.hpp>

namespace boost { namespace math { namespace detail{

   template <typename T, typename Policy>
   T ellint_rg_imp(T x, T y, T z, const Policy& pol)
   {
      BOOST_MATH_STD_USING
      static const char* function = "boost::math::ellint_rf<%1%>(%1%,%1%,%1%)";

      if(x < 0 || y < 0 || z < 0)
      {
         return policies::raise_domain_error<T>(function,
            "domain error, all arguments must be non-negative, "
            "only sensible result is %1%.",
            std::numeric_limits<T>::quiet_NaN(), pol);
      }
      //
      // Function is symmetric in x, y and z, but we require
      // (x - z)(y - z) >= 0 to avoid cancellation error in the result
      // which implies (for example) x >= z >= y
      //
      using std::swap;
      if(x < y)
         swap(x, y);
      if(x < z)
         swap(x, z);
      if(y > z)
         swap(y, z);

      BOOST_ASSERT(x >= z);
      BOOST_ASSERT(z >= y);
      //
      // Special cases from http://dlmf.nist.gov/19.20#ii
      //
      if(x == z)
      {
         if(y == z)
         {
            // x = y = z
            // This also works for x = y = z = 0 presumably.
            return sqrt(x);
         }
         else if(y == 0)
         {
            // x = y, z = 0
            return constants::pi<T>() * sqrt(x) / 4;
         }
         else
         {
            // x = z, y != 0
            swap(x, y);
            return (x == 0) ? T(sqrt(z) / 2) : T((z * ellint_rc_imp(x, z, pol) + sqrt(x)) / 2);
         }
      }
      else if(y == z)
      {
         if(x == 0)
            return constants::pi<T>() * sqrt(y) / 4;
         else
            return (y == 0) ? T(sqrt(x) / 2) : T((y * ellint_rc_imp(x, y, pol) + sqrt(x)) / 2);
      }
      else if(y == 0)
      {
         swap(y, z);
         //
         // Special handling for common case, from
         // Numerical Computation of Real or Complex Elliptic Integrals, eq.46
         //
         T xn = sqrt(x);
         T yn = sqrt(y);
         T x0 = xn;
         T y0 = yn;
         T sum = 0;
         T sum_pow = 0.25f;

         while(fabs(xn - yn) >= 2.7 * tools::root_epsilon<T>() * fabs(xn))
         {
            T t = sqrt(xn * yn);
            xn = (xn + yn) / 2;
            yn = t;
            sum_pow *= 2;
            sum += sum_pow * boost::math::pow<2>(xn - yn);
         }
         T RF = constants::pi<T>() / (xn + yn);
         return ((boost::math::pow<2>((x0 + y0) / 2) - sum) * RF) / 2;
      }
      return (z * ellint_rf_imp(x, y, z, pol)
         - (x - z) * (y - z) * ellint_rd_imp(x, y, z, pol) / 3
         + sqrt(x * y / z)) / 2;
   }

} // namespace detail

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type
   ellint_rg(T1 x, T2 y, T3 z, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_rg_imp(
         static_cast<value_type>(x),
         static_cast<value_type>(y),
         static_cast<value_type>(z), pol), "boost::math::ellint_rf<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   ellint_rg(T1 x, T2 y, T3 z)
{
   return ellint_rg(x, y, z, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_RG_HPP


// Copyright (c) 2020 Chris Richardson & Matthew Scroggs
// FEniCS Project
// SPDX-License-Identifier:    MIT

#include "lagrange.h"
#include "dof-permutations.h"
#include "lattice.h"
#include "libtab.h"
#include "polyset.h"
#include <Eigen/Dense>
#include <iostream>
#include <numeric>

using namespace libtab;

//----------------------------------------------------------------------------
FiniteElement libtab::create_lagrange(cell::type celltype, int degree,
                                      const std::string& name)
{
  if (celltype == cell::type::point)
    throw std::runtime_error("Invalid celltype");

  const int ndofs = polyset::dim(celltype, degree);

  const std::vector<std::vector<std::vector<int>>> topology
      = cell::topology(celltype);
  std::vector<std::vector<int>> entity_dofs(topology.size());

  // Create points at nodes, ordered by topology (vertices first)
  Eigen::ArrayXXd pt(ndofs, topology.size() - 1);
  if (degree == 0)
  {
    pt = lattice::create(celltype, 0, lattice::type::equispaced, true);
    for (std::size_t i = 0; i < entity_dofs.size(); ++i)
      entity_dofs[i].resize(topology[i].size(), 0);
    entity_dofs[topology.size() - 1][0] = 1;
  }
  else
  {
    int c = 0;
    for (std::size_t dim = 0; dim < topology.size(); ++dim)
    {
      for (std::size_t i = 0; i < topology[dim].size(); ++i)
      {
        const Eigen::ArrayXXd entity_geom
            = cell::sub_entity_geometry(celltype, dim, i);

        if (dim == 0)
        {
          pt.row(c++) = entity_geom.row(0);
          entity_dofs[0].push_back(1);
        }
        else if (dim == topology.size() - 1)
        {
          const Eigen::ArrayXXd lattice = lattice::create(
              celltype, degree, lattice::type::equispaced, false);
          for (int j = 0; j < lattice.rows(); ++j)
            pt.row(c++) = lattice.row(j);
          entity_dofs[dim].push_back(lattice.rows());
        }
        else
        {
          cell::type ct = cell::sub_entity_type(celltype, dim, i);
          const Eigen::ArrayXXd lattice
              = lattice::create(ct, degree, lattice::type::equispaced, false);
          entity_dofs[dim].push_back(lattice.rows());
          for (int j = 0; j < lattice.rows(); ++j)
          {
            pt.row(c) = entity_geom.row(0);
            for (int k = 0; k < lattice.cols(); ++k)
            {
              pt.row(c) += (entity_geom.row(k + 1) - entity_geom.row(0))
                           * lattice(j, k);
            }
            ++c;
          }
        }
      }
    }
  }

  int perm_count = 0;
  for (std::size_t i = 1; i < topology.size() - 1; ++i)
    perm_count += topology[i].size() * i;

  std::vector<Eigen::MatrixXd> base_permutations(
      perm_count, Eigen::MatrixXd::Identity(ndofs, ndofs));
  if (celltype == cell::type::triangle)
  {
    Eigen::ArrayXi edge_ref = dofperms::interval_reflection(degree - 1);
    for (int edge = 0; edge < 3; ++edge)
    {
      const int start = 3 + edge_ref.size() * edge;
      for (int i = 0; i < edge_ref.size(); ++i)
      {
        base_permutations[edge](start + i, start + i) = 0;
        base_permutations[edge](start + i, start + edge_ref[i]) = 1;
      }
    }
  }
  else if (celltype == cell::type::tetrahedron)
  {
    Eigen::ArrayXi edge_ref = dofperms::interval_reflection(degree - 1);
    for (int edge = 0; edge < 6; ++edge)
    {
      const int start = 4 + edge_ref.size() * edge;
      for (int i = 0; i < edge_ref.size(); ++i)
      {
        base_permutations[edge](start + i, start + i) = 0;
        base_permutations[edge](start + i, start + edge_ref[i]) = 1;
      }
    }
    Eigen::ArrayXi face_ref = dofperms::triangle_reflection(degree - 2);
    Eigen::ArrayXi face_rot = dofperms::triangle_rotation(degree - 2);
    for (int face = 0; face < 4; ++face)
    {
      const int start = 4 + edge_ref.size() * 6 + face_ref.size() * face;
      for (int i = 0; i < face_rot.size(); ++i)
      {
        base_permutations[6 + 2 * face](start + i, start + i) = 0;
        base_permutations[6 + 2 * face](start + i, start + face_rot[i]) = 1;
        base_permutations[6 + 2 * face + 1](start + i, start + i) = 0;
        base_permutations[6 + 2 * face + 1](start + i, start + face_ref[i]) = 1;
      }
    }
  }

  // Point evaluation of basis
  Eigen::MatrixXd dualmat = polyset::tabulate(celltype, degree, 0, pt)[0];
  Eigen::MatrixXd coeffs = compute_expansion_coefficients(
      Eigen::MatrixXd::Identity(ndofs, ndofs), dualmat);

  return FiniteElement(name, celltype, degree, {1}, coeffs, entity_dofs,
                       base_permutations);
}
//-----------------------------------------------------------------------------
FiniteElement libtab::create_dlagrange(cell::type celltype, int degree,
                                       const std::string& name)
{
  if (celltype != cell::type::interval and celltype != cell::type::triangle
      and celltype != cell::type::tetrahedron)
    throw std::runtime_error("Invalid celltype");

  // Only tabulate for scalar. Vector spaces can easily be built from
  // the scalar space.

  const int ndofs = polyset::dim(celltype, degree);

  std::vector<std::vector<std::vector<int>>> topology
      = cell::topology(celltype);
  std::vector<std::vector<int>> entity_dofs(topology.size());
  for (std::size_t i = 0; i < topology.size(); ++i)
    entity_dofs[i].resize(topology[i].size(), 0);
  entity_dofs[topology.size() - 1][0] = ndofs;

  Eigen::ArrayXXd geometry = cell::geometry(celltype);
  const Eigen::ArrayXXd lattice
      = lattice::create(celltype, degree, lattice::type::equispaced, true);

  // Create points at nodes, ordered by topology (vertices first)
  Eigen::ArrayXXd pt(ndofs, topology.size() - 1);
  for (int j = 0; j < lattice.rows(); ++j)
  {
    pt.row(j) = geometry.row(0);
    for (int k = 0; k < geometry.rows() - 1; ++k)
      pt.row(j) += (geometry.row(k + 1) - geometry.row(0)) * lattice(j, k);
  }

  // Point evaluation of basis
  Eigen::MatrixXd dualmat = polyset::tabulate(celltype, degree, 0, pt)[0];

  Eigen::MatrixXd coeffs = compute_expansion_coefficients(
      Eigen::MatrixXd::Identity(ndofs, ndofs), dualmat);

  int perm_count = 0;
  for (std::size_t i = 1; i < topology.size() - 1; ++i)
    perm_count += topology[i].size() * i;

  std::vector<Eigen::MatrixXd> base_permutations(
      perm_count, Eigen::MatrixXd::Identity(ndofs, ndofs));

  return FiniteElement(name, celltype, degree, {1}, coeffs, entity_dofs,
                       base_permutations);
}
//-----------------------------------------------------------------------------


/**
 * \file topological_search.hpp
 *
 * This library contains two simple nearest-neighbor search algorithms implemented
 * as functor templates. This library contains, in fact, three algorithms.
 *
 * First, a simple min_dist_linear_search algorithm is provided which is similar to std::min_element
 * except that it stores and compares the best distance value associated to the best iterator
 * to the current one (it is a simple linear search that avoid recomputation of the distance
 * at every iteration, which would be required if std::min_element was used instead).
 *
 * Second, a linear_neighbor_search algorithm is provided which simply does an exhaustive linear
 * search through all the vertices of a graph to find the nearest one to a given point, in a given
 * topology (as of topologies in the Boost Graph Library). This algorithms simply wraps the
 * min_dist_linear_search with the required distance and comparison function.
 *
 * Third, a best_only_neighbor_search algorithm is provided which is an approximation to an
 * exhaustive linear search by picking a number of random vertices from the graph and performing
 * a best only search down the graph to find the "nearest-neighbor". This is, of course, not going
 * to find the nearest-neighbor, but can significantly cut down on query time if finding the
 * nearest neighbor is not a strict requirement in the algorithm.
 *
 * \author Sven Mikael Persson <mikael.s.persson@gmail.com>
 * \date February 2011
 */

/*
 *    Copyright 2011 Sven Mikael Persson
 *
 *    THIS SOFTWARE IS DISTRIBUTED UNDER THE TERMS OF THE GNU GENERAL PUBLIC LICENSE v3 (GPLv3).
 *
 *    This file is part of ReaK.
 *
 *    ReaK is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 3 of the License, or
 *    (at your option) any later version.
 *
 *    ReaK is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with ReaK (as LICENSE in the root folder).
 *    If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef REAK_TOPOLOGICAL_SEARCH_HPP
#define REAK_TOPOLOGICAL_SEARCH_HPP

#include <boost/bind.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/topology.hpp>
#include <boost/graph/properties.hpp>

#include <vector>
#include <algorithm>

  /**
   * This function template is similar to std::min_element but can be used when the comparison
   * involves computing a derived quantity (a.k.a. distance). This algorithm will search for the
   * the element in the range [first,last) which has the "smallest" distance (of course, both the
   * distance metric and comparison can be overriden to perform something other than the canonical
   * Euclidean distance and less-than comparison, which would yield the element with minimum distance).
   * \tparam DistanceValue The value-type for the distance measures.
   * \tparam ForwardIterator The forward-iterator type.
   * \tparam GetDistanceFunction The functor type to compute the distance measure.
   * \tparam CompareFunction The functor type that can compare two distance measures (strict weak-ordering).
   * \param first Start of the range in which to search.
   * \param last One element past the last element in the range in which to search.
   * \param distance A callable object that returns a DistanceValue for a given element from the ForwardIterator dereferencing.
   * \param compare A callable object that returns true if the first element is the preferred one (less-than) of the two.
   * \param inf A DistanceValue which represents infinity (i.e. the very worst value with which to initialize the search).
   * \return The iterator to the best element in the range (best is defined as the one which would compare favorably to all the elements in the range with respect to the distance metric).
   */
  template <typename DistanceValue,
	    typename ForwardIterator,
            typename GetDistanceFunction,
	    typename CompareFunction>
  inline ForwardIterator min_dist_linear_search(ForwardIterator first,
						ForwardIterator last,
						GetDistanceFunction distance,
						CompareFunction compare,
						DistanceValue inf = std::numeric_limits<DistanceValue>::infinity()) {
    if(first == last) return last;
    DistanceValue d_best = inf;
    ForwardIterator result = last;
    for(; first != last; ++first) {
      DistanceValue d = distance(*first);
      if(compare(d, d_best)) {
	d_best = d;
	result = first;
      };
    };
    return result;
  };

  /**
   * This function template is a specialization of min_dist_linear_search for the default comparison
   * function which is the less-than operator.
   * \tparam DistanceValue The value-type for the distance measures.
   * \tparam ForwardIterator The forward-iterator type.
   * \tparam GetDistanceFunction The functor type to compute the distance measure.
   * \param first Start of the range in which to search.
   * \param last One element past the last element in the range in which to search.
   * \param distance A callable object that returns a DistanceValue for a given element from the ForwardIterator dereferencing.
   * \param inf A DistanceValue which represents infinity (i.e. the very worst value with which to initialize the search).
   * \return The iterator to the best element in the range (best is defined as the one which would compare favorably to all the elements in the range with respect to the distance metric).
   */
  template <typename DistanceValue, typename ForwardIterator, typename GetDistanceFunction>
  inline ForwardIterator min_dist_linear_search(ForwardIterator first,
						ForwardIterator last,
						GetDistanceFunction distance,
						DistanceValue inf = std::numeric_limits<DistanceValue>::infinity()) {
    return min_dist_linear_search(first,last,distance,std::less<DistanceValue>(),inf);
  };

  /**
   * This function template is similar to std::min_element but can be used when the comparison
   * involves computing a derived quantity (a.k.a. distance). This algorithm will search for the
   * the elements in the range [first,last) with the "smallest" distances (of course, both the
   * distance metric and comparison can be overriden to perform something other than the canonical
   * Euclidean distance and less-than comparison, which would yield the element with minimum distance).
   * This function will fill the output container with a number of nearest-neighbors.
   * \tparam DistanceValue The value-type for the distance measures.
   * \tparam ForwardIterator The forward-iterator type.
   * \tparam OutputContainer The container type which can contain the list of nearest-neighbors (STL like container, with iterators, insert, size, and pop_back).
   * \tparam GetDistanceFunction The functor type to compute the distance measure.
   * \tparam CompareFunction The functor type that can compare two distance measures (strict weak-ordering).
   * \param first Start of the range in which to search.
   * \param last One element past the last element in the range in which to search.
   * \param output The container that will have the sorted list of elements with the smallest distance.
   * \param distance A callable object that returns a DistanceValue for a given element from the ForwardIterator dereferencing.
   * \param compare A callable object that returns true if the first element is the preferred one (less-than) of the two.
   * \param max_neighbors The maximum number of elements of smallest distance to output in the sorted list.
   * \param radius The maximum distance value for which an element qualifies to be part of the output list.
   */
  template <typename DistanceValue,
	    typename ForwardIterator,
	    typename OutputContainer,
            typename GetDistanceFunction,
	    typename CompareFunction>
  inline void min_dist_linear_search(ForwardIterator first,
				     ForwardIterator last,
				     OutputContainer& output,
				     GetDistanceFunction distance,
				     CompareFunction compare,
				     unsigned int max_neighbors = 1,
				     DistanceValue radius = std::numeric_limits<DistanceValue>::infinity()) {
    output.clear();
    if(first == last) return;
    std::vector<DistanceValue> output_dist;
    for(; first != last; ++first) {
      DistanceValue d = distance(*first);
      if(!compare(d, radius))
	continue;
      typename std::vector<DistanceValue>::iterator it_lo = std::lower_bound(output_dist.begin(),output_dist.end(),d,compare);
      if((it_lo != output_dist.end()) || (output_dist.size() < max_neighbors)) {
	output_dist.insert(it_lo, d);
	typename OutputContainer::iterator itv = output.begin();
	for(typename std::vector<DistanceValue>::iterator it = output_dist.begin(); (itv != output.end()) && (it != it_lo); ++itv,++it) ;
	output.insert(itv, *first);
	if(output.size() > max_neighbors) {
	  output.pop_back();
	  output_dist.pop_back();
	};
      };
    };
  };

  /**
   * This function template is similar to std::min_element but can be used when the comparison
   * involves computing a derived quantity (a.k.a. distance). This algorithm will search for the
   * the element in the range [first,last) which has the "smallest" distance (of course, both the
   * distance metric and comparison can be overriden to perform something other than the canonical
   * Euclidean distance and less-than comparison, which would yield the element with minimum distance).
   * \tparam DistanceValue The value-type for the distance measures.
   * \tparam ForwardIterator The forward-iterator type.
   * \tparam OutputContainer The container type which can contain the list of nearest-neighbors (STL like container, with iterators, insert, size, and pop_back).
   * \tparam GetDistanceFunction The functor type to compute the distance measure.
   * \param first Start of the range in which to search.
   * \param last One element past the last element in the range in which to search.
   * \param output The container that will have the sorted list of elements with the smallest distance.
   * \param distance A callable object that returns a DistanceValue for a given element from the ForwardIterator dereferencing.
   * \param max_neighbors The maximum number of elements of smallest distance to output in the sorted list.
   * \param radius The maximum distance value for which an element qualifies to be part of the output list.
   */
  template <typename DistanceValue,
	    typename ForwardIterator,
	    typename OutputContainer,
            typename GetDistanceFunction>
  inline void min_dist_linear_search(ForwardIterator first,
				     ForwardIterator last,
				     OutputContainer& output,
				     GetDistanceFunction distance,
				     unsigned int max_neighbors = 1,
				     DistanceValue radius = std::numeric_limits<DistanceValue>::infinity()) {
    min_dist_linear_search(first,last,output,distance,std::less<DistanceValue>(),max_neighbors,radius);
  };

  /**
   * This functor template performs a linear nearest-neighbor search through a graph by invoquing
   * the distance function of an underlying topology. The call operator will return the vertex
   * of the graph whose position value is closest to a given position value.
   * \tparam CompareFunction The functor type that can compare two distance measures (strict weak-ordering).
   */
  template <typename CompareFunction = std::less<double> >
  struct linear_neighbor_search {

    CompareFunction m_compare;
    /**
     * Default constructor.
     * \param compare The comparison functor for ordering the distances (strict weak ordering).
     */
    linear_neighbor_search(CompareFunction compare = CompareFunction()) : m_compare(compare) { };

    /**
     * This function template computes the topological distance between a position and the position of a
     * vertex of a graph. This function is used as a helper to the call-operator overloads.
     * \tparam Vertex The vertex descriptor type.
     * \tparam Topology The topology type which contains the positions.
     * \tparam PositionMap The property-map type which can store the position associated with each vertex.
     * \param p A position in the space.
     * \param u A vertex which has a position associated to it, via the position property-map.
     * \param space The topology objects which define the space in which the positions reside.
     * \param position The property-map which can retrieve the position associated to each vertex.
     */
    template <typename Vertex, typename Topology, typename PositionMap>
    double distance(const typename boost::property_traits<PositionMap>::value_type& p,
                    Vertex u, const Topology& space, PositionMap position) const {
      return space.distance(p, get(position, u));
    };

    /**
     * This call-operator finds the nearest vertex of a graph, to a given position.
     * \tparam Graph The graph type which can contain the vertices, should model boost::VertexListGraphConcept.
     * \tparam Topology The topology type which contains the positions.
     * \tparam PositionMap The property-map type which can store the position associated with each vertex.
     * \param p A position in the space, to which the nearest-neighbor is sought.
     * \param g A graph containing the vertices from which to find the nearest-neighbor.
     * \param space The topology objects which define the space in which the positions reside.
     * \param position The property-map which can retrieve the position associated to each vertex.
     */
    template <typename Graph, typename Topology, typename PositionMap>
    typename boost::graph_traits<Graph>::vertex_descriptor operator()(const typename boost::property_traits<PositionMap>::value_type& p,
								      Graph& g,
								      const Topology& space,
								      PositionMap position) {
      typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
      typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIter;
      VertexIter ui,ui_end; tie(ui,ui_end) = vertices(g);
      return *(min_dist_linear_search(ui,ui_end,boost::bind(&linear_neighbor_search::distance<Vertex,Topology,PositionMap>,this,p,_1,space,position),m_compare,std::numeric_limits<double>::infinity()));
    };

    /**
     * This call-operator finds the nearest vertices of a graph, to a given position.
     * \tparam Graph The graph type which can contain the vertices, should
     *         model boost::VertexListGraphConcept.
     * \tparam Topology The topology type which contains the positions.
     * \tparam PositionMap The property-map type which can store the position associated
     *         with each vertex.
     * \tparam OutputContainer The container type which can contain the list of
     *         nearest-neighbors (STL like container, with iterators, insert, size, and pop_back).
     * \param p A position in the space, to which the nearest-neighbors are sought.
     * \param output The container for the list of nearest-neighbors, the output of this
     *        function, and will be sorted from the nearest neighbor in increasing order.
     * \param g A graph containing the vertices from which to find the nearest-neighbors.
     * \param space The topology objects which define the space in which the positions reside.
     * \param position The property-map which can retrieve the position associated to each vertex.
     * \param max_neighbors The maximum number of neighbors to have in the list.
     * \param radius The minimum distance around the position that a vertex should be in to be
     *        considered a neighbor.
     */
    template <typename Graph, typename Topology, typename PositionMap, typename OutputContainer>
    void operator()(const typename boost::property_traits<PositionMap>::value_type& p,
		    OutputContainer& output,
                    Graph& g,
		    const Topology& space,
		    PositionMap position,
		    unsigned int max_neighbors = 1,
		    double radius = std::numeric_limits<double>::infinity())
    {
      typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
      typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIter;
      VertexIter ui,ui_end; tie(ui,ui_end) = boost::vertices(g);
      min_dist_linear_search(ui,ui_end,output,
                             boost::bind(&linear_neighbor_search::distance<Vertex,Topology,PositionMap>,
                                                          this,p,_1,space,position),
                             m_compare,max_neighbors,radius);
    };
  };

  /**
   * This functor template performs a best-only nearest-neighbor search through a tree by invoquing
   * the distance function of an underlying topology. The call operator will return the vertex
   * of the graph whose position value is likely to be closest to a given position value. This
   * algorithm is approximate. It will select a M vertices from the graph from which it starts
   * a best-only search, where M is obtained as M = number_of_vertices / m_vertex_num_divider.
   * \tparam CompareFunction The functor type that can compare two distance measures (strict weak-ordering).
   */
  template <typename CompareFunction = std::less<double> >
  struct best_only_neighbor_search {

    unsigned int m_vertex_num_divider;
    CompareFunction m_compare;
    /**
     * Default constructor.
     * \param aVertexNumDivider The division factor (should be greater than 1) which determines the
     *        fraction of the total number of vertices that is used to stem the best-only searches.
     *        Typical values are between 4 and 10.
     * \param compare The comparison functor for ordering the distances (strict weak ordering).
     */
    best_only_neighbor_search(unsigned int aVertexNumDivider = 10,
			      CompareFunction compare = CompareFunction()) :
                              m_vertex_num_divider(aVertexNumDivider), m_compare(compare) { };

    /**
     * This function template computes the topological distance between a position and the position of a
     * vertex of a graph. This function is used as a helper to the call-operator overloads.
     * \tparam Vertex The vertex descriptor type.
     * \tparam Topology The topology type which contains the positions.
     * \tparam PositionMap The property-map type which can store the position associated with each vertex.
     * \param p A position in the space.
     * \param u A vertex which has a position associated to it, via the position property-map.
     * \param space The topology objects which define the space in which the positions reside.
     * \param position The property-map which can retrieve the position associated to each vertex.
     */
    template <typename Vertex, typename Topology, typename PositionMap>
    double distance(const typename boost::property_traits<PositionMap>::value_type& p,
                    Vertex u, const Topology& space, PositionMap position) const {
      return space.distance(p, get(position, u));
    };

    template <typename Graph, typename Topology, typename PositionMap>
    void search(const typename boost::property_traits<PositionMap>::value_type& p,
		typename boost::graph_traits<Graph>::vertex_descriptor& u,
		double& d_min, Graph& g, const Topology& space, PositionMap position) {
      typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
      typedef typename boost::graph_traits<Graph>::out_edge_iterator EdgeIter;
      d_min = distance(p,u,space,position);
      while(boost::out_degree(u,g)) {
        Vertex v_min = u;
        EdgeIter ei, ei_end;
        for(boost::tie(ei,ei_end) = boost::out_edges(u,g); ei != ei_end; ++ei) {
          Vertex v = boost::target(*ei,g); double d_v = distance(p,v,space,position);
          if(m_compare(d_v,d_min)) {
            d_min = d_v; v_min = v;
          };
        };
        if(v_min == u)
          return;
        u = v_min;
      };
      return;
    };

    /**
     * This call-operator finds the nearest vertex of a graph, to a given position.
     * \tparam Graph The graph type which can contain the vertices, should
     *         model boost::VertexListGraphConcept and boost::IncidenceGraphConcept.
     * \tparam Topology The topology type which contains the positions.
     * \tparam PositionMap The property-map type which can store the position associated with each vertex.
     * \param p A position in the space, to which the nearest-neighbor is sought.
     * \param g A graph containing the vertices from which to find the nearest-neighbor,
     *        should be tree-structured.
     * \param space The topology objects which define the space in which the positions reside.
     * \param position The property-map which can retrieve the position associated to each vertex.
     */
    template <typename Graph, typename Topology, typename PositionMap>
    typename boost::graph_traits<Graph>::vertex_descriptor operator()(const typename boost::property_traits<PositionMap>::value_type& p,
								      Graph& g, const Topology& space, PositionMap position) {
      typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
      if(m_vertex_num_divider == 0)
	m_vertex_num_divider = 1;
      Vertex u_min = boost::vertex(std::rand() % boost::num_vertices(g),g);
      double d_min;
      search(p,u_min,d_min,g,space,position);
      for(unsigned int i = 0; i < boost::num_vertices(g) / m_vertex_num_divider; ++i) {
        double d_v; Vertex v = boost::vertex(std::rand() % boost::num_vertices(g),g);
        search(p,v,d_v,g,space,position);
        if(m_compare(d_v,d_min)) {
          d_min = d_v; u_min = v;
        };
      };
      return u_min;
    };

    template <typename Graph, typename Topology, typename PositionMap, typename OutputContainer>
    void search(const typename boost::property_traits<PositionMap>::value_type& p,
		typename boost::graph_traits<Graph>::vertex_descriptor u, OutputContainer& output, std::vector<double>& output_dist,
		double d_min, Graph& g, const Topology& space, PositionMap position, unsigned int max_neighbors, double radius) {
      typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
      typedef typename boost::graph_traits<Graph>::out_edge_iterator EdgeIter;
      if(m_compare(d_min, radius)) {
        std::vector<double>::iterator it_lo = std::lower_bound(output_dist.begin(),output_dist.end(),d_min,m_compare);
        if((it_lo != output_dist.end()) || (output_dist.size() < max_neighbors)) {
 	  output_dist.insert(it_lo, d_min);
	  typename OutputContainer::iterator itv = output.begin();
	  for(std::vector<double>::iterator it = output_dist.begin(); (itv != output.end()) && (it != it_lo); ++itv,++it) ;
	  output.insert(itv, u);
	  if(output.size() > max_neighbors) {
	    output.pop_back();
	    output_dist.pop_back();
	  };
        };
      };
      EdgeIter ei, ei_end;
      for(boost::tie(ei,ei_end) = boost::out_edges(u,g); ei != ei_end; ++ei) {
	Vertex v = boost::target(*ei,g); double d_v = distance(p,v,space,position);
	if(m_compare(d_v,d_min))
	  search(p,v,output,output_dist,d_v,g,space,position,max_neighbors,radius);
      };
    };

    /**
     * This call-operator finds the nearest vertices of a graph, to a given position.
     * \tparam Graph The graph type which can contain the vertices, should
     *         model boost::VertexListGraphConcept and boost::IncidenceGraphConcept.
     * \tparam Topology The topology type which contains the positions.
     * \tparam PositionMap The property-map type which can store the position associated
     *         with each vertex.
     * \tparam OutputContainer The container type which can contain the list of
     *         nearest-neighbors (STL like container, with iterators, insert, size, and pop_back).
     * \param p A position in the space, to which the nearest-neighbors are sought.
     * \param output The container for the list of nearest-neighbors, the output of this
     *        function, and will be sorted from the nearest neighbor in increasing order.
     * \param g A graph containing the vertices from which to find the nearest-neighbors,
     *        should be tree-structured.
     * \param space The topology objects which define the space in which the positions reside.
     * \param position The property-map which can retrieve the position associated to each vertex.
     * \param max_neighbors The maximum number of neighbors to have in the list.
     * \param radius The minimum distance around the position that a vertex should be in to be
     *        considered a neighbor.
     */
    template <typename Graph, typename Topology, typename PositionMap, typename OutputContainer>
    void operator()(const typename boost::property_traits<PositionMap>::value_type& p, OutputContainer& output,
		    Graph& g, const Topology& space, PositionMap position, unsigned int max_neighbors = 1, double radius = std::numeric_limits<double>::infinity()) {
      typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
      output.clear();
      std::vector<double> output_dist;
      if(m_vertex_num_divider == 0)
	m_vertex_num_divider = 1;
      for(unsigned int i = 0; i < boost::num_vertices(g) / m_vertex_num_divider; ++i) {
        Vertex v = boost::vertex(std::rand() % boost::num_vertices(g),g);
	double d_v = distance(p,v,space,position);
        search(p,v,output,output_dist,d_v,g,space,position,max_neighbors,radius);
      };
    };
  };

#endif


// BLAS level 2 -- complex numbers

#include <iostream>
#include <complex>
#include <boost/numeric/bindings/blas/level1.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/conj.hpp>
#include <boost/numeric/bindings/io.hpp>
#include <boost/numeric/bindings/noop.hpp>
#ifdef F_USE_STD_VECTOR
#include <boost/numeric/bindings/std/vector.hpp>
#endif
#include "utils.h"

namespace ublas = boost::numeric::ublas;
namespace blas = boost::numeric::bindings::blas;

using std::cout;
using std::endl;

typedef double real_t;
typedef std::complex<real_t> cmplx_t;

#ifndef F_USE_STD_VECTOR
typedef ublas::vector<cmplx_t> vct_t;
typedef ublas::matrix<cmplx_t, ublas::row_major> m_t;
#else
typedef ublas::vector<cmplx_t, std::vector<cmplx_t> > vct_t;
typedef ublas::matrix<cmplx_t, ublas::column_major, std::vector<cmplx_t> > m_t;
#endif

int main() {

  cout << endl;

  vct_t vx (2);
  blas::set( 1, vx );
  std::cout << "vx " << bindings::noop( vx ) << std::endl;
  vct_t vy (4); // vector size can be larger
                // than corresponding matrix size
  blas::set( 0, vy );
  std::cout << "vy " << bindings::noop( vy ) << std::endl;
  cout << endl;

  m_t m (3, 2);
  init_m (m, kpp (1));
  print_m (m, "m");
  cout << endl;

  // vy = m vx
  blas::gemv ( 1.0, m, vx, 0.0, vy);
  std::cout << "m vx " << bindings::noop( vy ) << std::endl;
  blas::gemv ( 1.0, m, vx, 0.0, vy);
  std::cout << "m vx " << bindings::noop( vy ) << std::endl;
  cout << endl;

  m (0, 0) = cmplx_t (0., 1.);
  m (0, 1) = cmplx_t (0., 2.);
  m (1, 0) = cmplx_t (0., 3.);
  m (1, 1) = cmplx_t (0., 4.);
  m (2, 0) = cmplx_t (0., 5.);
  m (2, 1) = cmplx_t (0., 6.);
  print_m (m, "m");
  cout << endl;

  // vy = m vx
  blas::gemv ( 1.0, m, vx, 0.0, vy);
  std::cout << "m vx " << bindings::noop( vy ) << std::endl;
  blas::gemv ( 1.0, m, vx, 0.0, vy);
  std::cout << "m vx " << bindings::noop( vy ) << std::endl;
  cout << endl;

  m (0, 0) = cmplx_t (-1., 1.);
  m (0, 1) = cmplx_t (-2., 2.);
  m (1, 0) = cmplx_t (-3., 3.);
  m (1, 1) = cmplx_t (-4., 4.);
  m (2, 0) = cmplx_t (-5., 5.);
  m (2, 1) = cmplx_t (-6., 6.);
  print_m (m, "m");
  cout << endl;

  // vy = m vx
  blas::gemv ( 1.0, m, vx, 0.0, vy);
  std::cout << "m vx " << bindings::noop( vy ) << std::endl;
  blas::gemv ( 1.0, m, vx, 0, vy);
  std::cout << "m vx " << bindings::noop( vy ) << std::endl;
  cout << endl;

  blas::set ( 1, vx );
  std::cout << "vx " << bindings::noop( vx ) << std::endl;

  // vy = m vx
  blas::gemv ( 1.0, m, vx, 0.0, vy);
  std::cout << "m vx " << bindings::noop( vy ) << std::endl;
  blas::gemv ( 1.0, m, vx, 0.0, vy);
  std::cout << "m vx " << bindings::noop( vy ) << std::endl;
  cout << endl;

  blas::set ( cmplx_t (1, 1), vx );
  std::cout << "vx " << bindings::noop( vx ) << std::endl;

  // vy = m vx
  blas::gemv ( 1.0, m, vx, 0.0, vy);
  std::cout << "m vx " << bindings::noop( vy ) << std::endl;
  blas::gemv ( 1.0, m, vx, 0.0, vy);
  std::cout << "m vx " << bindings::noop( vy ) << std::endl;
  cout << endl;

  // vx = m^H vy
  blas::set ( cmplx_t (-1,-1), vy );
  std::cout << "vy " << bindings::noop( vy ) << std::endl;
  blas::gemv ( 1.0, bindings::conj(m), vy, 0.0, vx);
  std::cout << "m^H vy " << bindings::noop( vx ) << std::endl;
  cout << endl;

  m_t mx (2, 2);
  m_t my (3, 2);

  ublas::matrix_column<m_t> mxc0 (mx, 0),
                            mxc1 (mx, 1);
  ublas::matrix_column<m_t> myc0 (my, 0),
                            myc1 (my, 1);

  blas::set ( cmplx_t (1, 0), mxc0 );
  blas::set ( cmplx_t (0, 0), mxc1 );
  blas::set ( cmplx_t (0, 0), myc0 );
  blas::set ( cmplx_t (0, 0), myc1 );

  print_m (mx, "mx");
  cout << endl;
  print_m (my, "my");
  cout << endl;

  // my[.,0] = m mx[.,0]
  blas::gemv ( 1.0, m, mxc0, 0.0, myc0);
  print_m (my, "m mx[.,0]");

  cout << endl;

}


/*
Copyright (c) 2020 ETH Zurich

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

Author: Katrin Lasinger
*/

#define _USE_MATH_DEFINES

#include <numeric>
#include <vector>
#include <algorithm>
#include <omp.h>
#include "mex.h"
#include <math.h>

#include <Eigen/Dense>
using namespace Eigen;

/// matlab calling
void proj2d ( int nlhs, mxArray *plhs[],
                int nrhs, const mxArray *prhs[])
{

  typedef double Scalar;

  Scalar *part = (Scalar*)mxGetPr(prhs[0]);

  Scalar *P_vec = (Scalar*)mxGetPr(prhs[1]);

  Matrix<Scalar, 3, 4> P;

  for(int j=0;j<4;j++)
    for (int i=0;i<3;i++)
		P(i,j) = P_vec[i+j*3];

  const mwSize* dims = mxGetDimensions(prhs[0]);
  size_t numpart = dims[1]; // list of points
  size_t partDim = dims[0]; // featureDim

  plhs[0] = mxCreateDoubleMatrix(2,numpart, mxREAL);
  Scalar* part2d = (Scalar*)mxGetPr(plhs[0]);

  plhs[1] = mxCreateDoubleMatrix(3,numpart, mxREAL);
  Scalar* xhom = (Scalar*)mxGetPr(plhs[1]);

#pragma omp parallel for //schedule (static)
  for (long long i = 0; i < numpart; i++)
  {

	Vector4d pt;
	pt << part[i*3],part[i*3+1], part[i*3+2], 1;

	Vector3d pt2d_hom;
	pt2d_hom = P * pt;

	xhom[i*3] =pt2d_hom(0);
	xhom[i*3+1] = pt2d_hom(1);
	xhom[i*3+2] = pt2d_hom(2);
	part2d[i*2] = pt2d_hom(0) / pt2d_hom(2);
	part2d[i*2+1] = pt2d_hom(1) / pt2d_hom(2);

  }

}