// This file is part of Man, a robotic perception, locomotion, and
// team strategy application created by the Northern Bites RoboCup
// team of Bowdoin College in Brunswick, Maine, for the Aldebaran
// Nao robot.
//
// Man is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// Man 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 Lesser Public License for more details.
//
// You should have received a copy of the GNU General Public License
// and the GNU Lesser Public License along with Man.  If not, see
// <http://www.gnu.org/licenses/>.

#include <math.h>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>              // for lu_factorize
#include <boost/numeric/ublas/io.hpp>              // for cout

#include "nb/CoordFrame.h"

using namespace NBMath;
using namespace CoordFrame4D;

// -------------------- Helper matrix methods --------------------

//TODO: Move all these rot4D, etc into a Coord4D namespace.
// also, make all these use bounded matrices (see NBMatrixMath)
const NBMath::ufmatrix4
CoordFrame4D::rotation4D(const Axis axis,
                         const double angle) {
    NBMath::ufmatrix4 rot = boost::numeric::ublas::identity_matrix <double>(4);

    if (angle == 0.0) { //OPTIMIZAION POINT
        return rot;
    }

    //TODO: Make this one call:
    float sinAngle;
    float cosAngle;
    sincosf(angle, &sinAngle, &cosAngle);

    switch(axis) {
    case X_AXIS:
        rot(Y_AXIS, Y_AXIS) =  cosAngle;
        rot(Y_AXIS, Z_AXIS) = -sinAngle;
        rot(Z_AXIS, Y_AXIS) =  sinAngle;
        rot(Z_AXIS, Z_AXIS) =  cosAngle;
        break;
    case Y_AXIS:
        rot(X_AXIS, X_AXIS) =  cosAngle;
        rot(X_AXIS, Z_AXIS) =  sinAngle;
        rot(Z_AXIS, X_AXIS) = -sinAngle;
        rot(Z_AXIS, Z_AXIS) =  cosAngle;
        break;
    case Z_AXIS:
        rot(X_AXIS, X_AXIS) =  cosAngle;
        rot(X_AXIS, Y_AXIS) = -sinAngle;
        rot(Y_AXIS, X_AXIS) =  sinAngle;
        rot(Y_AXIS, Y_AXIS) =  cosAngle;
        break;
    default:
        break;
    }
    return rot;
}

const NBMath::ufmatrix4 CoordFrame4D::translation4D(const double dx,
                                                    const double dy,
                                                    const double dz) {
    NBMath::ufmatrix4 trans = boost::numeric::ublas::identity_matrix <double>(4);
    trans(X_AXIS, W_AXIS) = dx;
    trans(Y_AXIS, W_AXIS) = dy;
    trans(Z_AXIS, W_AXIS) = dz;
    return trans;
}

const NBMath::ufvector4 CoordFrame4D::vector4D(const double x, const double y,
                                               const double z,
                                               const double w) {
    NBMath::ufvector4 p = boost::numeric::ublas::zero_vector <double> (4);
    p(0) = x;
    p(1) = y;
    p(2) = z;
    p(3) = w;
    return p;
}

/**
 * Returns precalculated Trans[x,y,z].Rotz[wz].Roty[wy].Rotx[wx]
 */
const NBMath::ufmatrix4 CoordFrame4D::get6DTransform(const double x,
                                                     const double y,
                                                     const double z,
                                                     const double wx,
                                                     const double wy,
                                                     const double wz){
    float cwx,cwy,cwz,
        swx,swy,swz;
    sincosf(wx,&swx,&cwx);
    sincosf(wy,&swy,&cwy);
    sincosf(wz,&swz,&cwz);
    NBMath::ufmatrix4 r = boost::numeric::ublas::identity_matrix<double>(4);

    //Row 1
    r(0,0) =cwy*cwz;
    r(0,1) =cwz*swx*swy-cwx*swz;
    r(0,2) =cwx*cwz*swy+swx*swz;
    r(0,3) =x;
    //Row2
    r(1,0) =cwy*swz;
    r(1,1) =cwx*cwz+swx*swy*swz;
    r(1,2) =-cwz*swx+cwx*swy*swz;
    r(1,3) =y;
    //Row3
    r(2,0) =-swy;
    r(2,1) =cwy*swx;
    r(2,2) =cwx*cwy;
    r(2,3) =z;

    return r;
}

const NBMath::ufmatrix4
CoordFrame4D::invertHomogenous(const NBMath::ufmatrix4 source){
    const NBMath::ufmatrix3 Rt = trans(subrange(source,0,3,0,3));
    const NBMath::ufvector3 Rtd = -prod(Rt,
                                        CoordFrame3D::vector3D(source(0,3),
                                                               source(1,3),
                                                               source(2,3)));
    NBMath::ufmatrix4 result =
        boost::numeric::ublas::identity_matrix<double>(4);
    subrange(result,0,3,0,3) = Rt;
    result(0,3) = Rtd(0);
    result(1,3) = Rtd(1);
    result(2,3) = Rtd(2);
    return result;
}


#include <list>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/index/rtree.hpp>

namespace bg  = boost::geometry;
namespace bgi = boost::geometry::index;

typedef bg::model::point<float, 2, bg::cs::cartesian> point2;
typedef bg::model::box<point2> box2;
typedef std::pair<point2, uint> value2;
typedef bgi::rtree<value2, bgi::quadratic<16>> r2tree;

typedef bg::model::point<float, 3, bg::cs::cartesian> point;
typedef bg::model::box<point> box;
typedef std::pair<point, uint> value;
typedef bgi::rtree<value, bgi::quadratic<16>> r3tree;

#include "spatial-index.hpp"

namespace perceive
{
// ------------------------------------------------------------- Spatial-2-Index

inline LabeledVector2 value_to_lv2(const value2& v, const size_t& size)
{
   Expects(v.second < size);
   return std::make_pair(to_vec2(Vector2f(v.first.get<0>(), v.first.get<1>())),
                         v.second);
}

struct Spatial2Index::Pimpl
{
   Pimpl(Spatial2Index& si) {}

   r2tree index;
   size_t size = 0;

   template<typename Iterator> void init(Iterator begin, Iterator end)
   {
      index.clear();
      auto to_r2tree = [&](const Vector2& p, uint idx) {
         return std::make_pair(point2(float(p.x), float(p.y)), idx);
      };
      uint32_t counter = 0;
      while(begin != end) index.insert(to_r2tree(*begin++, counter++));
      size = counter;
   }
};

Spatial2Index::Spatial2Index() { pimpl_ = new Pimpl(*this); }
Spatial2Index::~Spatial2Index() { delete pimpl_; }
Spatial2Index::Spatial2Index(const Spatial2Index& rhs)
    : pimpl_(nullptr)
{
   pimpl_ = new Pimpl(*this);
   *this  = rhs;
}
Spatial2Index::Spatial2Index(Spatial2Index&& rhs)
    : pimpl_(nullptr)
{
   std::swap(pimpl_, rhs.pimpl_);
}
Spatial2Index& Spatial2Index::operator=(const Spatial2Index& rhs)
{
   if(this == &rhs) return *this;
   *pimpl_ = *(rhs.pimpl_);
   return *this;
}
Spatial2Index& Spatial2Index::operator=(Spatial2Index&& rhs)
{
   if(this == &rhs) return *this;
   delete pimpl_;
   pimpl_ = nullptr;
   std::swap(pimpl_, rhs.pimpl_);
   return *this;
}
void Spatial2Index::clear() { pimpl_->index.clear(); }
void Spatial2Index::init(const vector<Vector2>& pts)
{
   pimpl_->init(pts.begin(), pts.end());
}
void Spatial2Index::init(const std::deque<Vector2>& pts)
{
   pimpl_->init(pts.begin(), pts.end());
}
size_t Spatial2Index::size() const noexcept { return pimpl_->size; }

bool Spatial2Index::empty() const noexcept { return size() == 0; }

void Spatial2Index::query_nearest(const Vector2& P,
                                  uint n_points,
                                  std::deque<LabeledVector2>& ret) const
{
   ret.clear();
   if(!empty()) {
      vector<value2> values;
      values.reserve(n_points);
      pimpl_->index.query(
          bgi::nearest(point2(float(P.x), float(P.y)), n_points),
          std::back_inserter(values));
      for(const value2& v : values) ret.push_back(value_to_lv2(v, size()));
   }
}

void Spatial2Index::query_nearest(const Vector2& P,
                                  uint n_points,
                                  vector<LabeledVector2>& ret) const
{
   ret.clear();
   if(!empty()) {
      vector<value2> values;
      values.reserve(n_points);
      pimpl_->index.query(
          bgi::nearest(point2(float(P.x), float(P.y)), n_points),
          std::back_inserter(values));
      ret.resize(values.size());
      for(uint i = 0; i < values.size(); ++i)
         ret[i] = value_to_lv2(values[i], size());
   }
}

LabeledVector2 Spatial2Index::query_nearest(const Vector2& P) const
{
   if(empty()) {
      return std::make_pair(Vector2::nan(), 0u);
   } else {
      std::list<value2> values;
      pimpl_->index.query(bgi::nearest(point2(float(P.x), float(P.y)), 1),
                          std::back_inserter(values));
      return value_to_lv2(values.front(), size());
   }
}

void Spatial2Index::query_region(const AABB& aabb,
                                 std::deque<LabeledVector2>& ret) const
{
   // find values intersecting some area defined by a box
   box2 query_box(point2(float(aabb.left), float(aabb.top)),
                  point2(float(aabb.right), float(aabb.bottom)));
   std::vector<value2> result_s;
   const auto& rtree = pimpl_->index;
   rtree.query(bgi::intersects(query_box), std::back_inserter(result_s));
   for(const value2& v : result_s) ret.push_back(value_to_lv2(v, size()));
}

void Spatial2Index::query_region(const AABB& aabb,
                                 vector<LabeledVector2>& ret) const
{
   // find values intersecting some area defined by a box
   box2 query_box(point2(float(aabb.left), float(aabb.top)),
                  point2(float(aabb.right), float(aabb.bottom)));
   std::vector<value2> values;
   const auto& rtree = pimpl_->index;
   rtree.query(bgi::intersects(query_box), std::back_inserter(values));
   ret.resize(values.size());
   for(uint i = 0; i < values.size(); ++i)
      ret[i] = value_to_lv2(values[i], size());
}

void Spatial2Index::query_region(const AABB& aabb, vector<Vector2>& ret) const
{
   // find values intersecting some area defined by a box
   box2 query_box(point2(float(aabb.left), float(aabb.top)),
                  point2(float(aabb.right), float(aabb.bottom)));
   std::vector<value2> values;
   const auto& rtree = pimpl_->index;
   rtree.query(bgi::intersects(query_box), std::back_inserter(values));
   ret.resize(values.size());
   for(uint i = 0; i < values.size(); ++i)
      ret[i] = to_vec2(
          Vector2f(values[i].first.get<0>(), values[i].first.get<1>()));
}

// ------------------------------------------------------------- Spatial-3-Index

inline LabeledVector3 value_to_lv3(const value& v)
{
   return std::make_pair(
       to_vec3(Vector3f(v.first.get<0>(), v.first.get<1>(), v.first.get<2>())),
       v.second);
}

struct Spatial3Index::Pimpl
{
   Pimpl(Spatial3Index& si) {}

   r3tree index;

   template<typename Iterator> void init(Iterator begin, Iterator end)
   {
      index.clear();
      auto to_r3tree = [&](const Vector3& q, uint idx) {
         const Vector3f p = to_vec3f(q);
         return std::make_pair(point(p.x, p.y, p.z), idx);
      };
      uint counter = 0;
      while(begin != end) index.insert(to_r3tree(*begin++, counter++));
   }
};

Spatial3Index::Spatial3Index() { pimpl_ = new Pimpl(*this); }
Spatial3Index::~Spatial3Index() { delete pimpl_; }
Spatial3Index::Spatial3Index(const Spatial3Index& rhs)
    : pimpl_(nullptr)
{
   pimpl_ = new Pimpl(*this);
   *this  = rhs;
}
Spatial3Index::Spatial3Index(Spatial3Index&& rhs)
    : pimpl_(nullptr)
{
   pimpl_     = rhs.pimpl_;
   rhs.pimpl_ = nullptr;
}
Spatial3Index& Spatial3Index::operator=(const Spatial3Index& rhs)
{
   if(this == &rhs) return *this;
   *pimpl_ = *(rhs.pimpl_);
   return *this;
}
Spatial3Index& Spatial3Index::operator=(Spatial3Index&& rhs)
{
   if(this == &rhs) return *this;
   delete pimpl_;
   pimpl_     = rhs.pimpl_;
   rhs.pimpl_ = nullptr;
   return *this;
}
void Spatial3Index::clear() { pimpl_->index.clear(); }
void Spatial3Index::init(const vector<Vector3>& pts)
{
   pimpl_->init(pts.begin(), pts.end());
}
void Spatial3Index::init(const std::deque<Vector3>& pts)
{
   pimpl_->init(pts.begin(), pts.end());
}

void Spatial3Index::query_nearest(const Vector3& Q,
                                  uint n_points,
                                  std::deque<LabeledVector3>& ret) const
{
   vector<value> values;
   values.reserve(n_points);
   const Vector3f P = to_vec3f(Q);
   pimpl_->index.query(bgi::nearest(point(P.x, P.y, P.z), n_points),
                       std::back_inserter(values));
   for(const value& v : values) ret.push_back(value_to_lv3(v));
}

LabeledVector3 Spatial3Index::query_nearest(const Vector3& Q) const
{
   std::list<value> values;
   const Vector3f P = to_vec3f(Q);
   pimpl_->index.query(bgi::nearest(point(P.x, P.y, P.z), 1),
                       std::back_inserter(values));
   return value_to_lv3(values.front());
}

} // namespace perceive


#include "camodocal/calib/HandEyeCalibration.h"

#include <boost/throw_exception.hpp>
#include <iostream>

#include <ceres/ceres.h>
#include "camodocal/EigenUtils.h"
#include "camodocal/calib/DualQuaternion.h"

namespace camodocal {

/// @todo there may be an alignment issue, see
/// http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
class PoseError {
  public:
    PoseError(Eigen::Vector3d r1, Eigen::Vector3d t1, Eigen::Vector3d r2,
              Eigen::Vector3d t2)
        : m_rvec1(r1), m_rvec2(r2), m_tvec1(t1), m_tvec2(t2) {}

    template <typename T>
    bool operator()(const T* const q4x1, const T* const t3x1,
                    T* residual) const {
        Eigen::Quaternion<T> q(q4x1[0], q4x1[1], q4x1[2], q4x1[3]);
        Eigen::Matrix<T, 3, 1> t;
        t << t3x1[0], t3x1[1], t3x1[2];

        DualQuaternion<T> dq(q, t);

        Eigen::Matrix<T, 3, 1> r1 = m_rvec1.cast<T>();
        Eigen::Matrix<T, 3, 1> t1 = m_tvec1.cast<T>();
        Eigen::Matrix<T, 3, 1> r2 = m_rvec2.cast<T>();
        Eigen::Matrix<T, 3, 1> t2 = m_tvec2.cast<T>();

        DualQuaternion<T> dq1(AngleAxisToQuaternion<T>(r1), t1);
        DualQuaternion<T> dq2(AngleAxisToQuaternion<T>(r2), t2);
        DualQuaternion<T> dq1_ = dq * dq2 * dq.inverse();

        DualQuaternion<T> diff = (dq1.inverse() * dq1_).log();
        residual[0] = diff.real().squaredNorm() + diff.dual().squaredNorm();

        return true;
    }

  private:
    Eigen::Vector3d m_rvec1, m_rvec2, m_tvec1, m_tvec2;

  public:
    /// @see
    /// http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

bool HandEyeCalibration::mVerbose = true;

HandEyeCalibration::HandEyeCalibration() {}

void HandEyeCalibration::setVerbose(bool on) { mVerbose = on; }

/// Reorganize data to prepare for running SVD
/// Daniilidis 1999 Section 6, Equations (31) and (33), on page 291
template <typename T>
static Eigen::MatrixXd ScrewToStransposeBlockofT(
    const Eigen::Matrix<T, 3, 1>& a, const Eigen::Matrix<T, 3, 1>& a_prime,
    const Eigen::Matrix<T, 3, 1>& b, const Eigen::Matrix<T, 3, 1>& b_prime) {
    Eigen::MatrixXd Stranspose(6, 8);
    Stranspose.setZero();

    typedef Eigen::Matrix<T, 3, 1> VecT;
    auto skew_a_plus_b = skew(VecT(a + b));
    auto a_minus_b = a - b;
    Stranspose.block<3, 1>(0, 0) = a_minus_b;
    Stranspose.block<3, 3>(0, 1) = skew_a_plus_b;
    Stranspose.block<3, 1>(3, 0) = a_prime - b_prime;
    Stranspose.block<3, 3>(3, 1) = skew(VecT(a_prime + b_prime));
    Stranspose.block<3, 1>(3, 4) = a_minus_b;
    Stranspose.block<3, 3>(3, 5) = skew_a_plus_b;

    return Stranspose;
}

/// Reorganize data to prepare for running SVD
/// Daniilidis 1999 Section 6, Equations (31) and (33), on page 291
// @pre no zero rotations, thus (rvec1.norm() != 0 && rvec2.norm() != 0) == true
template <typename T>
static Eigen::MatrixXd AxisAngleToSTransposeBlockOfT(
    const Eigen::Matrix<T, 3, 1>& rvec1, const Eigen::Matrix<T, 3, 1>& tvec1,
    const Eigen::Matrix<T, 3, 1>& rvec2, const Eigen::Matrix<T, 3, 1>& tvec2) {
    double theta1, d1;
    Eigen::Vector3d l1, m1;
    AngleAxisAndTranslationToScrew(rvec1, tvec1, theta1, d1, l1, m1);

    double theta2, d2;
    Eigen::Vector3d l2, m2;
    AngleAxisAndTranslationToScrew(rvec2, tvec2, theta2, d2, l2, m2);

    Eigen::Vector3d a = l1;
    Eigen::Vector3d a_prime = m1;
    Eigen::Vector3d b = l2;
    Eigen::Vector3d b_prime = m2;

    return ScrewToStransposeBlockofT(a, a_prime, b, b_prime);
}

// docs in header
void HandEyeCalibration::estimateHandEyeScrew(
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs2,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs2,
    Eigen::Matrix4d& H_12, bool planarMotion) {
    int motionCount = rvecs1.size();
    Eigen::MatrixXd T(motionCount * 6, 8);
    T.setZero();

    for (size_t i = 0; i < motionCount; ++i) {
        const Eigen::Vector3d& rvec1 = rvecs1.at(i);
        const Eigen::Vector3d& tvec1 = tvecs1.at(i);
        const Eigen::Vector3d& rvec2 = rvecs2.at(i);
        const Eigen::Vector3d& tvec2 = tvecs2.at(i);

        // Skip cases with zero rotation
        if (rvec1.norm() == 0 || rvec2.norm() == 0)
            continue;

        T.block<6, 8>(i * 6, 0) =
            AxisAngleToSTransposeBlockOfT(rvec1, tvec1, rvec2, tvec2);
    }

    auto dq = estimateHandEyeScrewInitial(T, planarMotion);

    H_12 = dq.toMatrix();
    if (mVerbose) {
        std::cout << "# INFO: Before refinement: H_12 = " << std::endl;
        std::cout << H_12 << std::endl;
    }

    estimateHandEyeScrewRefine(dq, rvecs1, tvecs1, rvecs2, tvecs2);

    H_12 = dq.toMatrix();
    if (mVerbose) {
        std::cout << "# INFO: After refinement: H_12 = " << std::endl;
        std::cout << H_12 << std::endl;
    }
}

// docs in header
void HandEyeCalibration::estimateHandEyeScrew(
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs2,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs2,
    Eigen::Matrix4d& H_12, ceres::Solver::Summary& summary, bool planarMotion) {
    int motionCount = rvecs1.size();
    Eigen::MatrixXd T(motionCount * 6, 8);
    T.setZero();

    for (size_t i = 0; i < motionCount; ++i) {
        const Eigen::Vector3d& rvec1 = rvecs1.at(i);
        const Eigen::Vector3d& tvec1 = tvecs1.at(i);
        const Eigen::Vector3d& rvec2 = rvecs2.at(i);
        const Eigen::Vector3d& tvec2 = tvecs2.at(i);

        // Skip cases with zero rotation
        if (rvec1.norm() == 0 || rvec2.norm() == 0)
            continue;

        T.block<6, 8>(i * 6, 0) =
            AxisAngleToSTransposeBlockOfT(rvec1, tvec1, rvec2, tvec2);
    }

    auto dq = estimateHandEyeScrewInitial(T, planarMotion);

    H_12 = dq.toMatrix();
    if (mVerbose) {
        std::cout << "# INFO: Before refinement: H_12 = " << std::endl;
        std::cout << H_12 << std::endl;
    }

    estimateHandEyeScrewRefine(dq, rvecs1, tvecs1, rvecs2, tvecs2, summary);

    H_12 = dq.toMatrix();
    if (mVerbose) {
        std::cout << "# INFO: After refinement: H_12 = " << std::endl;
        std::cout << H_12 << std::endl;
    }
}

// docs in header
DualQuaterniond
HandEyeCalibration::estimateHandEyeScrewInitial(Eigen::MatrixXd& T,
                                                bool planarMotion) {

    // dq(r1, t1) = dq * dq(r2, t2) * dq.inv
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(T, Eigen::ComputeFullU |
                                                 Eigen::ComputeFullV);

    // v7 and v8 span the null space of T, v6 may also be one
    // if rank = 5.
    Eigen::Matrix<double, 8, 1> v6 = svd.matrixV().block<8, 1>(0, 5);
    Eigen::Matrix<double, 8, 1> v7 = svd.matrixV().block<8, 1>(0, 6);
    Eigen::Matrix<double, 8, 1> v8 = svd.matrixV().block<8, 1>(0, 7);

    // if rank = 5
    if (planarMotion) //(rank == 5)
    {
        if (mVerbose) {
            std::cout
                << "# INFO: No unique solution, returned an arbitrary one. "
                << std::endl;
        }

        v7 += v6;
    }

    Eigen::Vector4d u1 = v7.block<4, 1>(0, 0);
    Eigen::Vector4d v1 = v7.block<4, 1>(4, 0);
    Eigen::Vector4d u2 = v8.block<4, 1>(0, 0);
    Eigen::Vector4d v2 = v8.block<4, 1>(4, 0);

    double lambda1 = 0;
    double lambda2 = 0.0;

    if (u1.dot(v1) == 0.0) {
        std::swap(u1, u2);
        std::swap(v1, v2);
    }
    if (u1.dot(v1) != 0.0) {
        double s[2];
        solveQuadraticEquation(u1.dot(v1), u1.dot(v2) + u2.dot(v1), u2.dot(v2),
                               s[0], s[1]);

        // find better solution for s
        double t[2];
        for (int i = 0; i < 2; ++i) {
            t[i] =
                s[i] * s[i] * u1.dot(u1) + 2 * s[i] * u1.dot(u2) + u2.dot(u2);
        }

        int idx;
        if (t[0] > t[1]) {
            idx = 0;
        } else {
            idx = 1;
        }

        double discriminant =
            4.0 * square(u1.dot(u2)) - 4.0 * (u1.dot(u1) * u2.dot(u2));
        if (discriminant == 0.0 && mVerbose) {
            //            std::cout << "# INFO: Noise-free case" << std::endl;
        }

        lambda2 = sqrt(1.0 / t[idx]);
        lambda1 = s[idx] * lambda2;
    } else {
        if (u1.norm() == 0 && u2.norm() > 0) {
            lambda1 = 0;
            lambda2 = 1.0 / u2.norm();
        } else if (u2.norm() == 0 && u1.norm() > 0) {
            lambda1 = 1.0 / u1.norm();
            lambda2 = 0;
        } else {
            std::ostringstream ss;

            ss << "camodocal::HandEyeCalibration error: normalization could "
                  "not be handled. Your rotations and translations are "
                  "probably either not aligned or not passed in properly.";
            ss << "u1:" << std::endl;
            ss << u1 << std::endl;
            ss << "v1:" << std::endl;
            ss << v1 << std::endl;
            ss << "u2:" << std::endl;
            ss << u2 << std::endl;
            ss << "v2:" << std::endl;
            ss << v2 << std::endl;
            ss << "Not handled yet. Your rotations and translations are "
                  "probably either not aligned or not passed in properly."
               << std::endl;

            BOOST_THROW_EXCEPTION(std::runtime_error(ss.str()));
        }
    }

    // rotation
    Eigen::Vector4d q_coeffs = lambda1 * u1 + lambda2 * u2;
    Eigen::Vector4d q_prime_coeffs = lambda1 * v1 + lambda2 * v2;

    Eigen::Quaterniond q(q_coeffs(0), q_coeffs(1), q_coeffs(2), q_coeffs(3));
    Eigen::Quaterniond d(q_prime_coeffs(0), q_prime_coeffs(1),
                         q_prime_coeffs(2), q_prime_coeffs(3));

    return DualQuaterniond(q, d);
}

// docs in header
bool HandEyeCalibration::solveQuadraticEquation(double a, double b, double c,
                                                double& x1, double& x2) {
    double delta2 = b * b - 4.0 * a * c;

    if (delta2 < 0.0) {
        return false;
    }

    double delta = sqrt(delta2);

    x1 = (-b + delta) / (2.0 * a);
    x2 = (-b - delta) / (2.0 * a);

    return true;
}

// docs in header
void HandEyeCalibration::estimateHandEyeScrewRefine(
    DualQuaterniond& dq,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs2,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs2,
    ceres::Solver::Summary& summary) {
    Eigen::Matrix4d H = dq.toMatrix();
    double p[7] = {dq.real().w(), dq.real().x(), dq.real().y(), dq.real().z(),
                   H(0, 3),       H(1, 3),       H(2, 3)};

    ceres::Problem problem;
    for (size_t i = 0; i < rvecs1.size(); i++) {
        // ceres deletes the objects allocated here for the user
        ceres::CostFunction* costFunction =
            new ceres::AutoDiffCostFunction<PoseError, 1, 4, 3>(
                new PoseError(rvecs1[i], tvecs1[i], rvecs2[i], tvecs2[i]));

        problem.AddResidualBlock(costFunction, NULL, p, p + 4);
    }

    // ceres deletes the object allocated here for the user
    ceres::LocalParameterization* quaternionParameterization =
        new ceres::QuaternionParameterization;

    problem.SetParameterization(p, quaternionParameterization);

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.jacobi_scaling = true;
    options.max_num_iterations = 500;

    // ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    if (mVerbose) {
        std::cout << summary.BriefReport() << std::endl;
    }

    Eigen::Quaterniond q(p[0], p[1], p[2], p[3]);
    Eigen::Vector3d t;
    t << p[4], p[5], p[6];
    dq = DualQuaterniond(q, t);
}

void HandEyeCalibration::estimateHandEyeScrewRefine(
    DualQuaterniond& dq,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs1,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& rvecs2,
    const std::vector<Eigen::Vector3d,
                      Eigen::aligned_allocator<Eigen::Vector3d>>& tvecs2) {
    Eigen::Matrix4d H = dq.toMatrix();
    double p[7] = {dq.real().w(), dq.real().x(), dq.real().y(), dq.real().z(),
                   H(0, 3),       H(1, 3),       H(2, 3)};
    ceres::Solver::Summary summary;

    ceres::Problem problem;
    for (size_t i = 0; i < rvecs1.size(); i++) {
        // ceres deletes the objects allocated here for the user
        ceres::CostFunction* costFunction =
            new ceres::AutoDiffCostFunction<PoseError, 1, 4, 3>(
                new PoseError(rvecs1[i], tvecs1[i], rvecs2[i], tvecs2[i]));

        problem.AddResidualBlock(costFunction, NULL, p, p + 4);
    }

    // ceres deletes the object allocated here for the user
    ceres::LocalParameterization* quaternionParameterization =
        new ceres::QuaternionParameterization;

    problem.SetParameterization(p, quaternionParameterization);

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.jacobi_scaling = true;
    options.max_num_iterations = 500;

    // ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    if (mVerbose) {
        std::cout << summary.BriefReport() << std::endl;
    }

    Eigen::Quaterniond q(p[0], p[1], p[2], p[3]);
    Eigen::Vector3d t;
    t << p[4], p[5], p[6];
    dq = DualQuaterniond(q, t);
}
}


///////////////////////////////////////////////////////////////////////////////
// df.hpp                                                                    //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under 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_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_DF_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_DF_HPP_ER_2010
#include <boost/mpl/detail/wrapper.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/cells_count.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/common/df_formula.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/lost_df.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/tag.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{
namespace pearson_chisq_statistic{

  	template<typename Keys,typename AccSet>
    long degrees_of_freedom(
        const boost::mpl::detail::wrapper<
            pearson_chisq_statistic::tag::independence_between<Keys>
        >& statistic,
        const AccSet& acc
    )
    {
        namespace ns = contingency_table;
        return pearson_chisq_statistic::degrees_of_freedom<long>(
            ns::cells_count<Keys>( acc ),
            pearson_chisq_statistic::lost_degrees_of_freedom( statistic, acc )
        );
    }

}// pearson_chisq_statistic
}// contingency_table
}// detail
}// statistics
}// boost

#endif


// (C) Copyright Andrew Sutton 2007
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

//[tiernan_girth_circumference
#include <iostream>

#include <boost/graph/directed_graph.hpp>
#include <boost/graph/tiernan_all_cycles.hpp>

#include "helper.hpp"

using namespace std;
using namespace boost;

// Declare the graph type and its vertex and edge types.
typedef directed_graph<> Graph;
typedef graph_traits<Graph>::vertex_descriptor Vertex;
typedef graph_traits<Graph>::edge_descriptor Edge;

int
main(int argc, char *argv[])
{
    // Create the graph and read it from standard input.
    Graph g;
    read_graph(g, cin);

    // Compute the girth and circumference simulataneously
    size_t girth, circ;
    tie(girth, circ) = tiernan_girth_and_circumference(g);

    // Print the result
    cout << "girth: " << girth << endl;
    cout << "circumference: " << circ << endl;

    return 0;
}
//]


#pragma once

#include <Eigen/Sparse>
#include <cassert>
#include <algorithm>
#include <ostream>

template <typename Scalar>
using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

template <typename Scalar>
using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;

namespace detail
{
    template <typename Scalar, typename Index>
    Eigen::VectorXi submatrix_sparsity_pattern(const Eigen::SparseMatrix<Scalar, 0, Index> & matrix,
                                               const std::vector<Index> & reverse_rowmap,
                                               const std::vector<Index> & cols)
    {
        typedef typename Eigen::SparseMatrix<Scalar, 0, Index>::InnerIterator InnerIterator;
        const auto submat_cols = static_cast<Index>(cols.size());

        // Each entry in the vector holds the number of non-zero rows in the column
        Eigen::VectorXi sparsity_pattern(submat_cols);

        for (Index col = 0; col < submat_cols; ++col)
        {
            sparsity_pattern(col) = 0;
            const auto original_col = cols[col];

            for (InnerIterator it(matrix, original_col); it; ++it)
            {
                const auto submatrix_row = reverse_rowmap[it.row()];
                if (submatrix_row >= 0)
                {
                    sparsity_pattern(col) += 1;
                }
            }
        }

        return sparsity_pattern;
    }
}

template <typename Scalar, typename Index>
Eigen::SparseMatrix<Scalar, 0, Index> sparse_submatrix(const Eigen::SparseMatrix<Scalar, 0, Index> & matrix,
                                                       const std::vector<Index> & rows,
                                                       const std::vector<Index> & cols)
{
    typedef typename Eigen::SparseMatrix<Scalar, 0, Index>::InnerIterator InnerIterator;
    assert(std::is_sorted(rows.cbegin(), rows.cend()) && "Row indices must be sorted.");
    assert(std::is_sorted(cols.cbegin(), cols.cend()) && "Column indices must be sorted.");

    // rows and cols map indices from the submatrix into the original matrix. We want to build a reverse map,
    // which is generally not surjective, hence we denote elements that should not be present by -1.
    std::vector<Index> reverse_rowmap(matrix.rows(), -1);

    for (size_t submatrix_row = 0; submatrix_row < rows.size(); ++submatrix_row)
    {
        const auto original_row = rows[submatrix_row];
        reverse_rowmap[original_row] = submatrix_row;
    }

    const auto submat_rows = static_cast<Index>(rows.size());
    const auto submat_cols = static_cast<Index>(cols.size());

    Eigen::SparseMatrix<Scalar, 0, Index> submat(submat_rows, submat_cols);
    if (submat_rows == 0 || submat_cols == 0)
    {
        return submat;
    }

    submat.reserve(detail::submatrix_sparsity_pattern(matrix, reverse_rowmap, cols));

    for (Index col = 0; col < submat.cols(); ++col)
    {
        const auto original_col = cols[col];

        // We implicitly make the assumption here that the original matrix's columns are relatively sparse,
        // which is almost always the case.
        for (InnerIterator it(matrix, original_col); it; ++it)
        {
            const auto submatrix_row = reverse_rowmap[it.row()];
            if (submatrix_row >= 0)
            {
                submat.insert(submatrix_row, col) = it.value();
            }
        }
    }

    submat.makeCompressed();
    return submat;
};

template <typename Scalar, typename Index>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> sparse_submatrix_as_dense(
        const Eigen::SparseMatrix<Scalar, 0, Index> & matrix,
        const std::vector<Index> & rows,
        const std::vector<Index> & cols)
{
    typedef typename Eigen::SparseMatrix<Scalar, 0, Index>::InnerIterator InnerIterator;
    assert(std::is_sorted(rows.cbegin(), rows.cend()) && "Row indices must be sorted.");
    assert(std::is_sorted(cols.cbegin(), cols.cend()) && "Column indices must be sorted.");

    const auto submat_rows = static_cast<Index>(rows.size());
    const auto submat_cols = static_cast<Index>(cols.size());

    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> submat(submat_rows, submat_cols);
    submat.setZero();

    if (submat_rows > 0)
    {
        for (Index col = 0; col < submat.cols(); ++col)
        {
            const auto original_col = cols[col];
            Index current_submat_row = 0;

            InnerIterator it(matrix, original_col);

            for (InnerIterator it(matrix, original_col); it; ++it) {
                while (current_submat_row < submat_rows && it.row() > rows[current_submat_row]) {
                    ++current_submat_row;
                }

                if (current_submat_row < submat_rows && it.row() == rows[current_submat_row]) {
                    submat(current_submat_row, col) = it.value();
                }
            }
        }
    }

    return submat;
};

template <typename Scalar, int Rows, int Cols>
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> submatrix(const Eigen::Matrix<Scalar, Rows, Cols> &matrix,
                                                                const std::vector<int> rows,
                                                                const std::vector<int> cols)
{
    assert(std::is_sorted(rows.cbegin(), rows.cend()) && "Row indices must be sorted");
    assert(std::is_sorted(cols.cbegin(), cols.cend()) && "Col indices must be sorted");

    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> result(rows.size(), cols.size());

    for (size_t j = 0; j < cols.size(); ++j)
    {
        for (size_t i = 0; i < rows.size(); ++i)
        {
            const int row = static_cast<int>(rows[i]);
            const int col = static_cast<int>(cols[j]);
            result(i, j) = matrix(row, col);
        }
    }

    return result;
};


#include "visualizer.h"
#include "mesh.h"
#include "subdiv_evaluator.h"
#include "fitting_functor.h"

#include <Eigen/Eigen>
#include <Open3D/Open3D.h>

#include <iostream>
#include <random>
#include "timer.h"

using Vector2d = Eigen::Vector2d;
using Vector3d = Eigen::Vector3d;

// creates a mesh using given parameters, and evaluates points on the surface
std::vector<Vector3d> evaluate_parametric_mesh(const Vector3d scale, const Vector3d &translation, const std::vector<SurfacePoint> &sps)
{
    Mesh mesh = Mesh::create_sphere();
    mesh.transform(scale, translation);
    SubdivEvaluator evaluator(mesh);
    SurfaceFeatures sf(sps.size());
    evaluator.evaluate_subdiv_surface(mesh.vertices(), sps, sf, false);
    return sf.S;
}

std::vector<SurfacePoint> offset_gt_correspondences(const Mesh &mesh, const std::vector<SurfacePoint> &sps_gt)
{
    std::vector<SurfacePoint> sps{sps_gt};
    SubdivEvaluator evaluator{mesh};

    std::random_device rd;
    std::mt19937 random_generator(rd());
    std::normal_distribution<double> dist_offset(0.0, 3.0);
    for (auto &sp : sps)
    {
        Vector2d du = {dist_offset(random_generator), dist_offset(random_generator)};
        FittingFunctor::update_surface_point(mesh, evaluator, sp.face, sp.u, du);
    }

    return sps;
}

void print_vector_in_single_line(const std::string &param_name, const Eigen::VectorXd &param)
{
    std::cout << param_name << ": [";
    for (int i{0}; i < param.size(); ++i)
    {
        std::cout << param(i);
        if (i != param.size() - 1)
            std::cout << ", ";
    }
    std::cout << "]\n";
}

int main()
{
    // for drawing various geometries in 1 window
    Visualizer visualizer;

    // template mesh
    Mesh template_mesh{Mesh::create_sphere()};
    // visualizer.add_mesh(template_mesh.tri_mesh);

    // groundtruth parameters
    const Vector3d scale_gt{2, 1.5, 1.0};
    const Vector3d translation_gt{1.0, 2.0, 3.0};
    const int n_data{100};
    const std::vector<SurfacePoint> sps_gt = SurfacePoint::generate(n_data, template_mesh.n_triangles());

    // generate data with groundtruth parameters
    std::vector<Vector3d> points_observed = evaluate_parametric_mesh(scale_gt, translation_gt, sps_gt);
    visualizer.add_point_cloud(points_observed, {0, 0, 1});

    // init parameters
    Vector3d scale_init{1., 1., 1.};
    Vector3d translation_init{0.0, 0, 0};
    Mesh mesh{Mesh::create_sphere()};
    mesh.transform(scale_init, translation_init);
    visualizer.add_mesh(mesh.tri_mesh);

    // init correspondences
    std::vector<SurfacePoint> sps_init = offset_gt_correspondences(mesh, sps_gt);
    std::vector<Vector3d> points_init = evaluate_parametric_mesh(scale_init, translation_init, sps_init);
    // visualizer.add_point_cloud(points_init, {1, 0, 0});

    // optimize
    FittingFunctor::InputType params(scale_init, translation_init, sps_init);
    FittingFunctor fitting_functor(std::make_shared<std::vector<Vector3d>>(points_observed), mesh, template_mesh, &visualizer);
    Eigen::LevenbergMarquardt<FittingFunctor> lm(fitting_functor);
    lm.setVerbose(true);
    lm.setMaxfev(10);
    Timer timer;
    Eigen::LevenbergMarquardtSpace::Status info = lm.minimize(params);
    std::cout << "\nOptimization took " << timer.elapsed() << "s\n";

    // log
    std::cout << "\nGroundtruth\n";
    print_vector_in_single_line("Scale", scale_gt);
    print_vector_in_single_line("Translation", translation_gt);

    std::cout << "\nInitial\n";
    print_vector_in_single_line("Scale", scale_init);
    print_vector_in_single_line("Translation", translation_init);

    std::cout << "\nOptimized\n";
    print_vector_in_single_line("Scale", params.scale);
    print_vector_in_single_line("Translation", params.translation);

    // hold visualizer
    std::cout << "\nPress `q` to close visualizer window\n";
    visualizer.run();

    return 0;
}


//
// $Id$
//
// Original author: Matt Chambers <matt.chambers .@. vanderbilt.edu>
// Copyright 2011 Vanderbilt University
//
// Licensed under the Code Project Open License, Version 1.02 (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.codeproject.com/info/cpol10.aspx
//
// 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 "pwiz/utility/misc/unit.hpp"
#include "pwiz/utility/misc/Std.hpp"
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/framework/accumulator_set.hpp>
#include "percentile.hpp"

using namespace pwiz::util;
using namespace boost::accumulators;

void test()
{
    // tested at http://www.wessa.net

    accumulator_set<double, stats<tag::percentile> > acc;
    const double epsilon = 1e-6;

    acc(10);
    unit_assert_equal(10, percentile(acc, percentile_number = 25), epsilon);
    unit_assert_equal(10, percentile(acc, percentile_number = 50), epsilon);
    unit_assert_equal(10, percentile(acc, percentile_number = 75), epsilon);

    acc(30);
    unit_assert_equal(15, percentile(acc, percentile_number = 25), epsilon);
    unit_assert_equal(20, percentile(acc, percentile_number = 50), epsilon);
    unit_assert_equal(25, percentile(acc, percentile_number = 75), epsilon);

    acc(20);
    unit_assert_equal(15, percentile(acc, percentile_number = 25), epsilon);
    unit_assert_equal(20, percentile(acc, percentile_number = 50), epsilon);
    unit_assert_equal(25, percentile(acc, percentile_number = 75), epsilon);

    acc(40);
    unit_assert_equal(17.5, percentile(acc, percentile_number = 25), epsilon);
    unit_assert_equal(25, percentile(acc, percentile_number = 50), epsilon);
    unit_assert_equal(32.5, percentile(acc, percentile_number = 75), epsilon);

    acc(50);
    unit_assert_equal(20, percentile(acc, percentile_number = 25), epsilon);
    unit_assert_equal(30, percentile(acc, percentile_number = 50), epsilon);
    unit_assert_equal(40, percentile(acc, percentile_number = 75), epsilon);

    acc(60);
    unit_assert_equal(22.5, percentile(acc, percentile_number = 25), epsilon);
    unit_assert_equal(35, percentile(acc, percentile_number = 50), epsilon);
    unit_assert_equal(47.5, percentile(acc, percentile_number = 75), epsilon);

    acc(80);
    unit_assert_equal(25, percentile(acc, percentile_number = 25), epsilon);
    unit_assert_equal(40, percentile(acc, percentile_number = 50), epsilon);
    unit_assert_equal(55, percentile(acc, percentile_number = 75), epsilon);

    acc(35);
    unit_assert_equal(27.5, percentile(acc, percentile_number = 25), epsilon);
    unit_assert_equal(37.5, percentile(acc, percentile_number = 50), epsilon);
    unit_assert_equal(52.5, percentile(acc, percentile_number = 75), epsilon);

    acc(77); acc(88); acc(99); acc(100);
    unit_assert_equal(21, percentile(acc, percentile_number = 10), epsilon);
    unit_assert_equal(31, percentile(acc, percentile_number = 20), epsilon);
    unit_assert_equal(36.5, percentile(acc, percentile_number = 30), epsilon);
    unit_assert_equal(44, percentile(acc, percentile_number = 40), epsilon);
    unit_assert_equal(55, percentile(acc, percentile_number = 50), epsilon);
    unit_assert_equal(70.2, percentile(acc, percentile_number = 60), epsilon);
    unit_assert_equal(79.1, percentile(acc, percentile_number = 70), epsilon);
    unit_assert_equal(86.4, percentile(acc, percentile_number = 80), epsilon);
    unit_assert_equal(97.9, percentile(acc, percentile_number = 90), epsilon);
}

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

    try
    {
        test();
    }
    catch (exception& e)
    {
        TEST_FAILED(e.what())
    }
    catch (...)
    {
        TEST_FAILED("Caught unknown exception.")
    }

    TEST_EPILOG
}


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example mtl4-with-viennacl.cpp
*
*   This tutorial shows how data can be directly transferred from the MTL4 Library to ViennaCL objects using the built-in convenience wrappers.
*
*   The first step is to include the necessary headers and activate the MTL4 convenience functions in ViennaCL:
**/

// System headers
#include <iostream>

// MTL4 headers
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

// Must be set prior to any ViennaCL includes if you want to use ViennaCL algorithms on MTL4 objects
#define VIENNACL_WITH_MTL4 1

// ViennaCL headers
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/compressed_matrix.hpp"

#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"

// Some helper functions for this tutorial:
#include "Random.hpp"
#include "vector-io.hpp"
#include "../benchmarks/benchmark-utils.hpp"

/**
*    The following function contains the main code for this tutorial.
*    It consists of the following steps:
*      - Creates MTL4 matrices and vectors
*      - Initializes them with data
*      - Create ViennaCL objects
*      - Copy them over to the respective ViennaCL objects
*      - Compute matrix-vector products in both MTL4 and ViennaCL and compare results.
*
**/
template<typename ScalarType>
void run_tutorial()
{
  typedef mtl::dense2D<ScalarType>        MTL4DenseMatrix;
  typedef mtl::compressed2D<ScalarType>   MTL4SparseMatrix;

  /**
  * Create and fill dense matrices from the MTL4 library:
  **/
  mtl::dense2D<ScalarType>   mtl4_densemat(5, 5);
  mtl::dense2D<ScalarType>   mtl4_densemat2(5, 5);
  mtl4_densemat(0,0) = 2.0;   mtl4_densemat(0,1) = -1.0;
  mtl4_densemat(1,0) = -1.0;  mtl4_densemat(1,1) =  2.0;  mtl4_densemat(1,2) = -1.0;
  mtl4_densemat(2,1) = -1.0;  mtl4_densemat(2,2) = -1.0;  mtl4_densemat(2,3) = -1.0;
  mtl4_densemat(3,2) = -1.0;  mtl4_densemat(3,3) =  2.0;  mtl4_densemat(3,4) = -1.0;
                              mtl4_densemat(4,4) = -1.0;  mtl4_densemat(4,4) = -1.0;

  /**
  * Create and fill sparse matrices from the MTL4 library:
  **/
  MTL4SparseMatrix mtl4_sparsemat;
  set_to_zero(mtl4_sparsemat);
  mtl4_sparsemat.change_dim(5, 5);

  MTL4SparseMatrix mtl4_sparsemat2;
  set_to_zero(mtl4_sparsemat2);
  mtl4_sparsemat2.change_dim(5, 5);

  {
    mtl::matrix::inserter< MTL4SparseMatrix >  ins(mtl4_sparsemat);
    typedef typename mtl::Collection<MTL4SparseMatrix>::value_type  ValueType;
    ins(0,0) <<  ValueType(2.0);   ins(0,1) << ValueType(-1.0);
    ins(1,1) <<  ValueType(2.0);   ins(1,2) << ValueType(-1.0);
    ins(2,2) << ValueType(-1.0);   ins(2,3) << ValueType(-1.0);
    ins(3,3) <<  ValueType(2.0);   ins(3,4) << ValueType(-1.0);
    ins(4,4) << ValueType(-1.0);
  }

  /**
  * Create and fill a few vectors from the MTL4 library:
  **/
  mtl::dense_vector<ScalarType> mtl4_rhs(5, 0.0);
  mtl::dense_vector<ScalarType> mtl4_result(5, 0.0);
  mtl::dense_vector<ScalarType> mtl4_temp(5, 0.0);

  mtl4_rhs(0) = 10.0;
  mtl4_rhs(1) = 11.0;
  mtl4_rhs(2) = 12.0;
  mtl4_rhs(3) = 13.0;
  mtl4_rhs(4) = 14.0;

  /**
  * Create the corresponding ViennaCL objects:
  **/
  viennacl::vector<ScalarType> vcl_rhs(5);
  viennacl::vector<ScalarType> vcl_result(5);
  viennacl::matrix<ScalarType> vcl_densemat(5, 5);
  viennacl::compressed_matrix<ScalarType> vcl_sparsemat(5, 5);

  /**
  * Directly copy the MTL4 objects to ViennaCL objects
  **/
  viennacl::copy(&(mtl4_rhs[0]), &(mtl4_rhs[0]) + 5, vcl_rhs.begin());  //method 1: via iterator interface (cf. std::copy())
  viennacl::copy(mtl4_rhs, vcl_rhs);  //method 2: via built-in wrappers (convenience layer)

  viennacl::copy(mtl4_densemat, vcl_densemat);
  viennacl::copy(mtl4_sparsemat, vcl_sparsemat);

  // For completeness: Copy matrices from ViennaCL back to Eigen:
  viennacl::copy(vcl_densemat, mtl4_densemat2);
  viennacl::copy(vcl_sparsemat, mtl4_sparsemat2);

  /**
  * Run dense matrix-vector products and compare results:
  **/
  mtl4_result = mtl4_densemat * mtl4_rhs;
  vcl_result = viennacl::linalg::prod(vcl_densemat, vcl_rhs);
  viennacl::copy(vcl_result, mtl4_temp);
  mtl4_result -= mtl4_temp;
  std::cout << "Difference for dense matrix-vector product: " << mtl::two_norm(mtl4_result) << std::endl;
  mtl4_result = mtl4_densemat2 * mtl4_rhs - mtl4_temp;
  std::cout << "Difference for dense matrix-vector product (MTL4->ViennaCL->MTL4): "
            << mtl::two_norm(mtl4_result) << std::endl;

  /**
  * Run sparse matrix-vector products and compare results:
  **/
  mtl4_result = mtl4_sparsemat * mtl4_rhs;
  vcl_result = viennacl::linalg::prod(vcl_sparsemat, vcl_rhs);
  viennacl::copy(vcl_result, mtl4_temp);
  mtl4_result -= mtl4_temp;
  std::cout << "Difference for sparse matrix-vector product: " << mtl::two_norm(mtl4_result) << std::endl;
  mtl4_result = mtl4_sparsemat2 * mtl4_rhs - mtl4_temp;
  std::cout << "Difference for sparse matrix-vector product (MTL4->ViennaCL->MTL4): "
            << mtl::two_norm(mtl4_result) << std::endl;

}

/**
*   In the main() routine we only call the worker function defined above with both single and double precision arithmetic.
**/
int main(int, char *[])
{
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "## Single precision" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  run_tutorial<float>();

#ifdef VIENNACL_HAVE_OPENCL
  if ( viennacl::ocl::current_device().double_support() )
#endif
  {
    std::cout << "----------------------------------------------" << std::endl;
    std::cout << "## Double precision" << std::endl;
    std::cout << "----------------------------------------------" << std::endl;
    run_tutorial<double>();
  }

  /**
  *   That's it. Print a success message and exit.
  **/
  std::cout << std::endl;
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;
  std::cout << std::endl;
}


#include "ale/Rotation.h"

#include <exception>

#include <Eigen/Geometry>

#include "ale/InterpUtils.h"

namespace ale {

///////////////////////////////////////////////////////////////////////////////
// Helper Functions
///////////////////////////////////////////////////////////////////////////////

  // Helper function to convert an axis number into a unit Eigen vector down that axis.
  Eigen::Vector3d axis(int axisIndex) {
    switch (axisIndex) {
      case 0:
        return Eigen::Vector3d::UnitX();
        break;
      case 1:
        return Eigen::Vector3d::UnitY();
        break;
      case 2:
        return Eigen::Vector3d::UnitZ();
        break;
      default:
        throw std::invalid_argument("Axis index must be 0, 1, or 2.");
    }
  }

  /**
   * Create the skew symmetric matrix used when computing the derivative of a
   * rotation matrix.
   *
   * This is actually the transpose of the skew AV matrix because we define AV
   * as the AV from the destination to the source. This matches how NAIF
   * defines AV.
   */
  Eigen::Quaterniond::Matrix3 avSkewMatrix(const Vec3d& av) {
    Eigen::Quaterniond::Matrix3 avMat;
    avMat <<  0.0,    av.z, -av.y,
             -av.z,  0.0,    av.x,
              av.y, -av.x,  0.0;
    return avMat;
  }

  ///////////////////////////////////////////////////////////////////////////////
  // Rotation Impl class
  ///////////////////////////////////////////////////////////////////////////////

  // Internal representation of the rotation as an Eigen Double Quaternion
  class Rotation::Impl {
    public:
      Impl() : quat(Eigen::Quaterniond::Identity()) { }

      Impl(double w, double x, double y, double z) : quat(w, x, y, z) { }

      Impl(const std::vector<double>& matrix) {
        if (matrix.size() != 9) {
          throw std::invalid_argument("Rotation matrix must be 3 by 3.");
        }
        quat = Eigen::Quaterniond(Eigen::Quaterniond::Matrix3(matrix.data()));
      }

      Impl(const std::vector<double>& angles, const std::vector<int>& axes) {
        if (angles.empty() || axes.empty()) {
          throw std::invalid_argument("Angles and axes must be non-empty.");
        }
        if (angles.size() != axes.size()) {
          throw std::invalid_argument("Number of angles and axes must be equal.");
        }
        quat = Eigen::Quaterniond::Identity();

        for (size_t i = 0; i < angles.size(); i++) {
          quat *= Eigen::Quaterniond(Eigen::AngleAxisd(angles[i], axis(axes[i])));
        }
      }

      Impl(const std::vector<double>& axis, double theta) {
        if (axis.size() != 3) {
          throw std::invalid_argument("Rotation axis must have 3 elements.");
        }
        Eigen::Vector3d eigenAxis((double *) axis.data());
        quat = Eigen::Quaterniond(Eigen::AngleAxisd(theta, eigenAxis.normalized()));
      }

      Eigen::Quaterniond quat;
  };

  ///////////////////////////////////////////////////////////////////////////////
  // Rotation Class
  ///////////////////////////////////////////////////////////////////////////////

  Rotation::Rotation() :
        m_impl(new Impl()) { }

  Rotation::Rotation(double w, double x, double y, double z) :
        m_impl(new Impl(w, x, y, z)) { }

  Rotation::Rotation(const std::vector<double>& matrix) :
        m_impl(new Impl(matrix)) { }

  Rotation::Rotation(const std::vector<double>& angles, const std::vector<int>& axes) :
        m_impl(new Impl(angles, axes)) { }

  Rotation::Rotation(const std::vector<double>& axis, double theta) :
        m_impl(new Impl(axis, theta)) { }

  Rotation::~Rotation() = default;

  Rotation::Rotation(Rotation && other) noexcept = default;

  Rotation& Rotation::operator=(Rotation && other) noexcept = default;

  // unique_ptr doesn't have a copy constructor so we have to define one
  Rotation::Rotation(const Rotation& other) : m_impl(new Impl(*other.m_impl)) { }

  // unique_ptr doesn't have an assignment operator so we have to define one
  Rotation& Rotation::operator=(const Rotation& other) {
    if (this != &other) {
      m_impl.reset(new Impl(*other.m_impl));
    }
    return *this;
  }

  std::vector<double> Rotation::toQuaternion() const {
    Eigen::Quaterniond normalized = m_impl->quat.normalized();
    return {normalized.w(), normalized.x(), normalized.y(), normalized.z()};
  }

  std::vector<double> Rotation::toRotationMatrix() const {
    Eigen::Quaterniond::RotationMatrixType mat = m_impl->quat.toRotationMatrix();
    return std::vector<double>(mat.data(), mat.data() + mat.size());
  }

  std::vector<double> Rotation::toStateRotationMatrix(const Vec3d &av) const {
    Eigen::Quaterniond::Matrix3 rotMat = m_impl->quat.toRotationMatrix();
    Eigen::Quaterniond::Matrix3 avMat = avSkewMatrix(av);
    Eigen::Quaterniond::Matrix3 dtMat = rotMat * avMat;
    return {rotMat(0,0), rotMat(0,1), rotMat(0,2), 0.0,         0.0,         0.0,
            rotMat(1,0), rotMat(1,1), rotMat(1,2), 0.0,         0.0,         0.0,
            rotMat(2,0), rotMat(2,1), rotMat(2,2), 0.0,         0.0,         0.0,
            dtMat(0,0),  dtMat(0,1),  dtMat(0,2),  rotMat(0,0), rotMat(0,1), rotMat(0,2),
            dtMat(1,0),  dtMat(1,1),  dtMat(1,2),  rotMat(1,0), rotMat(1,1), rotMat(1,2),
            dtMat(2,0),  dtMat(2,1),  dtMat(2,2),  rotMat(2,0), rotMat(2,1), rotMat(2,2)};
  }

  std::vector<double> Rotation::toEuler(const std::vector<int>& axes) const {
    if (axes.size() != 3) {
      throw std::invalid_argument("Must have 3 axes to convert to Euler angles.");
    }
    if (axes[0] < 0 || axes[0] > 2 ||
        axes[1] < 0 || axes[1] > 2 ||
        axes[2] < 0 || axes[2] > 2) {
      throw std::invalid_argument("Invalid axis number.");
    }
    Eigen::Vector3d angles = m_impl->quat.toRotationMatrix().eulerAngles(
          axes[0],
          axes[1],
          axes[2]);
    return std::vector<double>(angles.data(), angles.data() + angles.size());
  }

  std::pair<std::vector<double>, double> Rotation::toAxisAngle() const {
    Eigen::AngleAxisd eigenAxisAngle(m_impl->quat);
    std::pair<std::vector<double>, double> axisAngle;
    axisAngle.first = std::vector<double>(
          eigenAxisAngle.axis().data(),
          eigenAxisAngle.axis().data() + eigenAxisAngle.axis().size()
    );
    axisAngle.second = eigenAxisAngle.angle();
    return axisAngle;
  }

  Vec3d Rotation::operator()(const Vec3d &vector) const {
    Eigen::Vector3d eigenVector(vector.x, vector.y, vector.z);
    Eigen::Vector3d rotatedVector = m_impl->quat._transformVector(eigenVector);
    std::vector<double> tempVec = std::vector<double>(rotatedVector.data(), rotatedVector.data() + rotatedVector.size());
    return Vec3d(tempVec);
  }

  State Rotation::operator()(
        const State& state,
        const Vec3d& av
  ) const {
    Vec3d position = state.position;
    Vec3d velocity = state.velocity;

    Eigen::Vector3d positionVector(position.x, position.y, position.z);
    Eigen::Vector3d velocityVector(velocity.x, velocity.y, velocity.z);
    Eigen::Quaterniond::Matrix3 rotMat = m_impl->quat.toRotationMatrix();
    Eigen::Quaterniond::Matrix3 avMat = avSkewMatrix(av);
    Eigen::Quaterniond::Matrix3 rotationDerivative = rotMat * avMat;
    Eigen::Vector3d rotatedPosition = rotMat * positionVector;
    Eigen::Vector3d rotatedVelocity = rotMat * velocityVector + rotationDerivative * positionVector;

    return State({rotatedPosition(0), rotatedPosition(1), rotatedPosition(2),
                  rotatedVelocity(0), rotatedVelocity(1), rotatedVelocity(2)});
  }

  Rotation Rotation::inverse() const {
    Eigen::Quaterniond inverseQuat = m_impl->quat.inverse();
    return Rotation(inverseQuat.w(), inverseQuat.x(), inverseQuat.y(), inverseQuat.z());
  }

  Rotation Rotation::operator*(const Rotation& rightRotation) const {
    Eigen::Quaterniond combinedQuat = m_impl->quat * rightRotation.m_impl->quat;
    return Rotation(combinedQuat.w(), combinedQuat.x(), combinedQuat.y(), combinedQuat.z());
  }

  Rotation Rotation::interpolate(
        const Rotation& nextRotation,
        double t,
        RotationInterpolation interpType
  ) const {
    Eigen::Quaterniond interpQuat;
    switch (interpType) {
      case SLERP:
        interpQuat = m_impl->quat.slerp(t, nextRotation.m_impl->quat);
        break;
      case NLERP:
        interpQuat = Eigen::Quaterniond(
              linearInterpolate(m_impl->quat.w(), nextRotation.m_impl->quat.w(), t),
              linearInterpolate(m_impl->quat.x(), nextRotation.m_impl->quat.x(), t),
              linearInterpolate(m_impl->quat.y(), nextRotation.m_impl->quat.y(), t),
              linearInterpolate(m_impl->quat.z(), nextRotation.m_impl->quat.z(), t)
        );
        interpQuat.normalize();
        break;
      default:
        throw std::invalid_argument("Unsupported rotation interpolation type.");
        break;
    }
    return Rotation(interpQuat.w(), interpQuat.x(), interpQuat.y(), interpQuat.z());
  }

}


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

void
SavePPM(const char*filename)
{
    cube c=randi<cube>(100,100,3, distr_param(0,255));
    c.save(filename,ppm_binary);
}


// This example compiles with C++11.
// C++11 and higher don't need the StdVector include (as mentioned inside the
// include itself).
//#include <Eigen/StdVector>
// If we use C++17 there is no need to take care of memory alignment:
// https://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html
#include <pico_toolshed/scoped_timer.hpp>
#include <pico_tree/eigen.hpp>
#include <pico_tree/kd_tree.hpp>

// Important! The Eigen example is not a performance benchmark. So don't take
// the "elapsed time" numbers too seriously.

using Index = int;

template <typename Point>
using PointsMapCm = Eigen::Map<
    Eigen::Matrix<
        typename Point::Scalar,
        Point::RowsAtCompileTime,
        Eigen::Dynamic>,
    Eigen::AlignedMax>;
// The alignment used by Eigen equals Eigen::AlignedMax. Note that Eigen can
// look at the data pointer to know if it is properly aligned.

template <typename Point>
using PointsMapRm = Eigen::Map<
    Eigen::Matrix<
        typename Point::Scalar,
        Eigen::Dynamic,
        Point::ColsAtCompileTime,
        Eigen::RowMajor>,
    Eigen::AlignedMax>;

std::size_t const kRunCount = 1024 * 1024;
int const kNumPoints = 1024 * 1024 * 2;
float const kArea = 1000.0;
Index const kMaxLeafCount = 16;

// Certain fixed size matrices require us to use aligned memory.
// https://eigen.tuxfamily.org/dox-devel/group__TopicFixedSizeVectorizable.html
template <typename PointX>
std::vector<PointX, Eigen::aligned_allocator<PointX>> GenerateRandomEigenN(
    int n, typename PointX::Scalar size) {
  std::vector<PointX, Eigen::aligned_allocator<PointX>> random(n);
  for (auto& p : random) {
    p = PointX::Random() * size / typename PointX::Scalar(2.0);
  }

  return random;
}

// Creates a KdTree from a vector of Eigen::VectorX and searches for nearest
// neighbors.
void BasicVector() {
  using PointX = Eigen::Vector2f;
  using Scalar = typename PointX::Scalar;

  // Including <pico_tree/eigen.hpp> provides support for Eigen types with
  // std::vector.
  pico_tree::KdTree<pico_tree::StdTraits<
      std::vector<PointX, Eigen::aligned_allocator<PointX>>>>
      tree(GenerateRandomEigenN<PointX>(kNumPoints, kArea), kMaxLeafCount);

  PointX p = PointX::Random() * kArea / Scalar(2.0);

  pico_tree::Neighbor<Index, Scalar> nn;
  ScopedTimer t("pico_tree eigen vector", kRunCount);
  for (std::size_t i = 0; i < kRunCount; ++i) {
    tree.SearchNn(p, &nn);
  }
}

// Creates a KdTree from an Eigen::Matrix<> and searches for nearest neighbors.
void BasicMatrix() {
  using Scalar = typename Eigen::Matrix3Xf::Scalar;
  constexpr int Dim = Eigen::Matrix3Xf::RowsAtCompileTime;

  Eigen::Vector3f p = Eigen::Vector3f::Random() * kArea / Scalar(2.0);

  // The KdTree takes the matrix by value. Prevent a copy by:
  // * Using a move.
  // * Creating an Eigen::Map<>.
  // * Wrap with an std::reference_wrapper<>.
  {
    pico_tree::KdTree<pico_tree::EigenTraits<Eigen::Matrix3Xf>> tree(
        Eigen::Matrix3Xf::Random(Dim, kNumPoints) * kArea / Scalar(2.0),
        kMaxLeafCount);

    pico_tree::Neighbor<Index, Scalar> nn;
    ScopedTimer t("pico_tree eigen val", kRunCount);
    for (std::size_t i = 0; i < kRunCount; ++i) {
      tree.SearchNn(p, &nn);
    }
  }

  {
    Eigen::Matrix3Xf matrix =
        Eigen::Matrix3Xf::Random(Dim, kNumPoints) * kArea / Scalar(2.0);

    pico_tree::KdTree<
        pico_tree::EigenTraits<std::reference_wrapper<Eigen::Matrix3Xf>>>
        tree(matrix, kMaxLeafCount);

    pico_tree::Neighbor<Index, Scalar> nn;
    ScopedTimer t("pico_tree eigen ref", kRunCount);
    for (std::size_t i = 0; i < kRunCount; ++i) {
      tree.SearchNn(p, &nn);
    }
  }
}

// Creates a KdTree from a col-major matrix. The matrix maps an
// std::vector<Eigen::Vector3f>.
void VectorMapColMajor() {
  using PointX = Eigen::Vector3f;
  using Scalar = typename PointX::Scalar;
  constexpr int Dim = PointX::RowsAtCompileTime;
  using Map = PointsMapCm<PointX>;

  auto points = GenerateRandomEigenN<PointX>(kNumPoints, kArea);
  PointX p = PointX::Random() * kArea / Scalar(2.0);

  std::cout << "Eigen RowMajor: " << Map::IsRowMajor << std::endl;
  {
    pico_tree::KdTree<pico_tree::EigenTraits<Map>> tree(
        Map(points.data()->data(), Dim, points.size()), kMaxLeafCount);

    std::vector<pico_tree::Neighbor<Index, Scalar>> knn;
    ScopedTimer t("pico_tree deflt l2", kRunCount);
    for (std::size_t i = 0; i < kRunCount; ++i) {
      tree.SearchKnn(p, 1, &knn);
    }
  }
}

// Creates a KdTree from a row-major matrix. The matrix maps an
// std::vector<Eigen::RowVector3f>.
void VectorMapRowMajor() {
  using PointX = Eigen::RowVector3f;
  using Scalar = typename PointX::Scalar;
  constexpr int Dim = PointX::ColsAtCompileTime;
  using Map = PointsMapRm<PointX>;

  auto points = GenerateRandomEigenN<PointX>(kNumPoints, kArea);
  PointX p = PointX::Random() * kArea / Scalar(2.0);

  std::cout << "Eigen RowMajor: " << PointX::IsRowMajor << std::endl;

  {
    pico_tree::KdTree<pico_tree::EigenTraits<Map>> tree(
        Map(points.data()->data(), points.size(), Dim), kMaxLeafCount);

    std::vector<pico_tree::Neighbor<Index, Scalar>> knn;
    ScopedTimer t("pico_tree deflt l2", kRunCount);
    for (std::size_t i = 0; i < kRunCount; ++i) {
      tree.SearchKnn(p, 1, &knn);
    }
  }
}

// The Metrics demo shows how it can be beneficial to use the metrics supplied
// by the <pico_tree/eigen.hpp> header.
//
// Suppose we want to use a KdTree with a spatial dimension of 3 using floats as
// a scalar. In this case we can use Eigen::Vector3f as the data type. However,
// Eigen::Vector3f doesn't benefit from vectorization. With Eigen::Vector4f we
// can, but in this case we have one dimension too many!
//
// Luckily, it is possible use a different dimension for both the points and the
// KdTree, but some care needs to be taken:
// * The default Metrics don't make explicit use of vectorization (perhaps
// implicitly through optimization by the compiler) but the Eigen based Metrics
// may do so.
// * The extra coordinate of Eigen::Vector4f must be set 0 so it doesn't
// influence any of the distance calculations. E.g., the squared distance uses a
// dot product.
//
// See also:
// http://eigen.tuxfamily.org/index.php?title=UsingVector4fForVector3fOperations
void Metrics() {
  // Eigen::Vector4f requires aligned memory.
  using PointX = Eigen::Vector4f;
  using Scalar = typename PointX::Scalar;
  using Map = PointsMapCm<PointX>;
  // Tell the KdTree to use a spatial dimension of 3 instead of 4.
  constexpr int Dim = PointX::RowsAtCompileTime - 1;

  auto points = GenerateRandomEigenN<PointX>(kNumPoints, kArea);
  // The Eigen::Map uses the dimension of 4.
  Map map(points.data()->data(), PointX::RowsAtCompileTime, points.size());
  // Set the last row (4th coordinate) to 0.
  map.bottomRows<1>().setZero();

  PointX p = PointX::Random() * kArea / Scalar(2.0);
  // Again, set the last row (4th coordinate) to 0.
  p.w() = Scalar(0.0);

  std::cout << "Eigen Metrics: " << std::endl;

  {
    // Using an std::reference_wrapper prevents a copy.
    using Traits = pico_tree::StdTraits<std::reference_wrapper<
        std::vector<PointX, Eigen::aligned_allocator<PointX>>>>;

    pico_tree::KdTree<
        Traits,
        pico_tree::EigenL2Squared<Scalar>,
        pico_tree::SplitterSlidingMidpoint<Traits>,
        Dim>
        tree(points, kMaxLeafCount);

    std::vector<pico_tree::Neighbor<Index, Scalar>> knn;
    ScopedTimer t("pico_tree eigen l2", kRunCount);
    for (std::size_t i = 0; i < kRunCount; ++i) {
      tree.SearchKnn(p, 1, &knn);
    }
  }

  {
    // Using an Eigen::Map prevents a copy.
    using Traits = pico_tree::EigenTraits<Map>;

    pico_tree::KdTree<
        Traits,
        pico_tree::EigenL1<Scalar>,
        pico_tree::SplitterSlidingMidpoint<Traits>,
        Dim>
        tree(map, kMaxLeafCount);

    std::vector<pico_tree::Neighbor<Index, Scalar>> knn;
    ScopedTimer t("pico_tree eigen l1", kRunCount);
    for (std::size_t i = 0; i < kRunCount; ++i) {
      tree.SearchKnn(p, 1, &knn);
    }
  }
}

int main() {
  BasicVector();
  BasicMatrix();
  VectorMapColMajor();
  VectorMapRowMajor();
  Metrics();
  return 0;
}


//
// SPDX-License-Identifier: BSD-3-Clause
// Copyright Contributors to the OpenEXR Project.
//

// clang-format off

#include <Python.h>
#include <boost/python.hpp>
#include <boost/format.hpp>
#include <boost/python/make_constructor.hpp>
#include "PyImath.h"
#include "PyImathMathExc.h"
#include "PyImathFixedArray.h"
#include "PyImathRandom.h"
#include "PyImathDecorators.h"

namespace PyImath{
using namespace boost::python;

template <class Rand, class T>
static T
nextf2 (Rand &rand, T min, T max)
{
    MATH_EXC_ON;
    return rand.nextf(min, max);
}

template <class Rand>
static float
nextGauss (Rand &rand)
{
    MATH_EXC_ON;
    return gaussRand(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec3<T> nextGaussSphere(Rand &rand, const IMATH_NAMESPACE::Vec3<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::gaussSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
}
template <class T, class Rand>
static IMATH_NAMESPACE::Vec2<T> nextGaussSphere(Rand &rand, const IMATH_NAMESPACE::Vec2<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::gaussSphereRand<IMATH_NAMESPACE::Vec2<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec3<T> nextHollowSphere(Rand &rand, const IMATH_NAMESPACE::Vec3<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::hollowSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec2<T> nextHollowSphere(Rand &rand, const IMATH_NAMESPACE::Vec2<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::hollowSphereRand<IMATH_NAMESPACE::Vec2<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec3<T> nextSolidSphere(Rand &rand, const IMATH_NAMESPACE::Vec3<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::solidSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
}

template <class T, class Rand>
static IMATH_NAMESPACE::Vec2<T> nextSolidSphere(Rand &rand, const IMATH_NAMESPACE::Vec2<T> &v)
{
    MATH_EXC_ON;
    return IMATH_NAMESPACE::solidSphereRand<IMATH_NAMESPACE::Vec2<T>,Rand>(rand);
}

template <class Rand>
static Rand *Rand_constructor1(unsigned long int seed)
{
    return new Rand(seed);
}

template <class Rand>
static Rand *Rand_constructor2(Rand rand)
{
    Rand *r = new Rand();
    *r = rand;

    return r;
}

template <class T, class Rand>
static PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >
hollowSphereRand(Rand &rand, int num)
{
    MATH_EXC_ON;
    PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >  retval(num);
    for (int i=0; i<num; ++i) {
        retval[i] = IMATH_NAMESPACE::hollowSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
    }
    return retval;
}

template <class T, class Rand>
static PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >
solidSphereRand(Rand &rand, int num)
{
    MATH_EXC_ON;
    PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >  retval(num);
    for (int i=0; i<num; ++i) {
        retval[i] = IMATH_NAMESPACE::solidSphereRand<IMATH_NAMESPACE::Vec3<T>,Rand>(rand);
    }
    return retval;
}

PYIMATH_EXPORT
class_<IMATH_NAMESPACE::Rand32>
register_Rand32()
{
    float (IMATH_NAMESPACE::Rand32::*nextf1)(void) = &IMATH_NAMESPACE::Rand32::nextf;

    IMATH_NAMESPACE::Vec3<float> (*nextGaussSphere1)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec3<double> (*nextGaussSphere2)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<float> (*nextGaussSphere3)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<double> (*nextGaussSphere4)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand32>;

    IMATH_NAMESPACE::Vec3<float> (*nextHollowSphere1)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec3<double> (*nextHollowSphere2)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<float> (*nextHollowSphere3)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<double> (*nextHollowSphere4)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand32>;

    IMATH_NAMESPACE::Vec3<float> (*nextSolidSphere1)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec3<double> (*nextSolidSphere2)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<float> (*nextSolidSphere3)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand32>;
    IMATH_NAMESPACE::Vec2<double> (*nextSolidSphere4)(IMATH_NAMESPACE::Rand32 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand32>;

    class_< IMATH_NAMESPACE::Rand32 > rand32_class("Rand32");
    rand32_class
        .def(init<>("default construction"))
        .def("__init__", make_constructor(Rand_constructor1<IMATH_NAMESPACE::Rand32>))
        .def("__init__", make_constructor(Rand_constructor2<IMATH_NAMESPACE::Rand32>))
        .def("init", &IMATH_NAMESPACE::Rand32::init,
             "r.init(i) -- initialize with integer "
			 "seed i")

        .def("nexti", &IMATH_NAMESPACE::Rand32::nexti,
        	 "r.nexti() -- return the next integer "
			 "value in the uniformly-distributed "
			 "sequence")
        .def("nextf", nextf1,
        	 "r.nextf() -- return the next floating-point "
			 "value in the uniformly-distributed "
			 "sequence\n"

        	 "r.nextf(float, float) -- return the next floating-point "
			 "value in the uniformly-distributed "
			 "sequence")
        .def("nextf", &nextf2 <IMATH_NAMESPACE::Rand32, float>)

        .def("nextb", &IMATH_NAMESPACE::Rand32::nextb,
	 	     "r.nextb() -- return the next boolean "
			 "value in the uniformly-distributed "
			 "sequence")

        .def("nextGauss", &nextGauss<IMATH_NAMESPACE::Rand32>,
        	 "r.nextGauss() -- returns the next "
			 "floating-point value in the normally "
			 "(Gaussian) distributed sequence")

        .def("nextGaussSphere", nextGaussSphere1,
	 		 "r.nextGaussSphere(v) -- returns the next "
			 "point whose distance from the origin "
			 "has a normal (Gaussian) distribution with "
			 "mean 0 and variance 1.  The vector "
			 "argument, v, specifies the dimension "
			 "and number type.")
        .def("nextGaussSphere", nextGaussSphere2)
        .def("nextGaussSphere", nextGaussSphere3)
        .def("nextGaussSphere", nextGaussSphere4)

        .def("nextHollowSphere", nextHollowSphere1,
        	 "r.nextHollowSphere(v) -- return the next "
	 		 "point uniformly distributed on the surface "
	 		 "of a sphere of radius 1 centered at the "
	 		 "origin.  The vector argument, v, specifies "
			 "the dimension and number type.")
        .def("nextHollowSphere", nextHollowSphere2)
        .def("nextHollowSphere", nextHollowSphere3)
        .def("nextHollowSphere", nextHollowSphere4)

        .def("nextSolidSphere", nextSolidSphere1,
        	 "r.nextSolidSphere(v) -- return the next "
			 "point uniformly distributed in a sphere "
			 "of radius 1 centered at the origin.  The "
			 "vector argument, v, specifies the "
			 "dimension and number type.")
        .def("nextSolidSphere", nextSolidSphere2)
        .def("nextSolidSphere", nextSolidSphere3)
        .def("nextSolidSphere", nextSolidSphere4)
        ;

    def("hollowSphereRand",&hollowSphereRand<float,IMATH_NAMESPACE::Rand32>,"hollowSphereRand(randObj,num) return XYZ vectors uniformly "
        "distributed across the surface of a sphere generated from the given Rand32 object",
        args("randObj","num"));

    def("solidSphereRand",&solidSphereRand<float,IMATH_NAMESPACE::Rand32>,"solidSphereRand(randObj,num) return XYZ vectors uniformly "
        "distributed through the volume of a sphere generated from the given Rand32 object",
        args("randObj","num"));

    decoratecopy(rand32_class);

    return rand32_class;
}

PYIMATH_EXPORT
class_<IMATH_NAMESPACE::Rand48>
register_Rand48()
{
    double (IMATH_NAMESPACE::Rand48::*nextf1)(void) = &IMATH_NAMESPACE::Rand48::nextf;

    IMATH_NAMESPACE::Vec3<float> (*nextGaussSphere1)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec3<double> (*nextGaussSphere2)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<float> (*nextGaussSphere3)(IMATH_NAMESPACE::Rand48&, const IMATH_NAMESPACE::Vec2<float> &v) = &nextGaussSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<double> (*nextGaussSphere4)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextGaussSphere<double,IMATH_NAMESPACE::Rand48>;

    IMATH_NAMESPACE::Vec3<float> (*nextHollowSphere1)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec3<double> (*nextHollowSphere2)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<float> (*nextHollowSphere3)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<float> &v) = &nextHollowSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<double> (*nextHollowSphere4)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextHollowSphere<double,IMATH_NAMESPACE::Rand48>;

    IMATH_NAMESPACE::Vec3<float> (*nextSolidSphere1)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec3<double> (*nextSolidSphere2)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec3<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<float> (*nextSolidSphere3)(IMATH_NAMESPACE::Rand48&, const IMATH_NAMESPACE::Vec2<float> &v) = &nextSolidSphere<float,IMATH_NAMESPACE::Rand48>;
    IMATH_NAMESPACE::Vec2<double> (*nextSolidSphere4)(IMATH_NAMESPACE::Rand48 &, const IMATH_NAMESPACE::Vec2<double> &v) = &nextSolidSphere<double,IMATH_NAMESPACE::Rand48>;

    class_< IMATH_NAMESPACE::Rand48 > rand48_class("Rand48");
    rand48_class
        .def(init<>("default construction"))
        .def("__init__", make_constructor(Rand_constructor1<IMATH_NAMESPACE::Rand48>))
        .def("__init__", make_constructor(Rand_constructor2<IMATH_NAMESPACE::Rand48>))
        .def("init", &IMATH_NAMESPACE::Rand48::init,
             "r.init(i) -- initialize with integer "
			 "seed i")

        .def("nexti", &IMATH_NAMESPACE::Rand48::nexti,
        	 "r.nexti() -- return the next integer "
			 "value in the uniformly-distributed "
			 "sequence")

        .def("nextf", nextf1,
        	 "r.nextf() -- return the next double "
			 "value in the uniformly-distributed "
			 "sequence\n"

        	 "r.nextf(double,double) -- return the next double "
			 "value in the uniformly-distributed "
			 "sequence")
        .def("nextf", &nextf2 <IMATH_NAMESPACE::Rand48, double>)

        .def("nextb", &IMATH_NAMESPACE::Rand48::nextb,
	 	     "r.nextb() -- return the next boolean "
			 "value in the uniformly-distributed "
			 "sequence")

        .def("nextGauss", &nextGauss<IMATH_NAMESPACE::Rand48>,
        	 "r.nextGauss() -- returns the next "
			 "floating-point value in the normally "
			 "(Gaussian) distributed sequence")

        .def("nextGaussSphere", nextGaussSphere1,
	 		 "r.nextGaussSphere(v) -- returns the next "
			 "point whose distance from the origin "
			 "has a normal (Gaussian) distribution with "
			 "mean 0 and variance 1.  The vector "
			 "argument, v, specifies the dimension "
			 "and number type.")
        .def("nextGaussSphere", nextGaussSphere2)
        .def("nextGaussSphere", nextGaussSphere3)
        .def("nextGaussSphere", nextGaussSphere4)

        .def("nextHollowSphere", nextHollowSphere1,
        	 "r.nextHollowSphere(v) -- return the next "
	 		 "point uniformly distributed on the surface "
	 		 "of a sphere of radius 1 centered at the "
	 		 "origin.  The vector argument, v, specifies "
			 "the dimension and number type.")
        .def("nextHollowSphere", nextHollowSphere2)
        .def("nextHollowSphere", nextHollowSphere3)
        .def("nextHollowSphere", nextHollowSphere4)

        .def("nextSolidSphere", nextSolidSphere1,
        	 "r.nextSolidSphere(v) -- return the next "
			 "point uniformly distributed in a sphere "
			 "of radius 1 centered at the origin.  The "
			 "vector argument, v, specifies the "
			 "dimension and number type.")
        .def("nextSolidSphere", nextSolidSphere2)
        .def("nextSolidSphere", nextSolidSphere3)
        .def("nextSolidSphere", nextSolidSphere4)
        ;

    decoratecopy(rand48_class);

    return rand48_class;
}

//

PyObject *
Rand32::wrap (const IMATH_NAMESPACE::Rand32 &r)
{
    boost::python::return_by_value::apply <IMATH_NAMESPACE::Rand32>::type converter;
    PyObject *p = converter (r);
    return p;
}

PyObject *
Rand48::wrap (const IMATH_NAMESPACE::Rand48 &r)
{
    boost::python::return_by_value::apply <IMATH_NAMESPACE::Rand48>::type converter;
    PyObject *p = converter (r);
    return p;
}

} //namespace PyIMath


#ifndef SKYLARK_CT_DATA_HPP
#define SKYLARK_CT_DATA_HPP

#ifndef SKYLARK_SKETCH_HPP
#error "Include top-level sketch.hpp instead of including individuals headers"
#endif

#include <boost/random.hpp>
#include <boost/property_tree/ptree.hpp>

namespace skylark { namespace sketch {

namespace bstrand = boost::random;

/**
 * Cauchy Transform (data)
 *
 * The CT is simply a dense random matrix with i.i.d Cauchy variables
 */
struct CT_data_t :
   public random_dense_transform_data_t<bstrand::cauchy_distribution> {

    typedef random_dense_transform_data_t<bstrand::cauchy_distribution> base_t;

    /// Params structure
    struct params_t : public sketch_params_t {

        params_t(double C) : C(C) {

        }

        const double C;
    };

    CT_data_t(int N, int S, double C, skylark::base::context_t& context)
        : base_t(N, S, C / static_cast<double>(S),
            bstrand::cauchy_distribution<double>(),
            context, "CT"), _C(C) {

        context = base_t::build();
    }

    CT_data_t(int N, int S, const params_t& params,
        skylark::base::context_t& context)
        : base_t(N, S, params.C / static_cast<double>(S),
            bstrand::cauchy_distribution<double>(),
            context, "CT"), _C(params.C) {

        context = base_t::build();
    }

    CT_data_t(const boost::property_tree::ptree &pt) :
        base_t(pt.get<int>("N"), pt.get<int>("S"),
            pt.get<double>("C") / pt.get<double>("S"),
            bstrand::cauchy_distribution<double>(),
            base::context_t(pt.get_child("creation_context")), "CT"),
        _C(pt.get<double>("C")) {

        base_t::build();
    }

    /**
     *  Serializes a sketch to a string.
     *
     *  @param[out] property_tree describing the sketch.
     */
    virtual
    boost::property_tree::ptree to_ptree() const {
        boost::property_tree::ptree pt;
        sketch_transform_data_t::add_common(pt);
        pt.put("C", _C);
        return pt;
    }

protected:

    CT_data_t(int N, int S, double C, const skylark::base::context_t& context,
        std::string type)
        : base_t(N, S, C / static_cast<double>(S),
            bstrand::cauchy_distribution<double>(),
            context, type), _C(C) {

    }

private:

    double _C;
};

} } /** namespace skylark::sketch */

#endif // SKYLARK_CT_DATA_HPP