// Copyright (c) 2019 OUXT Polaris
//
// 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.

#ifndef PCL_APPS__MATCHING__NDT_MATCHING__NDT_MATCHING_TWIST_ESTIMATOR_COMPONENT_HPP_
#define PCL_APPS__MATCHING__NDT_MATCHING__NDT_MATCHING_TWIST_ESTIMATOR_COMPONENT_HPP_

#if __cplusplus
extern "C" {
#endif

// The below macros are taken from https://gcc.gnu.org/wiki/Visibility and from
// demos/composition/include/composition/visibility_control.h at https://github.com/ros2/demos
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_EXPORT __attribute__((dllexport))
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_IMPORT __attribute__((dllimport))
#else
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_EXPORT __declspec(dllexport)
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_IMPORT __declspec(dllimport)
#endif
#ifdef PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_BUILDING_DLL
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_PUBLIC PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_EXPORT
#else
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_PUBLIC PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_IMPORT
#endif
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_PUBLIC_TYPE \
  PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_PUBLIC
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_LOCAL
#else
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_EXPORT __attribute__((visibility("default")))
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_IMPORT
#if __GNUC__ >= 4
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_PUBLIC __attribute__((visibility("default")))
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_LOCAL __attribute__((visibility("hidden")))
#else
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_PUBLIC
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_LOCAL
#endif
#define PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_PUBLIC_TYPE
#endif

#if __cplusplus
}  // extern "C"
#endif

// Headers in ROS2
#include <pcl_conversions/pcl_conversions.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/transform_datatypes.h>
#include <tf2_eigen/tf2_eigen.h>

#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

// Headers in PCL
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>

// Headers in Boost
#include <boost/circular_buffer.hpp>
#include <boost/optional.hpp>

// Headers in STL
#include <string>

namespace pcl_apps
{
class NdtMatchingTwistEstimatorComponent : public rclcpp::Node
{
public:
  PCL_APPS_NDT_MATCHING_TWIST_ESTIMATOR_PUBLIC
  explicit NdtMatchingTwistEstimatorComponent(const rclcpp::NodeOptions & options);

private:
  std::string input_cloud_topic_;
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_input_cloud_;
  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr current_twist_pub_;
  boost::circular_buffer<pcl::PointCloud<pcl::PointXYZ>::Ptr> buffer_;
  boost::circular_buffer<rclcpp::Time> timestamps_;
  double transform_epsilon_;
  double step_size_;
  double resolution_;
  int max_iterations_;
  std::string input_cloud_frame_id_;
  boost::optional<geometry_msgs::msg::TwistStamped> estimateCurrentTwist();
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt_;
  double toSec(rclcpp::Duration duration)
  {
    double ret;
    double nsecs = static_cast<double>(duration.nanoseconds());
    ret = nsecs * std::pow(static_cast<double>(10.0), -9);
    return ret;
  }
  double toSec(rclcpp::Time stamp)
  {
    double ret;
    double nsecs = static_cast<double>(stamp.nanoseconds());
    ret = nsecs * std::pow(static_cast<double>(10.0), -9);
    return ret;
  }
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_handler_ptr_;
};
}  // namespace pcl_apps

#endif  // PCL_APPS__MATCHING__NDT_MATCHING__NDT_MATCHING_TWIST_ESTIMATOR_COMPONENT_HPP_


// (C) Copyright Renaud Detry   2007-2015.
// Distributed under the GNU General Public License and under the
// BSD 3-Clause License (See accompanying file LICENSE.txt).

/** @file */

#include <nuklei/PoseEstimator.h>
#include <boost/bind.hpp>
#include <nuklei/parallelizer.h>

namespace nuklei
{

  PoseEstimator::PoseEstimator(const double locH,
                               const double oriH,
                               const int nChains,
                               const int n,
                               boost::shared_ptr<CustomIntegrandFactor> cif,
                               const bool partialview,
                               const bool progress) :
  evaluationStrategy_(KernelCollection::MAX_EVAL),
  loc_h_(locH), ori_h_(oriH),
  nChains_(nChains), n_(n),
  cif_(cif), partialview_(partialview),
  progress_(progress), meshTol_(4)
  {
    if (nChains_ <= 0) nChains_ = 8;
    parallel_ = typeFromName<parallelizer>(PARALLELIZATION);
  }

  kernel::se3
  PoseEstimator::modelToSceneTransformation(const boost::optional<kernel::se3>& gtTransfo) const
  {
    NUKLEI_TRACE_BEGIN();
    int n = -1;

    if (n_ <= 0)
    {
      n = objectModel_.size();
      if (n > 1000)
      {
        NUKLEI_WARN("Warning: Object model has more than 1000 points. "
                    "To keep computational cost low, only 1000 points will be "
                    "used at each inference loop. "
                    "Use -n to force a large number of model points.");
        n = 1000;
      }
    }
    else
      n = n_;

    KernelCollection poses;
    if (!hasOpenMP())
    {
      NUKLEI_WARN("Nuklei has not been compiled with OpenMP support. "
                  "Pose estimation will use a single core.");
    }

    if (progress_)
      pi_->initialize(0, 10*n*(partialview_?4:1)*nChains_ / 10, "Estimating pose", 0);

    parallelizer p(nChains_, parallel_);
    std::vector<kernel::se3> retv =
    p.run<kernel::se3>(boost::bind(&PoseEstimator::mcmc, this, n),
                       kernel::base::WeightAccessor());

    if (progress_)
      pi_->forceEnd();

    for (std::vector<kernel::se3>::const_iterator i = retv.begin();
         i != retv.end(); ++i)
      poses.add(*i);

    int success = 0;
    if (gtTransfo)
    {
      for (KernelCollection::const_sort_iterator i = poses.sortBegin(poses.size()); i != i.end(); ++i)
      {
        coord_pair d = i->polyDistanceTo(*gtTransfo);
        coord_pair t = coord_pair(gtTransfo->getLocH(), gtTransfo->getOriH());
        bool s = (d.first < t.first && d.second < t.second);
        if (s)
          success++;
        std::cout << "Matching score: " << i->getWeight() << ", ";
        std::cout << "distance to GT: " << d.first << " " << d.second << ", ";
        if (s) std::cout << "success";
        else std::cout << "failure";
        std::cout << std::endl;
      }
      std::cout << "Number of successful chains: " << success << " out of ";
      std::cout << poses.size() << "." << std::endl;
    }

    kernel::se3 pose(*poses.sortBegin(1));
    pose.setWeight(findMatchingScore(pose));

    return pose;
    NUKLEI_TRACE_END();
  }

  double
  PoseEstimator::findMatchingScore(const kernel::se3& pose) const
  {
    NUKLEI_TRACE_BEGIN();
    kernel::se3 t = pose;

    if (!partialview_)
    {
      double w1 = 0, w2 = 0;

      KernelCollection tmp = objectModel_;
      tmp.transformWith(t);
      tmp.computeKernelStatistics();
      tmp.buildKdTree();

      for (KernelCollection::const_iterator i = objectModel_.begin();
           i != objectModel_.end(); ++i)
      {
        w1 += sceneModel_.evaluationAt(*i->polyTransformedWith(t),
                                       evaluationStrategy_);
      }

      for (KernelCollection::const_iterator i = sceneModel_.begin();
           i != sceneModel_.end(); ++i)
      {
        w2 += tmp.evaluationAt(*i, evaluationStrategy_);
      }

      t.setWeight(w1/objectModel_.size() * (cif_?cif_->factor(pose):1.));
      //t.setWeight(std::sqrt(w1/objectModel_.size()*w2/objectModel_.size()));
    }
    else
    {
      // Use a mesh to compute the partial view of the model and compute the
      // matching score from that.

      KernelCollection::const_partialview_iterator viewIterator =
      objectModel_.partialViewBegin(viewpointInFrame(t), meshTol_, false, true);
      for (KernelCollection::const_partialview_iterator i = viewIterator;
           i != i.end(); ++i)
      {
        weight_t w = sceneModel_.evaluationAt(*i->polyTransformedWith(t),
                                              evaluationStrategy_);
        t.setWeight(t.getWeight() + w);
      }
      t.setWeight(t.getWeight()/std::pow(std::distance(viewIterator, viewIterator.end()), .7) * (cif_?cif_->factor(pose):1.));

      if (cif_ && !cif_->test(t))
        t.setWeight(0);
    }

    return t.getWeight();
    NUKLEI_TRACE_END();
  }

  void PoseEstimator::load(const std::string& objectFilename,
                           const std::string& sceneFilename,
                           const std::string& meshfile,
                           const std::string& viewpointfile,
                           const bool light,
                           const bool computeNormals)
  {
    NUKLEI_TRACE_BEGIN();
    KernelCollection objectModel, sceneModel;
    readObservations(objectFilename, objectModel);
    readObservations(sceneFilename, sceneModel);
    Vector3 viewpoint(0, 0, 0);
    if (partialview_)
    {
      NUKLEI_ASSERT(!viewpointfile.empty());
      viewpoint = kernel::se3(*readSingleObservation(viewpointfile)).getLoc();
    }
    load(objectModel, sceneModel, meshfile, viewpoint,
         light, computeNormals);
    NUKLEI_TRACE_END();
  }

  void PoseEstimator::load(const KernelCollection& objectModel,
                           const KernelCollection& sceneModel,
                           const std::string& meshfile,
                           const Vector3& viewpoint,
                           const bool light,
                           const bool computeNormals)
  {
    NUKLEI_TRACE_BEGIN();
    objectModel_ = objectModel;
    sceneModel_ = sceneModel;
    viewpoint_ = viewpoint;

    if (objectModel_.size() == 0 || sceneModel_.size() == 0)
      NUKLEI_THROW("Empty input cloud.");

    if (objectModel_.front().polyType() == kernel::base::R3)
    {
      if (computeNormals)
      {
        objectModel_.buildNeighborSearchTree();
        objectModel_.computeSurfaceNormals();
      }
    }

    if (sceneModel_.front().polyType() == kernel::base::R3)
    {
      if (computeNormals)
      {
        sceneModel_.buildNeighborSearchTree();
        sceneModel_.computeSurfaceNormals();
      }
    }

    if (objectModel_.front().polyType() != sceneModel_.front().polyType())
      NUKLEI_THROW("Input point clouds must be defined on the same domain.");

    if (light && sceneModel_.size() > 10000)
    {
      sceneModel_.computeKernelStatistics();
      KernelCollection tmp;
      KernelCollection::sample_iterator i =
      sceneModel_.sampleBegin(10000);
      for (; i != i.end(); i++)
      {
        tmp.add(*i);
      }
      sceneModel_ = tmp;
    }

    objectSize_ = objectModel_.moments()->getLocH();

    if (loc_h_ <= 0)
      loc_h_ = objectSize_ / 10;

    sceneModel_.setKernelLocH(loc_h_);
    sceneModel_.setKernelOriH(ori_h_);
    objectModel_.setKernelLocH(loc_h_);
    objectModel_.setKernelOriH(ori_h_);

    objectModel_.computeKernelStatistics();
    sceneModel_.computeKernelStatistics();
    sceneModel_.buildKdTree();

    if (partialview_)
    {
      if (!meshfile.empty())
        objectModel_.readMeshFromOffFile(meshfile);
      else
        objectModel_.buildMesh();
      objectModel_.buildPartialViewCache(meshTol_, as_const(objectModel_).front().polyType() == kernel::base::R3XS2P);
    }

    // Create dummy ProgressIndicator
    if (progress_)
      pi_.reset(new ProgressIndicator(1, "", 11));
    NUKLEI_TRACE_END();
  }

  // Temperature function (cooling factor)
  double PoseEstimator::Ti(const unsigned i, const unsigned F)
  {
    {
      const double T0 = .5;
      const double TF = .05;

      return std::max(T0 * std::pow(TF/T0, double(i)/F), TF);
    }
  }

  bool PoseEstimator::recomputeIndices(std::vector<int>& indices,
                                       const kernel::se3& nextPose,
                                       const int n) const
  {
    Vector3 mean = objectModel_.mean()->getLoc();
    Vector3 v = la::normalized(viewpointInFrame(nextPose) - mean);
    std::vector<int> pindices = objectModel_.partialView(v, meshTol_, true, true);

    if (pindices.size() < 20) return false;
    indices = pindices;

    //      indices = objectModel_.partialView(viewpointInFrame(nextPose),
    //                                         meshTol_);
    std::random_shuffle(indices.begin(), indices.end(), Random::uniformInt);
    if (indices.size() > n)
      indices.resize(n);
    return true;
  }

  /**
   * This function implements the algorithm of Fig. 2: Simulated annealing
   * algorithm of the ACCV paper.
   * - T_j is given by @c temperature
   * - u is given by Random::uniform()
   * - w_j is @c currentPose
   */
  void
  PoseEstimator::metropolisHastings(kernel::se3& currentPose,
                                    weight_t &currentWeight,
                                    const weight_t temperature,
                                    const bool firstRun,
                                    const int n) const
  {
    NUKLEI_TRACE_BEGIN();

    // Randomly select particles from the object model
    std::vector<int> indices;
    for (KernelCollection::const_sample_iterator
         i = objectModel_.sampleBegin(n);
         i != i.end();
         i++)
    {
      indices.push_back(i.index());
    }
    std::random_shuffle(indices.begin(), indices.end(), Random::uniformInt);

    // Next chain state
    kernel::se3 nextPose;
    // Whether we go for a local or independent proposal
    bool independentProposal = false;

    if (Random::uniform() < .75 || firstRun)
    {
      // Go for independent proposal

      independentProposal = true;

      for (int count = 0; ; ++count)
      {
        if (count == 100) return;
        const kernel::base& randomModelPoint = objectModel_.at(indices.at(Random::uniformInt(indices.size())));
        kernel::se3::ptr k2 = randomModelPoint.polySe3Proj();
        kernel::se3::ptr k1 = sceneModel_.at(Random::uniformInt(sceneModel_.size())).polySe3Proj();

        nextPose = k1->transformationFrom(*k2);

        if (cif_ && !cif_->test(nextPose)) continue;

        if (partialview_)
        {
          bool visible = false;

          if (randomModelPoint.polyType() == kernel::base::R3XS2P)
            visible = objectModel_.isVisibleFrom(kernel::r3xs2p(randomModelPoint),
                                                 viewpointInFrame(nextPose),
                                                 meshTol_);
          else
            visible = objectModel_.isVisibleFrom(randomModelPoint.getLoc(),
                                                 viewpointInFrame(nextPose),
                                                 meshTol_);
          if (!visible) continue;

          if (! recomputeIndices(indices, nextPose, n))
            continue;
        }

        break;
      }
    }
    else
    {
      // Go for local proposal

      independentProposal = false;
      NUKLEI_DEBUG_ASSERT(currentPose.loc_h_ > 0 && currentPose.ori_h_ > 0);
      for (int count = 0; ; ++count)
      {
        if (count == 100) return;
        nextPose = currentPose.sample();
        if (cif_ && !cif_->test(nextPose)) continue;
        if (partialview_ && ! recomputeIndices(indices, nextPose, n))
          continue;
        break;
      }
    }

    weight_t weight = 0;

    double threshold = Random::uniform();

    double factor = (cif_?cif_->factor(nextPose):1.);

    // Go through the points of the model
    for (unsigned pi = 0; pi < indices.size(); ++pi)
    {
      const kernel::base& objectPoint =
      objectModel_.at(indices.at(pi));

      kernel::base::ptr test = objectPoint.polyTransformedWith(nextPose);

      weight_t w = 0;
      if (WEIGHTED_SUM_EVIDENCE_EVAL)
      {
        w = (sceneModel_.evaluationAt(*test,
                                      KernelCollection::WEIGHTED_SUM_EVAL) +
             WHITE_NOISE_POWER/sceneModel_.size() );
      }
      else
      {
        w = (sceneModel_.evaluationAt(*test, KernelCollection::MAX_EVAL) +
             WHITE_NOISE_POWER );
      }

      w *= factor;

      weight += w;

      // At least consider sqrt(size(model)) points
      if (pi < std::sqrt(indices.size())) continue;

      weight_t nextWeight = weight/(pi+1.);
      if (partialview_) nextWeight = weight/std::sqrt(pi+1.);
      // For the first run, consider all the points of the model
      if (firstRun)
      {
        if (pi == indices.size()-1)
        {
          currentPose = nextPose;
          currentWeight = nextWeight;
          return;
        }
        else continue;
      }

      weight_t dec = std::pow(nextWeight/currentWeight, 1./temperature);
      if (independentProposal) dec *= currentWeight/nextWeight;

      // Early abort
      if (dec < .6*threshold)
      {
        return;
      }

      // MH decision
      if (pi == indices.size()-1)
      {
        if (dec > threshold)
        {
          currentPose = nextPose;
          currentWeight = nextWeight;
        }
        return;
      }
    }
    NUKLEI_THROW("Reached forbidden state.");
    NUKLEI_TRACE_END();
  }

  kernel::se3
  PoseEstimator::mcmc(const int n) const
  {
    NUKLEI_TRACE_BEGIN();
    kernel::se3 currentPose, bestPose;
    weight_t currentWeight = 0;
    bestPose.setWeight(currentWeight);
    metropolisHastings(currentPose, currentWeight, 1, true, n);

    //fixme: See if nSteps should be computed as a function of n.
    int nSteps = 1000;
    if (partialview_) nSteps = 1000;
    //fixme:
    nSteps = 10*n*(partialview_?4:1);

    for (int i = 0; i < nSteps; i++)
    {
      {
        // begin and end bandwidths for the local proposal
        coord_t bLocH = objectSize_/10;
        coord_t eLocH = objectSize_/40;
        coord_t bOriH = .1;
        coord_t eOriH = .02;

        unsigned e = nSteps-1;

        currentPose.setLocH(double(e-i)/e * bLocH +
                            double(i)/e * eLocH);
        currentPose.setOriH(double(e-i)/e * bOriH +
                            double(i)/e * eOriH);
        if (currentPose.loc_h_ <= 0)
        {
          NUKLEI_THROW("Unexpected value for currentPose.loc_h_.");
        }
        if (progress_ && i%10 == 0) pi_->mtInc();
      }

      metropolisHastings(currentPose, currentWeight,
                         Ti(i, nSteps/5), false, n);

      if (currentWeight > bestPose.getWeight())
      {
        bestPose = currentPose;
        bestPose.setWeight(currentWeight);
      }
    }

    return bestPose;
    NUKLEI_TRACE_END();
  }

  Vector3 PoseEstimator::viewpointInFrame(const kernel::se3& frame) const
  {
    kernel::se3 origin;
    kernel::se3 invt = origin.transformationFrom(frame);
    kernel::r3 v;
    v.loc_ = viewpoint_;
    return v.transformedWith(invt).getLoc();
  }

  void
  PoseEstimator::writeAlignedModel(const std::string& filename,
                                   const kernel::se3& t) const
  {
    NUKLEI_TRACE_BEGIN();
    KernelCollection objectModel = objectModel_;
    if (partialview_)
    {
      objectModel = KernelCollection();
      kernel::se3 tt = t;
      for (KernelCollection::const_iterator i = objectModel_.begin();
           i != objectModel_.end(); ++i)
      {
        objectModel.add(*i);
        bool visible = false;
        const kernel::base& tmp = *i;
        if (tmp.polyType() == kernel::base::R3XS2P)
          visible = objectModel_.isVisibleFrom(kernel::r3xs2p(tmp),
                                               viewpointInFrame(tt),
                                               meshTol_);
        else
          visible = objectModel_.isVisibleFrom(tmp.getLoc(),
                                               viewpointInFrame(tt),
                                               meshTol_);
        if (visible)
        {
          RGBColor c(0, 0, 1);
          ColorDescriptor d;
          d.setColor(c);
          objectModel.back().setDescriptor(d);
        }
      }
    }
    objectModel.transformWith(t);
    writeObservations(filename,
                      objectModel,
                      Observation::SERIAL);
    NUKLEI_TRACE_END();
  }

  void PoseEstimator::setCustomIntegrandFactor(boost::shared_ptr<CustomIntegrandFactor> cif)
  {
    cif_ = cif;
  }

  boost::shared_ptr<CustomIntegrandFactor> PoseEstimator::getCustomIntegrandFactor() const
  {
    return cif_;
  }

}


// Copyright (c) 2019 Tim Perkins

// 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.

#ifndef TTFRM_TFRM_HPP
#define TTFRM_TFRM_HPP

#include <exception>
#include <tuple>

#include <fmt/format.h>
#include <Eigen/Geometry>

namespace ttfrm {

using Quat = Eigen::Quaterniond;
using Vec3 = Eigen::Vector3d;

template <typename FrameId>
class Tfrm;

template <typename FrameId_>
struct FramePair {
  using FrameId = FrameId_;

  FrameId to_frame;
  FrameId from_frame;
};

template <typename FrameId>
std::string Stringify(const Tfrm<FrameId>& tfrm);

template <typename FrameId>
std::string Stringify(const FramePair<FrameId>& frame_pair);

template <typename FrameId_>
class Tfrm {
 public:
  using FrameId = FrameId_;

  Tfrm(const FrameId& to_frame, const FrameId& from_frame, const Quat& rot, const Vec3& trans);
  Tfrm(const Tfrm& other_tfrm);
  Tfrm(Tfrm&& other_tfrm);

  static Tfrm<FrameId> Identity(const FrameId& to_frame, const FrameId& from_frame);
  static Tfrm<FrameId> FromRotation(const FrameId& to_frame, const FrameId& from_frame,
                                    const Quat& rot);
  static Tfrm<FrameId> FromTranslation(const FrameId& to_frame, const FrameId& from_frame,
                                       const Vec3& trans);
  static Tfrm<FrameId> FromIsometry(const FrameId& to_frame, const FrameId& from_frame,
                                    const Eigen::Isometry3d& isometry);

  Tfrm<FrameId>& operator=(const Tfrm<FrameId>& other_tfrm);
  Tfrm<FrameId>& operator=(Tfrm<FrameId>&& other_tfrm);

  bool operator==(const Tfrm<FrameId>& other_tfrm) const;
  bool operator!=(const Tfrm<FrameId>& other_tfrm) const;
  bool IsApprox(const Tfrm& other_tfrm, double precision = 1.0e-6) const;

  FramePair<FrameId> Frames() const;
  FrameId ToFrame() const;
  FrameId FromFrame() const;

  Quat Rotation() const;
  Vec3 Translation() const;

  Tfrm<FrameId> Inverse() const;

  Tfrm<FrameId> Compose(const Tfrm<FrameId>& other_tfrm) const;
  Tfrm<FrameId> operator*(const Tfrm<FrameId>& other_tfrm) const;
  Tfrm<FrameId> operator()(const Tfrm<FrameId>& other_tfrm) const;

  Vec3 Apply(const Vec3& trans) const;
  Vec3 operator*(const Vec3& trans) const;
  Vec3 operator()(const Vec3& trans) const;

  Tfrm<FrameId> Interpolate(const FrameId& interp_to_frame, const Tfrm<FrameId>& other_tfrm,
                            double t) const;

  Eigen::Isometry3d AsIsometry() const;

 private:
  FrameId to_frame_;
  FrameId from_frame_;
  Quat rot_;
  Vec3 trans_;

  friend std::string Stringify<FrameId>(const Tfrm<FrameId>& tfrm);
  friend std::string Stringify<FrameId>(const FramePair<FrameId>& frame_pair);
};

class TfrmComposeException : public std::runtime_error {
 public:
  template <typename FrameId>
  TfrmComposeException(const Tfrm<FrameId>& tfrm, const Tfrm<FrameId>& other_tfrm);
  TfrmComposeException(const TfrmComposeException& other_comp_excp) = default;
  TfrmComposeException(TfrmComposeException&& other_comp_excp) = default;
};

class TfrmInterpolateException : public std::runtime_error {
 public:
  template <typename FrameId>
  TfrmInterpolateException(const Tfrm<FrameId>& tfrm, const Tfrm<FrameId>& other_tfrm);
  TfrmInterpolateException(const TfrmInterpolateException& other_comp_excp) = default;
  TfrmInterpolateException(TfrmInterpolateException&& other_comp_excp) = default;
};

template <typename FrameId>
Tfrm<FrameId>::Tfrm(const FrameId& to_frame, const FrameId& from_frame, const Quat& rot,
                    const Vec3& trans)
    : to_frame_(to_frame), from_frame_(from_frame), rot_(rot.normalized()), trans_(trans)
{
  // Do nothing
}

template <typename FrameId>
Tfrm<FrameId>::Tfrm(const Tfrm<FrameId>& other_tfrm)
    : to_frame_(other_tfrm.to_frame_),
      from_frame_(other_tfrm.from_frame_),
      rot_(other_tfrm.rot_),
      trans_(other_tfrm.trans_)
{
  // Do nothing
}

template <typename FrameId>
Tfrm<FrameId>::Tfrm(Tfrm&& other_tfrm)
    : to_frame_(std::move(other_tfrm.to_frame_)),
      from_frame_(std::move(other_tfrm.from_frame_)),
      rot_(std::move(other_tfrm.rot_)),
      trans_(std::move(other_tfrm.trans_))
{
  // Do nothing
}

template <typename FrameId>
Tfrm<FrameId> Tfrm<FrameId>::Identity(const FrameId& to_frame, const FrameId& from_frame)
{
  return Tfrm<FrameId>(to_frame, from_frame, Quat::Identity(), Vec3::Zero());
}

template <typename FrameId>
Tfrm<FrameId> Tfrm<FrameId>::FromRotation(const FrameId& to_frame, const FrameId& from_frame,
                                          const Quat& rot)
{
  return Tfrm<FrameId>(to_frame, from_frame, rot, Vec3::Zero());
}

template <typename FrameId>
Tfrm<FrameId> Tfrm<FrameId>::FromTranslation(const FrameId& to_frame, const FrameId& from_frame,
                                             const Vec3& trans)
{
  return Tfrm<FrameId>(to_frame, from_frame, Quat::Identity(), trans);
}

template <typename FrameId>
Tfrm<FrameId> Tfrm<FrameId>::FromIsometry(const FrameId& to_frame, const FrameId& from_frame,
                                          const Eigen::Isometry3d& isometry)
{
  return Tfrm<FrameId>(to_frame, from_frame, Quat(isometry.rotation()), isometry.translation());
}

template <typename FrameId>
Tfrm<FrameId>& Tfrm<FrameId>::operator=(const Tfrm& other_tfrm)
{
  to_frame_ = other_tfrm.to_frame_;
  from_frame_ = other_tfrm.from_frame_;
  rot_ = other_tfrm.rot_;
  trans_ = other_tfrm.trans_;
  return *this;
}

template <typename FrameId>
Tfrm<FrameId>& Tfrm<FrameId>::operator=(Tfrm&& other_tfrm)
{
  to_frame_ = std::move(other_tfrm.to_frame_);
  from_frame_ = std::move(other_tfrm.from_frame_);
  rot_ = std::move(other_tfrm.rot_);
  trans_ = std::move(other_tfrm.trans_);
  return *this;
}

template <typename FrameId>
bool Tfrm<FrameId>::operator==(const Tfrm& other_tfrm) const
{
  static auto QuatEquality = [](const Quat& q1, const Quat& q2) {
    return (q1.w() == q2.w() && q1.x() == q2.x() && q1.y() == q2.y() && q1.z() == q2.z());
  };
  return (to_frame_ == other_tfrm.to_frame_ && from_frame_ == other_tfrm.from_frame_
          && QuatEquality(rot_, other_tfrm.rot_) && trans_ == other_tfrm.trans_);
}

template <typename FrameId>
bool Tfrm<FrameId>::operator!=(const Tfrm& other_tfrm) const
{
  return !(*this == other_tfrm);
}

template <typename FrameId>
bool Tfrm<FrameId>::IsApprox(const Tfrm& other_tfrm, double precision) const
{
  return (to_frame_ == other_tfrm.to_frame_ && from_frame_ == other_tfrm.from_frame_
          && rot_.isApprox(other_tfrm.rot_, precision)
          && trans_.isApprox(other_tfrm.trans_, precision));
}

template <typename FrameId>
FramePair<FrameId> Tfrm<FrameId>::Frames() const
{
  return {to_frame_, from_frame_};
}

template <typename FrameId>
FrameId Tfrm<FrameId>::ToFrame() const
{
  return to_frame_;
}

template <typename FrameId>
FrameId Tfrm<FrameId>::FromFrame() const
{
  return from_frame_;
}

template <typename FrameId>
Quat Tfrm<FrameId>::Rotation() const
{
  return rot_;
}

template <typename FrameId>
Vec3 Tfrm<FrameId>::Translation() const
{
  return trans_;
}

template <typename FrameId>
Tfrm<FrameId> Tfrm<FrameId>::Inverse() const
{
  const Quat inv_rot = rot_.inverse();
  return Tfrm<FrameId>(from_frame_, to_frame_, inv_rot, inv_rot * -trans_);
}

template <typename FrameId>
Tfrm<FrameId> Tfrm<FrameId>::Compose(const Tfrm& other_tfrm) const
{
  if (from_frame_ != other_tfrm.to_frame_) {
    throw TfrmComposeException(*this, other_tfrm);
  }
  // T2(T1(v)) = R2 * (R1 * v + t1) + t2 = (R2 * R1) * v + (t2 + R2 * t1)
  return Tfrm<FrameId>(to_frame_, other_tfrm.from_frame_, rot_ * other_tfrm.rot_,
                       trans_ + rot_ * other_tfrm.trans_);
}

template <typename FrameId>
Tfrm<FrameId> Tfrm<FrameId>::operator*(const Tfrm& other_tfrm) const
{
  return Compose(other_tfrm);
}

template <typename FrameId>
Tfrm<FrameId> Tfrm<FrameId>::operator()(const Tfrm& other_tfrm) const
{
  return Compose(other_tfrm);
}

template <typename FrameId>
Vec3 Tfrm<FrameId>::Apply(const Vec3& trans) const
{
  // T(v) = R * v + t
  return Vec3(rot_ * trans + trans_);
}

template <typename FrameId>
Vec3 Tfrm<FrameId>::operator*(const Vec3& trans) const
{
  return Apply(trans);
}

template <typename FrameId>
Vec3 Tfrm<FrameId>::operator()(const Vec3& trans) const
{
  return Apply(trans);
}

template <typename FrameId>
Tfrm<FrameId> Tfrm<FrameId>::Interpolate(const FrameId& interp_to_frame,
                                         const Tfrm<FrameId>& other_tfrm, double ratio) const
{
  if (from_frame_ != other_tfrm.from_frame_) {
    throw TfrmInterpolateException(*this, other_tfrm);
  }
  return Tfrm<FrameId>(interp_to_frame, from_frame_, rot_.slerp(ratio, other_tfrm.rot_),
                       trans_ + ratio * (other_tfrm.trans_ - trans_));
}

template <typename FrameId>
Eigen::Isometry3d Tfrm<FrameId>::AsIsometry() const
{
  // The concatenation of the translation and the rotation
  return Eigen::Translation3d(trans_) * rot_;
}

template <typename FrameId>
TfrmComposeException::TfrmComposeException(const Tfrm<FrameId>& tfrm,
                                           const Tfrm<FrameId>& other_tfrm)
    : std::runtime_error(fmt::format("Cannot compose transforms {} and {}",
                                     Stringify(tfrm.Frames()), Stringify(other_tfrm.Frames())))
{
  // Do nothing
}

template <typename FrameId>
TfrmInterpolateException::TfrmInterpolateException(const Tfrm<FrameId>& tfrm,
                                                   const Tfrm<FrameId>& other_tfrm)
    : std::runtime_error(fmt::format("Cannot interpolate transforms {} and {}",
                                     Stringify(tfrm.Frames()), Stringify(other_tfrm.Frames())))
{
  // Do nothing
}

template <typename FrameId>
std::string Stringify(const Tfrm<FrameId>& tfrm)
{
  return fmt::format(
      "([{}] <- [{}], ROT: (W: {}, X: {}, Y: {}, Z: {}), TRANS: (X: {}, Y: {}, Z: {}))",
      tfrm.to_frame_, tfrm.from_frame_, tfrm.rot_.w(), tfrm.rot_.x(), tfrm.rot_.y(), tfrm.rot_.z(),
      tfrm.trans_.x(), tfrm.trans_.y(), tfrm.trans_.z());
}

template <typename FrameId>
std::string Stringify(const FramePair<FrameId>& frame_pair)
{
  return fmt::format("([{}] <- [{}])", frame_pair.to_frame, frame_pair.from_frame);
}

}  // namespace ttfrm

#endif  // TTFRM_TFRM_HPP


///////////////////////////////////////////////////////////////
//  Copyright 2013 John Maddock. 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_

#ifndef BOOST_MP_FLOAT128_HPP
#define BOOST_MP_FLOAT128_HPP

#include <boost/config.hpp>
#include <boost/scoped_array.hpp>
#include <boost/functional/hash.hpp>
#include <boost/multiprecision/number.hpp>

#if defined(BOOST_INTEL) && !defined(BOOST_MP_USE_FLOAT128) && !defined(BOOST_MP_USE_QUAD)
#  if defined(BOOST_INTEL_CXX_VERSION) && (BOOST_INTEL_CXX_VERSION >= 1310) && defined(__GNUC__)
#    if (__GNUC__ > 4) || ((__GNUC__ == 4) && (__GNUC_MINOR__ >= 6))
#      define BOOST_MP_USE_FLOAT128
#    endif
#  endif

#  ifndef BOOST_MP_USE_FLOAT128
#    define BOOST_MP_USE_QUAD
#  endif
#endif

#if defined(__GNUC__) && !defined(BOOST_MP_USE_FLOAT128) && !defined(BOOST_MP_USE_QUAD)
#  define BOOST_MP_USE_FLOAT128
#endif

#if !defined(BOOST_MP_USE_FLOAT128) && !defined(BOOST_MP_USE_QUAD)
#  error "Sorry compiler is neither GCC, not Intel, don't know how to configure this header."
#endif
#if defined(BOOST_MP_USE_FLOAT128) && defined(BOOST_MP_USE_QUAD)
#  error "Oh dear, both BOOST_MP_USE_FLOAT128 and BOOST_MP_USE_QUAD are defined, which one should I be using?"
#endif

#if defined(BOOST_MP_USE_FLOAT128)

extern "C" {
#include <quadmath.h>
}

typedef __float128 float128_type;

#elif defined(BOOST_MP_USE_QUAD)

#include <boost/multiprecision/detail/float_string_cvt.hpp>

typedef _Quad float128_type;

extern "C" {
_Quad __ldexpq(_Quad, int);
_Quad __frexpq(_Quad, int*);
_Quad __fabsq(_Quad);
_Quad __floorq(_Quad);
_Quad __ceilq(_Quad);
_Quad __sqrtq(_Quad);
_Quad __truncq(_Quad);
_Quad __expq(_Quad);
_Quad __powq(_Quad, _Quad);
_Quad __logq(_Quad);
_Quad __log10q(_Quad);
_Quad __sinq(_Quad);
_Quad __cosq(_Quad);
_Quad __tanq(_Quad);
_Quad __asinq(_Quad);
_Quad __acosq(_Quad);
_Quad __atanq(_Quad);
_Quad __sinhq(_Quad);
_Quad __coshq(_Quad);
_Quad __tanhq(_Quad);
_Quad __fmodq(_Quad, _Quad);
_Quad __atan2q(_Quad, _Quad);

#define ldexpq __ldexpq
#define frexpq __frexpq
#define fabsq __fabsq
#define floorq __floorq
#define ceilq __ceilq
#define sqrtq __sqrtq
#define truncq __truncq
#define expq __expq
#define powq __powq
#define logq __logq
#define log10q __log10q
#define sinq __sinq
#define cosq __cosq
#define tanq __tanq
#define asinq __asinq
#define acosq __acosq
#define atanq __atanq
#define sinhq __sinhq
#define coshq __coshq
#define tanhq __tanhq
#define fmodq __fmodq
#define atan2q __atan2q
}

inline _Quad isnanq(_Quad v)
{
   return v != v;
}
inline _Quad isinfq(_Quad v)
{
   return __fabsq(v) > 1.18973149535723176508575932662800702e4932Q;
}

#endif

namespace boost{
namespace multiprecision{
namespace backends{

struct float128_backend;

}

using backends::float128_backend;

template<>
struct number_category<backends::float128_backend> : public mpl::int_<number_kind_floating_point> {};
#if defined(BOOST_MP_USE_QUAD)
template<>
struct number_category<float128_type> : public mpl::int_<number_kind_floating_point> {};
#endif

typedef number<float128_backend, et_off> float128;

#ifndef BOOST_NO_CXX11_CONSTEXPR

namespace quad_constants {
   constexpr __float128 quad_min = static_cast<__float128>(1)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN) / 1073741824;

   constexpr __float128 quad_denorm_min = static_cast<__float128>(1)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN)
      * static_cast<__float128>(DBL_MIN) / 5.5751862996326557854e+42;

   constexpr double dbl_mult = 8.9884656743115795386e+307;  // This has one bit set only.
   constexpr __float128 quad_max = (static_cast<__float128>(1) - 9.62964972193617926527988971292463659e-35)  // This now has all bits sets to 1
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult)
      * static_cast<__float128>(dbl_mult) * 65536;
}

#define BOOST_MP_QUAD_MIN boost::multiprecision::quad_constants::quad_min
#define BOOST_MP_QUAD_DENORM_MIN boost::multiprecision::quad_constants::quad_denorm_min
#define BOOST_MP_QUAD_MAX boost::multiprecision::quad_constants::quad_max

#else

#define BOOST_MP_QUAD_MIN 3.36210314311209350626267781732175260e-4932Q
#define BOOST_MP_QUAD_DENORM_MIN 6.475175119438025110924438958227646552e-4966Q
#define BOOST_MP_QUAD_MAX 1.18973149535723176508575932662800702e4932Q

#endif

namespace backends{

struct float128_backend
{
   typedef mpl::list<signed char, short, int, long, boost::long_long_type>   signed_types;
   typedef mpl::list<unsigned char, unsigned short,
      unsigned int, unsigned long, boost::ulong_long_type>           unsigned_types;
   typedef mpl::list<float, double, long double>                 float_types;
   typedef int                                                   exponent_type;

private:
   float128_type m_value;
public:
   BOOST_CONSTEXPR float128_backend() BOOST_NOEXCEPT : m_value(0) {}
   BOOST_CONSTEXPR float128_backend(const float128_backend& o) BOOST_NOEXCEPT : m_value(o.m_value) {}
   float128_backend& operator = (const float128_backend& o) BOOST_NOEXCEPT
   {
      m_value = o.m_value;
      return *this;
   }
   template <class T>
   BOOST_CONSTEXPR float128_backend(const T& i, const typename enable_if_c<is_convertible<T, float128_type>::value>::type* = 0) BOOST_NOEXCEPT_IF(noexcept(std::declval<float128_type&>() = std::declval<const T&>()))
      : m_value(i) {}
   template <class T>
   typename enable_if_c<is_arithmetic<T>::value || is_convertible<T, float128_type>::value, float128_backend&>::type operator = (const T& i) BOOST_NOEXCEPT_IF(noexcept(std::declval<float128_type&>() = std::declval<const T&>()))
   {
      m_value = i;
      return *this;
   }
   float128_backend(long double const& f)
   {
      BOOST_STATIC_CONSTEXPR __float128 inf_val = static_cast<__float128>(HUGE_VAL);
      if(boost::math::isinf(f))
         m_value = (f < 0) ? -inf_val : inf_val;
      else
         m_value = f;
   }
   float128_backend& operator=(long double const& f)
   {
      BOOST_STATIC_CONSTEXPR __float128 inf_val = static_cast<__float128>(HUGE_VAL);
      if(boost::math::isinf(f))
         m_value = (f < 0) ? -inf_val : inf_val;
      else
         m_value = f;
      return *this;
   }
   float128_backend& operator = (const char* s)
   {
#ifndef BOOST_MP_USE_QUAD
      char* p_end;
      m_value = strtoflt128(s, &p_end);
      if(p_end - s != (std::ptrdiff_t)std::strlen(s))
      {
         BOOST_THROW_EXCEPTION(std::runtime_error("Unable to interpret input string as a floating point value"));
      }
#else
      boost::multiprecision::detail::convert_from_string(*this, s);
#endif
      return *this;
   }
   void swap(float128_backend& o) BOOST_NOEXCEPT
   {
      std::swap(m_value, o.value());
   }
   std::string str(std::streamsize digits, std::ios_base::fmtflags f)const
   {
#ifndef BOOST_MP_USE_QUAD
      char buf[100];
      boost::scoped_array<char> buf2;
      std::string format = "%";
      if(f & std::ios_base::showpos)
         format += "+";
      if(f & std::ios_base::showpoint)
         format += "#";
      format += ".*";
      if(digits == 0)
         digits = 36;
      format += "Q";
      if(f & std::ios_base::scientific)
         format += "e";
      else if(f & std::ios_base::fixed)
         format += "f";
      else
         format += "g";

      int v = quadmath_snprintf (buf, 100, format.c_str(), digits, m_value);

      if((v < 0) || (v >= 99))
      {
         int v_max = v;
         buf2.reset(new char[v+3]);
         v = quadmath_snprintf (&buf2[0], v_max + 3, format.c_str(), digits, m_value);
         if(v >= v_max + 3)
         {
            BOOST_THROW_EXCEPTION(std::runtime_error("Formatting of float128_type failed."));
         }
         return &buf2[0];
      }
      return buf;
#else
      return boost::multiprecision::detail::convert_to_string(*this, digits ? digits : 37, f);
#endif
   }
   void negate() BOOST_NOEXCEPT
   {
      m_value = -m_value;
   }
   int compare(const float128_backend& o)const
   {
      return m_value == o.m_value ? 0 : m_value < o.m_value ? -1 : 1;
   }
   template <class T>
   int compare(const T& i)const
   {
      return m_value == i ? 0 : m_value < i ? -1 : 1;
   }
   float128_type& value()
   {
      return m_value;
   }
   const float128_type& value()const
   {
      return m_value;
   }
};

inline void eval_add(float128_backend& result, const float128_backend& a)
{
   result.value() += a.value();
}
template <class A>
inline void eval_add(float128_backend& result, const A& a)
{
   result.value() += a;
}
inline void eval_subtract(float128_backend& result, const float128_backend& a)
{
   result.value() -= a.value();
}
template <class A>
inline void eval_subtract(float128_backend& result, const A& a)
{
   result.value() -= a;
}
inline void eval_multiply(float128_backend& result, const float128_backend& a)
{
   result.value() *= a.value();
}
template <class A>
inline void eval_multiply(float128_backend& result, const A& a)
{
   result.value() *= a;
}
inline void eval_divide(float128_backend& result, const float128_backend& a)
{
   result.value() /= a.value();
}
template <class A>
inline void eval_divide(float128_backend& result, const A& a)
{
   result.value() /= a;
}

inline void eval_add(float128_backend& result, const float128_backend& a, const float128_backend& b)
{
   result.value() = a.value() + b.value();
}
template <class A>
inline void eval_add(float128_backend& result, const float128_backend& a, const A& b)
{
   result.value() = a.value() + b;
}
inline void eval_subtract(float128_backend& result, const float128_backend& a, const float128_backend& b)
{
   result.value() = a.value() - b.value();
}
template <class A>
inline void eval_subtract(float128_backend& result, const float128_backend& a, const A& b)
{
   result.value() = a.value() - b;
}
template <class A>
inline void eval_subtract(float128_backend& result, const A& a, const float128_backend& b)
{
   result.value() = a - b.value();
}
inline void eval_multiply(float128_backend& result, const float128_backend& a, const float128_backend& b)
{
   result.value() = a.value() * b.value();
}
template <class A>
inline void eval_multiply(float128_backend& result, const float128_backend& a, const A& b)
{
   result.value() = a.value() * b;
}
inline void eval_divide(float128_backend& result, const float128_backend& a, const float128_backend& b)
{
   result.value() = a.value() / b.value();
}

template <class R>
inline void eval_convert_to(R* result, const float128_backend& val)
{
   *result = static_cast<R>(val.value());
}

inline void eval_frexp(float128_backend& result, const float128_backend& arg, int* exp)
{
   result.value() = frexpq(arg.value(), exp);
}

inline void eval_ldexp(float128_backend& result, const float128_backend& arg, int exp)
{
   result.value() = ldexpq(arg.value(), exp);
}

inline void eval_floor(float128_backend& result, const float128_backend& arg)
{
   result.value() = floorq(arg.value());
}
inline void eval_ceil(float128_backend& result, const float128_backend& arg)
{
   result.value() = ceilq(arg.value());
}
inline void eval_sqrt(float128_backend& result, const float128_backend& arg)
{
   result.value() = sqrtq(arg.value());
}
inline int eval_fpclassify(const float128_backend& arg)
{
   if(isnanq(arg.value()))
      return FP_NAN;
   else if(isinfq(arg.value()))
      return FP_INFINITE;
   else if(arg.value() == 0)
      return FP_ZERO;

   float128_backend t(arg);
   if(t.value() < 0)
      t.negate();
   if(t.value() < BOOST_MP_QUAD_MIN)
      return FP_SUBNORMAL;
   return FP_NORMAL;
}

inline void eval_increment(float128_backend& arg)
{
   ++arg.value();
}
inline void eval_decrement(float128_backend& arg)
{
   --arg.value();
}

/*********************************************************************
*
* abs/fabs:
*
*********************************************************************/

inline void eval_abs(float128_backend& result, const float128_backend& arg)
{
   result.value() = fabsq(arg.value());
}
inline void eval_fabs(float128_backend& result, const float128_backend& arg)
{
   result.value() = fabsq(arg.value());
}

/*********************************************************************
*
* Floating point functions:
*
*********************************************************************/

inline void eval_trunc(float128_backend& result, const float128_backend& arg)
{
   result.value() = truncq(arg.value());
}
/*
//
// This doesn't actually work... rely on our own default version instead.
//
inline void eval_round(float128_backend& result, const float128_backend& arg)
{
   if(isnanq(arg.value()) || isinf(arg.value()))
   {
      result = boost::math::policies::raise_rounding_error(
            "boost::multiprecision::trunc<%1%>(%1%)", 0,
            number<float128_backend, et_off>(arg),
            number<float128_backend, et_off>(arg),
            boost::math::policies::policy<>()).backend();
      return;
   }
   result.value() = roundq(arg.value());
}
*/

inline void eval_exp(float128_backend& result, const float128_backend& arg)
{
   result.value() = expq(arg.value());
}
inline void eval_log(float128_backend& result, const float128_backend& arg)
{
   result.value() = logq(arg.value());
}
inline void eval_log10(float128_backend& result, const float128_backend& arg)
{
   result.value() = log10q(arg.value());
}
inline void eval_sin(float128_backend& result, const float128_backend& arg)
{
   result.value() = sinq(arg.value());
}
inline void eval_cos(float128_backend& result, const float128_backend& arg)
{
   result.value() = cosq(arg.value());
}
inline void eval_tan(float128_backend& result, const float128_backend& arg)
{
   result.value() = tanq(arg.value());
}
inline void eval_asin(float128_backend& result, const float128_backend& arg)
{
   result.value() = asinq(arg.value());
}
inline void eval_acos(float128_backend& result, const float128_backend& arg)
{
   result.value() = acosq(arg.value());
}
inline void eval_atan(float128_backend& result, const float128_backend& arg)
{
   result.value() = atanq(arg.value());
}
inline void eval_sinh(float128_backend& result, const float128_backend& arg)
{
   result.value() = sinhq(arg.value());
}
inline void eval_cosh(float128_backend& result, const float128_backend& arg)
{
   result.value() = coshq(arg.value());
}
inline void eval_tanh(float128_backend& result, const float128_backend& arg)
{
   result.value() = tanhq(arg.value());
}
inline void eval_fmod(float128_backend& result, const float128_backend& a, const float128_backend& b)
{
   result.value() = fmodq(a.value(), b.value());
}
inline void eval_pow(float128_backend& result, const float128_backend& a, const float128_backend& b)
{
   result.value() = powq(a.value(), b.value());
}
inline void eval_atan2(float128_backend& result, const float128_backend& a, const float128_backend& b)
{
   result.value() = atan2q(a.value(), b.value());
}
#ifndef BOOST_MP_USE_QUAD
inline void eval_multiply_add(float128_backend& result, const float128_backend& a, const float128_backend& b, const float128_backend& c)
{
   result.value() = fmaq(a.value(), b.value(), c.value());
}
inline int eval_signbit BOOST_PREVENT_MACRO_SUBSTITUTION(const float128_backend& arg)
{
   return ::signbitq(arg.value());
}
#endif

inline std::size_t hash_value(const float128_backend& val)
{
   return  boost::hash_value(static_cast<double>(val.value()));
}

} // namespace backends

   template<boost::multiprecision::expression_template_option ExpressionTemplates>
   inline boost::multiprecision::number<float128_backend, ExpressionTemplates> asinh BOOST_PREVENT_MACRO_SUBSTITUTION(const boost::multiprecision::number<float128_backend, ExpressionTemplates>& arg)
   {
      return asinhq(arg.backend().value());
   }
   template<boost::multiprecision::expression_template_option ExpressionTemplates>
   inline boost::multiprecision::number<float128_backend, ExpressionTemplates> acosh BOOST_PREVENT_MACRO_SUBSTITUTION(const boost::multiprecision::number<float128_backend, ExpressionTemplates>& arg)
   {
      return acoshq(arg.backend().value());
   }
   template<boost::multiprecision::expression_template_option ExpressionTemplates>
   inline boost::multiprecision::number<float128_backend, ExpressionTemplates> atanh BOOST_PREVENT_MACRO_SUBSTITUTION(const boost::multiprecision::number<float128_backend, ExpressionTemplates>& arg)
   {
      return atanhq(arg.backend().value());
   }
   template<boost::multiprecision::expression_template_option ExpressionTemplates>
   inline boost::multiprecision::number<float128_backend, ExpressionTemplates> cbrt BOOST_PREVENT_MACRO_SUBSTITUTION(const boost::multiprecision::number<float128_backend, ExpressionTemplates>& arg)
   {
      return cbrtq(arg.backend().value());
   }
   template<boost::multiprecision::expression_template_option ExpressionTemplates>
   inline boost::multiprecision::number<float128_backend, ExpressionTemplates> erf BOOST_PREVENT_MACRO_SUBSTITUTION(const boost::multiprecision::number<float128_backend, ExpressionTemplates>& arg)
   {
      return erfq(arg.backend().value());
   }
   template<boost::multiprecision::expression_template_option ExpressionTemplates>
   inline boost::multiprecision::number<float128_backend, ExpressionTemplates> erfc BOOST_PREVENT_MACRO_SUBSTITUTION(const boost::multiprecision::number<float128_backend, ExpressionTemplates>& arg)
   {
      return erfcq(arg.backend().value());
   }
   template<boost::multiprecision::expression_template_option ExpressionTemplates>
   inline boost::multiprecision::number<float128_backend, ExpressionTemplates> expm1 BOOST_PREVENT_MACRO_SUBSTITUTION(const boost::multiprecision::number<float128_backend, ExpressionTemplates>& arg)
   {
      return expm1q(arg.backend().value());
   }
   template<boost::multiprecision::expression_template_option ExpressionTemplates>
   inline boost::multiprecision::number<float128_backend, ExpressionTemplates> lgamma BOOST_PREVENT_MACRO_SUBSTITUTION(const boost::multiprecision::number<float128_backend, ExpressionTemplates>& arg)
   {
      return lgammaq(arg.backend().value());
   }
   template<boost::multiprecision::expression_template_option ExpressionTemplates>
   inline boost::multiprecision::number<float128_backend, ExpressionTemplates> tgamma BOOST_PREVENT_MACRO_SUBSTITUTION(const boost::multiprecision::number<float128_backend, ExpressionTemplates>& arg)
   {
      return tgammaq(arg.backend().value());
   }
   template<boost::multiprecision::expression_template_option ExpressionTemplates>
   inline boost::multiprecision::number<float128_backend, ExpressionTemplates> log1p BOOST_PREVENT_MACRO_SUBSTITUTION(const boost::multiprecision::number<float128_backend, ExpressionTemplates>& arg)
   {
      return log1pq(arg.backend().value());
   }

#ifndef BOOST_MP_USE_QUAD
   template <multiprecision::expression_template_option ExpressionTemplates>
   inline boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> copysign BOOST_PREVENT_MACRO_SUBSTITUTION(const boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates>& a, const boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates>& b)
   {
      return ::copysignq(a.backend().value(), b.backend().value());
   }

   inline void eval_remainder(float128_backend& result, const float128_backend& a, const float128_backend& b)
   {
      result.value() = remainderq(a.value(), b.value());
   }
   inline void eval_remainder(float128_backend& result, const float128_backend& a, const float128_backend& b, int* pi)
   {
      result.value() = remquoq(a.value(), b.value(), pi);
   }
#endif

} // namespace multiprecision

namespace math {

   using boost::multiprecision::signbit;
   using boost::multiprecision::copysign;

} // namespace math

} // namespace boost

namespace boost{
namespace archive{

class binary_oarchive;
class binary_iarchive;

}

namespace serialization{ namespace float128_detail{

template <class Archive>
void do_serialize(Archive& ar, boost::multiprecision::backends::float128_backend& val, const mpl::false_&, const mpl::false_&)
{
   // saving
   // non-binary
   std::string s(val.str(0, std::ios_base::scientific));
   ar & s;
}
template <class Archive>
void do_serialize(Archive& ar, boost::multiprecision::backends::float128_backend& val, const mpl::true_&, const mpl::false_&)
{
   // loading
   // non-binary
   std::string s;
   ar & s;
   val = s.c_str();
}

template <class Archive>
void do_serialize(Archive& ar, boost::multiprecision::backends::float128_backend& val, const mpl::false_&, const mpl::true_&)
{
   // saving
   // binary
   ar.save_binary(&val, sizeof(val));
}
template <class Archive>
void do_serialize(Archive& ar, boost::multiprecision::backends::float128_backend& val, const mpl::true_&, const mpl::true_&)
{
   // loading
   // binary
   ar.load_binary(&val, sizeof(val));
}

} // detail

template <class Archive>
void serialize(Archive& ar, boost::multiprecision::backends::float128_backend& val, unsigned int /*version*/)
{
   typedef typename Archive::is_loading load_tag;
   typedef typename mpl::bool_<boost::is_same<Archive, boost::archive::binary_oarchive>::value || boost::is_same<Archive, boost::archive::binary_iarchive>::value> binary_tag;

   float128_detail::do_serialize(ar, val, load_tag(), binary_tag());
}

} // namepsace archive

} // namespace boost

namespace std{

template <boost::multiprecision::expression_template_option ExpressionTemplates>
class numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >
{
   typedef boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> number_type;
public:
   BOOST_STATIC_CONSTEXPR bool is_specialized = true;
   static number_type (min)() BOOST_NOEXCEPT { return BOOST_MP_QUAD_MIN; }
   static number_type (max)() BOOST_NOEXCEPT { return BOOST_MP_QUAD_MAX; }
   static number_type lowest() BOOST_NOEXCEPT { return -(max)(); }
   BOOST_STATIC_CONSTEXPR int digits = 113;
   BOOST_STATIC_CONSTEXPR int digits10 = 33;
   BOOST_STATIC_CONSTEXPR int max_digits10 = 36;
   BOOST_STATIC_CONSTEXPR bool is_signed = true;
   BOOST_STATIC_CONSTEXPR bool is_integer = false;
   BOOST_STATIC_CONSTEXPR bool is_exact = false;
   BOOST_STATIC_CONSTEXPR int radix = 2;
   static number_type epsilon() { return 1.92592994438723585305597794258492732e-34; /* this double value has only one bit set and so is exact */ }
   static number_type round_error() { return 0.5; }
   BOOST_STATIC_CONSTEXPR int min_exponent = -16381;
   BOOST_STATIC_CONSTEXPR int min_exponent10 = min_exponent * 301L / 1000L;
   BOOST_STATIC_CONSTEXPR int max_exponent = 16384;
   BOOST_STATIC_CONSTEXPR int max_exponent10 = max_exponent * 301L / 1000L;
   BOOST_STATIC_CONSTEXPR bool has_infinity = true;
   BOOST_STATIC_CONSTEXPR bool has_quiet_NaN = true;
   BOOST_STATIC_CONSTEXPR bool has_signaling_NaN = false;
   BOOST_STATIC_CONSTEXPR float_denorm_style has_denorm = denorm_present;
   BOOST_STATIC_CONSTEXPR bool has_denorm_loss = true;
   static number_type infinity() { return HUGE_VAL; /* conversion from double infinity OK */ }
   static number_type quiet_NaN() { return number_type("nan"); }
   static number_type signaling_NaN() { return 0; }
   static number_type denorm_min() { return BOOST_MP_QUAD_DENORM_MIN; }
   BOOST_STATIC_CONSTEXPR bool is_iec559 = true;
   BOOST_STATIC_CONSTEXPR bool is_bounded = false;
   BOOST_STATIC_CONSTEXPR bool is_modulo = false;
   BOOST_STATIC_CONSTEXPR bool traps = false;
   BOOST_STATIC_CONSTEXPR bool tinyness_before = false;
   BOOST_STATIC_CONSTEXPR float_round_style round_style = round_to_nearest;
};

template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::is_specialized;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::digits;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::digits10;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::max_digits10;

template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::is_signed;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::is_integer;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::is_exact;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::radix;

template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::min_exponent;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::max_exponent;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::min_exponent10;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST int numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::max_exponent10;

template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::has_infinity;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::has_quiet_NaN;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::has_signaling_NaN;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::has_denorm_loss;

template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::is_iec559;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::is_bounded;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::is_modulo;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::traps;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST bool numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::tinyness_before;

template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST float_round_style numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::round_style;
template <boost::multiprecision::expression_template_option ExpressionTemplates>
BOOST_CONSTEXPR_OR_CONST float_denorm_style numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::float128_backend, ExpressionTemplates> >::has_denorm;

} // namespace std

#endif


/**
 * Unit tests for the DFN KMeansClustering
 */

/**
 * @addtogroup DFNsTest
 * @{
 */

#include <catch.hpp>
#include <boost/assert.hpp>
#include <ImageFiltering/KMeansClustering.hpp>
#include <Converters/MatToFrameConverter.hpp>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace CDFF::DFN::ImageFiltering;
using namespace FrameWrapper;
using namespace Converters;
using namespace Array3DWrapper;
using namespace BaseTypesWrapper;

TEST_CASE( "Call to process (KMeansClustering)", "[process]" )
{
	// Prepare input data
    cv::FileStorage file_reader("../tests/Data/depthImg_Mana.yml.gz", cv::FileStorage::READ);
    ASSERT(file_reader.isOpened(), "Unable to input image file");
    cv::Mat input;
    file_reader.getFirstTopLevelNode() >> input;
    ASSERT(!input.empty(), "Something went wrong when reading the image");

    FrameSharedPtr inFrame = NewSharedFrame();
    Initialize(*inFrame);
    inFrame->metadata.mode = FrameMode::asn1Sccmode_XYZ;
    inFrame->metadata.status = FrameStatus::asn1Sccstatus_VALID;
    inFrame->data.rows = static_cast<T_UInt16>(input.rows);
    inFrame->data.cols = static_cast<T_UInt16>(input.cols);
    inFrame->data.rowSize = static_cast<T_UInt32>(input.step[0]);
    inFrame->data.channels = 3;
    inFrame->data.depth = Array3DDepth::asn1Sccdepth_32S;

    const size_t datasize = input.total() * input.elemSize();
    assert(sizeof(inFrame->data.data.arr) > datasize);
    std::copy(input.data, input.data + datasize, inFrame->data.data.arr);

    inFrame->data.data.nCount = static_cast<int>(input.total() * input.elemSize());

	// Instantiate DFN
	std::unique_ptr<KMeansClustering> kmeans_clustering (new KMeansClustering);

	// Send input data to DFN
	kmeans_clustering->imageInput(*inFrame);

	// Run DFN
	kmeans_clustering->process();

	// Query output data from DFN
	const Frame& output = kmeans_clustering->imageOutput();

	// Check the DFN output image is not empty
	BOOST_ASSERT(output.data.data.nCount > 0);
}

TEST_CASE( "Call to configure (KMeansClustering) ", "[configure]" )
{
    // Prepare input data
    cv::FileStorage file_reader("../tests/Data/depthImg_Mana.yml.gz", cv::FileStorage::READ);
    ASSERT(file_reader.isOpened(), "Unable to input image file");
    cv::Mat input;
    file_reader.getFirstTopLevelNode() >> input;
    ASSERT(!input.empty(), "Something went wrong when reading the image");

    FrameSharedPtr inFrame = NewSharedFrame();
    Initialize(*inFrame);
    inFrame->metadata.mode = FrameMode::asn1Sccmode_XYZ;
    inFrame->metadata.status = FrameStatus::asn1Sccstatus_VALID;
    inFrame->data.rows = static_cast<T_UInt16>(input.rows);
    inFrame->data.cols = static_cast<T_UInt16>(input.cols);
    inFrame->data.rowSize = static_cast<T_UInt32>(input.step[0]);
    inFrame->data.channels = 3;
    inFrame->data.depth = Array3DDepth::asn1Sccdepth_32S;

    const size_t datasize = input.total() * input.elemSize();
    assert(sizeof(inFrame->data.data.arr) > datasize);
    std::copy(input.data, input.data + datasize, inFrame->data.data.arr);

    inFrame->data.data.nCount = static_cast<int>(input.total() * input.elemSize());

    // Instantiate DFN
    std::unique_ptr<KMeansClustering> kmeans_clustering (new KMeansClustering);

    // Setup DFN
    kmeans_clustering->setConfigurationFile("../tests/ConfigurationFiles/DFNs/ImageFiltering/KMeansClustering.yaml");
    kmeans_clustering->configure();

    // Send input data to DFN
    kmeans_clustering->imageInput(*inFrame);

    // Run DFN
    kmeans_clustering->process();

    // Query output data from DFN
    const Frame& output = kmeans_clustering->imageOutput();

    // Check the DFN output image is not empty
    BOOST_ASSERT(output.data.data.nCount > 0);
}

/** @} */


//  Copyright (c)      2012 Daniel Kogler
//
//  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)

/*This benchmark measures the amount of time to join two threads together*/
#include "statstd.hpp"
#include <hpx/include/threadmanager.hpp>
#include <hpx/util/lightweight_test.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/lexical_cast.hpp>

//the function to be assigned to the thread.  It does nothing and therefore
//minimal extra time will be spent waiting for the thread to complete its
//workload before attempting to join to the main thread
void void_thread(){
}

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

//this runs a series of tests for a packaged_action.apply()
void run_tests(uint64_t);

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

///////////////////////////////////////////////////////////////////////////////
//necessary to run hpx
int hpx_main(variables_map& vm){
    uint64_t num = vm["number-spawned"].as<uint64_t>();
    csv = (vm.count("csv") ? true : false);
    run_tests(num);
    return hpx::finalize();
}

///////////////////////////////////////////////////////////////////////////////
int main(int argc, char* argv[]){
    // Configure application-specific options.
    options_description
        desc_commandline("usage: " HPX_APPLICATION_STRING " [options]");

    desc_commandline.add_options()
        ("number-spawned,N",
            boost::program_options::value<uint64_t>()
                ->default_value(50000),
            "number of created and joined")
        ("csv",
            "output results as csv "
            "(format:count,mean,accurate mean,variance,min,max)");

    // Initialize and run HPX
    outname = argv[0];
    return hpx::init(desc_commandline, argc, argv);
}

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

//measure how long it takes to spawn threads with a simple argumentless function
void run_tests(uint64_t num){
    uint64_t i = 0;
    double ot = timer_overhead(num);
    double mean2;
    vector<double> time;
    vector<hpx::thread> threads;
    threads.reserve(2*num);

    //first create a large number of threads
    //then measure how long it takes to join them back together
    for(; i < num; ++i)
        threads.push_back(hpx::thread(&void_thread));
    for(i = 0; i < num; i++){
        threads.push_back(hpx::thread(&void_thread));
    }

    //this is here simply to ensure that all other threads have completed the
    //assigned empty function; the scheduler should have had each of the other
    //threads run before this one
    hpx::thread temp(&void_thread);
    temp.join();

    //obtain the average join time
    high_resolution_timer t;
    for(i = 0; i < num; i++){
        threads[i].join();
    }
    mean2 = t.elapsed()/num;

    //obtain a statistical sampling of the join time
    time.reserve(num);
    string message = "Measuring how long it takes to join threads:";
    for(i = num; i < num+num; ++i){
        high_resolution_timer t1;
        threads[i].join();
        time.push_back(t1.elapsed());
    }
    printout(time, ot, mean2, message);
}


/*
 * ElevationMap.hpp
 *
 *  Created on: Dec 22, 2019
 *      Author: Peter XU
 *	 Institute: ZJU, CSC 104
 */

#pragma once

// Grid Map
#include <grid_map_ros/grid_map_ros.hpp>

// OpenCV
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <cv_bridge/cv_bridge.h>

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

// PCL
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

// Kindr
#include <kindr/Core>

// Boost
#include <boost/thread/recursive_mutex.hpp>

// ROS
#include <ros/ros.h>
#include <string>

using namespace std;

namespace elevation_mapping {
// typedef PointXYZRGBIC Anypoint;
// typedef pcl::PointCloud<Anypoint> pointCloud;

/*!
 * Elevation map stored as grid map handling elevation height, variance, color etc.
 */
class ElevationMap
{
 public:

  /*!
   * Constructor.
   */
  ElevationMap(ros::NodeHandle nodeHandle, string robot_name);

  /*!
   * Destructor.
   */
  virtual ~ElevationMap();

  /*!
   * Set the geometry of the elevation map. Clears all the data.
   * @param length the side lengths in x, and y-direction of the elevation map [m].
   * @param resolution the cell size in [m/cell].
   * @param position the 2d position of the elevation map in the elevation map frame [m].
   * @return true if successful.
   */
  void setGeometry(const grid_map::Length& length, const double& resolution, const grid_map::Position& position);

  /*!
   * Clears all data of the elevation map (data and time).
   * @return true if successful.
   */
  bool clear();

  /*!
   * Move the grid map w.r.t. to the grid map frame.
   * @param position the new location of the elevation map in the map frame.
   */

  void move(const grid_map::Index M_startindex, grid_map::Position M_position);

  void opt_move(grid_map::Position M_position, float update_height);
  /*!
   * Gets a reference to the raw grid map.
   * @return the raw grid map.
   */
  grid_map::GridMap& getRawGridMap();

  /*!
   * Gets a reference to the fused grid map.
   * @return the fused grid map.
   */
  grid_map::GridMap& getFusedGridMap();

  /*!
   * Gets the time of last map update.
   * @return time of the last map update.
   */
  ros::Time getTimeOfLastUpdate();

  /*!
   * Gets the time of last map fusion.
   * @return time of the last map fusion.
   */
  ros::Time getTimeOfLastFusion();

  /*!
   * Get the pose of the elevation map frame w.r.t. the inertial parent frame of the robot (e.g. world, map etc.).
   * @return pose of the elevation map frame w.r.t. the parent frame of the robot.
   */
  const kindr::HomTransformQuatD& getPose();

  /*!
   * Gets the position of a raw data point (x, y of cell position & height of cell value) in
   * the parent frame of the robot.
   * @param index the index of the requested cell.
   * @param position the position of the data point in the parent frame of the robot.
   * @return true if successful, false if no valid data available.
   */
  bool getPosition3dInRobotParentFrame(const Eigen::Array2i& index, kindr::Position3D& position);

  /*!
   * Gets the fused data mutex.
   * @return reference to the fused data mutex.
   */
  boost::recursive_mutex& getFusedDataMutex();

  /*!
   * Gets the raw data mutex.
   * @return reference to the raw data mutex.
   */
  boost::recursive_mutex& getRawDataMutex();

  /*!
   * Set the frame id.
   * @param frameId the frame id.
   */
  void setFrameId(const std::string& frameId);

  /*!
   * Get the frame id.
   * @return the frameId.
   */
  const std::string& getFrameId();

  /*!
   * Reformat robot name.
   * @return the frameId.
   */
  std::string reformatFrameId(std::string robotName);

  friend class ElevationMapping;

 private:

  /*!
   * Reads and verifies the ROS parameters.
   * @return true if successful.
   */
  bool readParameters();

  sensor_msgs::ImagePtr show(ros::Time timestamp, std::string robot_name, float trackPointTransformed_x, float trackPointTransformed_y, int length, float *elevation, float *var, int *point_colorR, int *point_colorG, int *point_colorB, float *rough, float *slope, float *traver, float *intensity);

  //! ROS nodehandle.
  ros::NodeHandle nodeHandle_;

  //! Raw elevation map as grid map.
  grid_map::GridMap rawMap_;

  grid_map::GridMap visualMap_;

  //! Underlying map, used for ground truth maps, multi-robot mapping etc.
  grid_map::GridMap underlyingMap_;

  //! True if underlying map has been set, false otherwise.
  bool hasUnderlyingMap_;

  //! Pose of the elevation map frame w.r.t. the inertial parent frame of the robot (e.g. world, map etc.).
  kindr::HomTransformQuatD pose_;

  //! ROS publishers.
  ros::Publisher elevationMapRawPublisher_;

  ros::Publisher visualMapPublisher_;
  ros::Publisher VpointsPublisher_;

  //! Underlying map subscriber.
  ros::Subscriber underlyingMapSubscriber_;

  //! Initial ros time
  ros::Time initialTime_;
  ros::Publisher orthomosaicPublisher_;

  std::string orthoDir;
  std::string robot_id;
  std::string robot_name_;
  std::string frame_id;

  //! Parameters. Are set through the ElevationMapping class.
  double minVariance_;
  double maxVariance_;
  double mahalanobisDistanceThreshold_;
  double multiHeightNoise_;
  double minHorizontalVariance_;
  double maxHorizontalVariance_;
  std::string underlyingMapTopic_;
  bool enableVisibilityCleanup_;
  double visibilityCleanupDuration_;
  double scanningDuration_;
};

} /* namespace */


#ifndef __fovis_rectification_hpp__
#define __fovis_rectification_hpp__

#include <inttypes.h>

#include <Eigen/Core>

#include "camera_intrinsics.hpp"

namespace fovis
{

/**
 * \ingroup FovisCore
 * \brief Maps image coordinates from an input image to image coordinates on a
 * rectified camera.
 *
 * A Rectification object represents the computation required to map pixels
 * from an input camera to pixels on a "rectified" virtual camera that shares
 * the same focal point, but may be rotated and have different projection
 * parameters (e.g., focal length, center of projection, etc.)
 *
 * This class is primary useful with stereo cameras to rectify pixels on both
 * input cameras to rectified cameras that share an image plane and look/up
 * vectors.  It can also be used for simple undistortion (e.g., no rotation,
 * and a rectified camera that has the same projection parameters as the input
 * camera, but with no distortion).
 *
 * For fast rectification, the Rectification object precomputes a mapping from
 * (u, v) image pixel coordinates to u', v' rectified image pixel coordinates.
 * The exact transformation from (u, v) to (u', v') is:
 *
 * -# undistort according to a plumb-bob distortion model
 * -# rotate points about the origin
 * -# reproject onto a rectified image plane.
 *
 * Rectification of pixels is done by bilinear interpolation on this
 * precomputed mapping.
 *
 */
class Rectification
{
  public:

    /**
     * Convenience constructor to create a Rectification object that simply
     * undistorts.  The rotation is set to identity, and the rectified camera
     * parameters are identical to the input camera parameters, but have no
     * distortion.
     */
    Rectification(const CameraIntrinsicsParameters& input_camera_params);

    /**
     * Constructor.  The rectified_camera_params are not allowed to have any
     * distortion.
     */
    Rectification(const CameraIntrinsicsParameters& input_camera_params,
            const Eigen::Matrix3d& rotation,
            const CameraIntrinsicsParameters& rectified_camera_params);

    ~Rectification();

    /**
     * \return the intrisic parameters of the input camera.
     */
    const CameraIntrinsicsParameters& getInputCameraParameters() const {
      return _input_camera;
    }

    /**
     * \return the rotation to apply between the undistorted input camera frame
     * and the output rectified camera frame.
     */
    const Eigen::Matrix3d& getRectificationRotation() const {
      return *_rotation;
    }

    /**
     * \return the projection parameters of the rectified camera.  The
     * distortion coefficients will always be zero.  Note that the focal length
     * and image dimensions of the rectified camera do not have to match those
     * of the input camera.
     */
    const CameraIntrinsicsParameters& getRectifiedCameraParameters() const {
      return _rectified_camera;
    }

    /**
     * Computes the undistorted image coordinates of the input distorted
     * coordinates (\p dist_u, \p dist_v).
     *
     * \param dist_u input distorted pixel u/x coordinate.
     * \param dist_v input distorted pixel v/y coordinate.
     * \param rect_uv output parameter.
     */
    void rectifyLookup(int dist_u, int dist_v,
                       Eigen::Vector2d* rect_uv) const
    {
      assert(dist_u >= 0 && dist_v >= 0 && dist_u < _input_camera.width && dist_v < _input_camera.height);
      int pixel_index = dist_v * _input_camera.width + dist_u;
      rect_uv->x() = _map_x[pixel_index];
      rect_uv->y() = _map_y[pixel_index];
    }

    /**
     * Computes the undistorted image coordinates of an input pixel specified
     * by its row-major pixel index.
     *
     * \param pixel_index corresponds to \f$ u * width + v \f$
     * \param rect_uv output parameter.
     *
     * \sa rectifyLookup
     */
    void rectifyLookupByIndex(int pixel_index,
                              Eigen::Vector2d* rect_uv) const
    {
      rect_uv->x() = _map_x[pixel_index];
      rect_uv->y() = _map_y[pixel_index];
    }

    /**
     * Computes the undistorted image coordinates of an input pixel via
     * bilinear interpolation of its integer-coordinate 4-neighboors.
     *
     * \param dist_uv input distorted pixel coordinates.
     * \param rect_uv output parameter.
     */
    void rectifyBilinearLookup(const Eigen::Vector2d& dist_uv,
                               Eigen::Vector2d* rect_uv) const {
      int u = (int)dist_uv.x();
      int v = (int)dist_uv.y();

      assert(u >= 0 && v >= 0 && u < _input_camera.width && v < _input_camera.height);

      // weights
      float wright  = (dist_uv.x() - u);
      float wbottom = (dist_uv.y() - v);
      float w[4] = {
        (1 - wright) * (1 - wbottom),
        wright * (1 - wbottom),
        (1 - wright) * wbottom,
        wright * wbottom
      };

      int ra_index = v * _input_camera.width + u;
      uint32_t neighbor_indices[4] = {
        ra_index,
        ra_index + 1,
        ra_index + _input_camera.width,
        ra_index + _input_camera.width + 1
      };

      rect_uv->x() = 0;
      rect_uv->y() = 0;
      for(int i = 0; i < 4; ++i) {
        rect_uv->x() += w[i] * _map_x[neighbor_indices[i]];
        rect_uv->y() += w[i] * _map_y[neighbor_indices[i]];
      }
    }

    /**
     * \return a deep copy of this object.
     */
    Rectification* makeCopy() const;

  private:
    Rectification() {}

    void populateMap();

    CameraIntrinsicsParameters _input_camera;

    Eigen::Matrix3d* _rotation;
    // use a pointer above to avoid EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    CameraIntrinsicsParameters _rectified_camera;

    float* _map_x;
    float* _map_y;
};

} /*  */

#endif


/** This is free and unencumbered software released into the public domain.

The authors of ISIS do not claim copyright on the contents of this file.
For more details about the LICENSE terms and the AUTHORS, you will
find files of those names at the top level of this repository. **/

/* SPDX-License-Identifier: CC0-1.0 */

#include <boost/bimap.hpp>
#include <boost/foreach.hpp>
#include "opencv2/opencv.hpp"
#include "opencv2/xfeatures2d.hpp"

#include "ORBAlgorithm.h"
#include <QVariant>

namespace Isis {

  /**
   * Constructs the ORB algorithm with default variables.
   */
  ORBAlgorithm::ORBAlgorithm() :
                 Feature2DAlgorithm("ORB", "Feature2D",
                                     ORBType::create()) {
    setupTypeMap();
    m_variables.merge( getAlgorithmVariables() );
  }

  /**
   * Constructs the ORB algorithm with the input variables
   *
   * @param cvars  The variables and values the algorithm will use.
   *               Variables that are not included will be set to their default.
   * @param config The config string used to construct cvars.
   */
  ORBAlgorithm::ORBAlgorithm(const PvlFlatMap &cvars, const QString &config) :
                  Feature2DAlgorithm("ORB", "Feature2D",
                                      ORBType::create(), cvars) {
    setupTypeMap();
    setConfig(config);
    setAlgorithmVariables(cvars);
    m_variables.merge(getAlgorithmVariables());
  }

  /**
   * Destroys the ORB algorithm
   */
  ORBAlgorithm::~ORBAlgorithm() { }

  /**
   * Returns a description of the ORB algorithm.
   *
   * @return @b QString A description of the ORB algorithm.
   */
  QString ORBAlgorithm::description() const {
    QString desc = "The OpenCV ORB Feature2D detector/extractor algorithm."
                   " See the documentation at "
      "http://docs.opencv.org/3.1.0/db/d95/classcv_1_1ORB.html";
    return (desc);
  }

  /**
   * Fills the map for converting the type variable.
   */
  void ORBAlgorithm::setupTypeMap() {
    m_typeMap.left.insert(std::pair<QString,int>("kBytes",      32));
    m_typeMap.left.insert(std::pair<QString,int>("HARRIS_SCORE", 0));
    m_typeMap.left.insert(std::pair<QString,int>("FAST_SCORE",   1));
  }

  /**
   * Creates an instance of the ORB algorithm.
   *
   * @param cvars  The variables and values the algorithm will use.
   *               Variables that are not included will be set to their default.
   * @param config The config string used to construct cvars.
   */
  Feature2DAlgorithm *ORBAlgorithm::create(const PvlFlatMap &vars, const QString &config) {
    return ( new ORBAlgorithm(vars, config) );
  }

  /**
   * Returns true if the algorithm has a detector.
   *
   * @return @b true if the algorithm has a detector.
   */
  bool ORBAlgorithm::hasDetector() const {
    return true;
  }

  /**
   * Returns true if the algorithm has an extractor.
   *
   * @return @b true if the algorithm has an extractor.
   */
  bool ORBAlgorithm::hasExtractor() const {
    return true;
  }

  /**
   * Returns true if the algorithm has a matcher.
   *
   * @return @b true if the algorithm has a matcher.
   */
  bool ORBAlgorithm::hasMatcher() const {
    return false;
  }

  /**
   * Returns the variables and their values used by the ORB algorithm.
   *
   * @return @b PvlFlatMap The variables and their values as keyword, value pairs.
   */
  PvlFlatMap ORBAlgorithm::getAlgorithmVariables( ) const {
    ORBPtr algorithm = m_algorithm.dynamicCast<ORBType>();

    PvlFlatMap variables;
    variables.add("nfeatures",     toString(algorithm->getMaxFeatures()));
    variables.add("scaleFactor",   toString(algorithm->getScaleFactor()));
    variables.add("nlevels",       toString(algorithm->getNLevels()));
    variables.add("edgeThreshold", toString(algorithm->getEdgeThreshold()));
    variables.add("firstLevel",    toString(algorithm->getFirstLevel()));
    variables.add("WTA_K",         toString(algorithm->getWTA_K()));
    variables.add("scoreType",     m_typeMap.right.at(algorithm->getScoreType()));
    variables.add("patchSize",     toString(algorithm->getPatchSize()));
    variables.add("fastThreshold", toString(algorithm->getFastThreshold()));
    return (variables);
  }

/**
 * Set parameters as provided by the variables
 *
 * @param variables Container of parameters to set
 *
 * @return @b int Number of variables actually set
 *
 * @throws IException::User "The input value is not valid for ORB's [scoreType] variable"
 */
  int ORBAlgorithm::setAlgorithmVariables(const PvlFlatMap &variables) {

    ORBPtr algorithm = m_algorithm.dynamicCast<ORBType>();

    int numSet(0);
    if ( variables.exists("nfeatures") ) {
      algorithm->setMaxFeatures(toInt(variables.get("nfeatures")));
      numSet++;
    }

    if ( variables.exists("scaleFactor") ) {
      algorithm->setScaleFactor(toDouble(variables.get("scaleFactor")));
      numSet++;
    }

     if ( variables.exists("nlevels") ) {
      algorithm->setNLevels(toInt(variables.get("nlevels")));
      numSet++;
     }

     if ( variables.exists("edgeThreshold") ) {
      algorithm->setEdgeThreshold(toInt(variables.get("edgeThreshold")));
      numSet++;
     }

     if ( variables.exists("firstLevel") ) {
      algorithm->setFirstLevel(toInt(variables.get("firstLevel")));
      numSet++;
     }

     if ( variables.exists("WTA_K") ) {
      algorithm->setWTA_K(toInt(variables.get("WTA_K")));
      numSet++;
     }

     if ( variables.exists("scoreType") ) {
       QString value = variables.get("scoreType");
       bool isInt;
       // Check if the value is an integer
       int intValue = value.toInt(&isInt);
       try {
         if (isInt) {
           // If it is an integer make sure it is a valid option
           m_typeMap.right.at(intValue);
         }
         else {
           // If it is a string, then convert it to an integer
           intValue = m_typeMap.left.at(value);
         }
       }
       catch (std::exception &e) {
         QString msg = "The input value [" + value +
                       "] is not valid for ORB's [Type] variable";
         throw IException(IException::User, msg, _FILEINFO_);
       }

      algorithm->setScoreType(ORBType::ScoreType(intValue));
      numSet++;

     if ( variables.exists("patchSize") ) {
      algorithm->setPatchSize(toInt(variables.get("patchSize")));
      numSet++;
     }

     if ( variables.exists("fastThreshold") ) {
      algorithm->setPatchSize(toInt(variables.get("fastThreshold")));
      numSet++;
     }
   }
   return (numSet);
  }

};  // namespace Isis


#include "sbs/rendering/pick.h"

#include "sbs/common/mesh.h"
#include "sbs/rendering/renderer.h"

#include <Eigen/LU>
#include <GLFW/glfw3.h>
#include <iostream>

namespace sbs {
namespace rendering {

picker_t::picker_t(
    renderer_t const* renderer,
    std::vector<common::shared_vertex_surface_mesh_i*> const& nodes)
    : renderer_(renderer),
      nodes_(nodes),
      is_node_picked_(nodes.size(), false),
      picked_vertices_(nodes.size(), 0u),
      is_picking_{false},
      xprev_{0.},
      yprev_{0.}
{
}

void picker_t::mouse_button_pressed_event(GLFWwindow* window, int button, int action, int mods)
{
    if (!should_picking_start)
        return;

    is_picking_ = should_picking_start(button, action, mods);
    if (is_picking_)
    {
        if (!should_pick)
            return;

        int viewport_gl[4];
        glGetIntegerv(GL_VIEWPORT, viewport_gl);
        Eigen::Vector4d const viewport{
            static_cast<double>(viewport_gl[0]),
            static_cast<double>(viewport_gl[1]),
            static_cast<double>(viewport_gl[2]),
            static_cast<double>(viewport_gl[3])};

        float const aspect_ratio =
            static_cast<float>(viewport(2)) / static_cast<float>(viewport(3));
        Eigen::Matrix4d const projection = renderer_->camera().projection_matrix(aspect_ratio);
        Eigen::Matrix4d const view       = renderer_->camera().view_matrix();

        double x, y;
        glfwGetCursorPos(window, &x, &y);
        xprev_ = x;
        yprev_ = y;

        auto const ray = sbs::rendering::unproject_ray({x, y}, viewport, projection, view);

        for (std::size_t i = 0u; i < nodes_.size(); ++i)
        {
            auto const& node = nodes_[i];

            if (!should_pick(node))
                continue;

            auto const vid = rendering::pick_vertex(ray, *node);
            if (!vid.has_value())
                continue;

            is_node_picked_[i]  = true;
            picked_vertices_[i] = *vid;

            if (!picked)
                continue;

            picked(node, *vid);
        }
    }
    else
    {
        unpick();
    }
}

void picker_t::mouse_moved_event(GLFWwindow* window, double x, double y)
{
    if (!should_picking_stop)
    {
        return;
    }

    if (!is_picking_)
    {
        return;
    }

    bool const left_mouse_button_pressed =
        glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS;
    bool const right_mouse_button_pressed =
        glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS;
    bool const middle_mouse_button_pressed =
        glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE) == GLFW_PRESS;

    bool const alt_key_pressed = glfwGetKey(window, GLFW_KEY_LEFT_ALT) == GLFW_PRESS ||
                                 glfwGetKey(window, GLFW_KEY_RIGHT_ALT) == GLFW_PRESS;
    bool const ctrl_key_pressed = glfwGetKey(window, GLFW_KEY_LEFT_CONTROL) == GLFW_PRESS ||
                                  glfwGetKey(window, GLFW_KEY_RIGHT_CONTROL) == GLFW_PRESS;
    bool const shift_key_pressed = glfwGetKey(window, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS ||
                                   glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT) == GLFW_PRESS;

    is_picking_ = !should_picking_stop(
        left_mouse_button_pressed,
        right_mouse_button_pressed,
        middle_mouse_button_pressed,
        alt_key_pressed,
        ctrl_key_pressed,
        shift_key_pressed);

    if (!is_picking_)
    {
        unpick();
    }
    else
    {
        int viewport_gl[4];
        glGetIntegerv(GL_VIEWPORT, viewport_gl);
        Eigen::Vector4d const viewport{
            static_cast<double>(viewport_gl[0]),
            static_cast<double>(viewport_gl[1]),
            static_cast<double>(viewport_gl[2]),
            static_cast<double>(viewport_gl[3])};

        float const aspect_ratio =
            static_cast<float>(viewport(2)) / static_cast<float>(viewport(3));
        Eigen::Matrix4d const projection = renderer_->camera().projection_matrix(aspect_ratio);
        Eigen::Matrix4d const view       = renderer_->camera().view_matrix();

        for (std::size_t i = 0u; i < nodes_.size(); ++i)
        {
            if (!is_node_picked_[i])
                continue;

            if (!on_picker_moved)
                continue;

            common::point_t const p1 = rendering::unproject(
                Eigen::Vector3d{xprev_, yprev_, 0.},
                viewport,
                projection,
                view);
            common::point_t const p2 =
                rendering::unproject(Eigen::Vector3d{x, y, 0.}, viewport, projection, view);
            Eigen::Vector3d const d{p2.x() - p1.x(), p2.y() - p1.y(), p2.z() - p1.z()};

            double const dx = x - xprev_;
            double const dy = y - yprev_;

            on_picker_moved(dx, dy, d, nodes_[i], picked_vertices_[i]);
        }
    }

    xprev_ = x;
    yprev_ = y;
}

bool picker_t::is_picking() const
{
    return is_picking_;
}

bool picker_t::is_usable() const
{
    return static_cast<bool>(should_picking_start) && static_cast<bool>(should_picking_stop) &&
           static_cast<bool>(should_pick);
}

void picker_t::unpick()
{
    for (std::size_t i = 0u; i < nodes_.size(); ++i)
    {
        if (is_node_picked_[i])
        {
            is_node_picked_[i] = false;
            if (unpicked)
            {
                unpicked(nodes_[i], picked_vertices_[i]);
            }
        }
    }
}

sbs::common::point_t unproject(
    Eigen::Vector3d const& coords,
    Eigen::Vector4d const& viewport,
    Eigen::Matrix4d const& projection,
    Eigen::Matrix4d const& view)
{
    double const x = (coords.x() - viewport(0)) / viewport(2);
    double const y = 1. - (coords.y() - viewport(1)) / viewport(3);

    Eigen::Vector4d const projective_screen_space_point =
        Eigen::Vector4d{x, y, coords.z(), 1.}.array() * 2. - 1.;

    Eigen::Matrix4d const screen_to_world_transform = (projection * view).inverse();

    Eigen::Vector4d const projective_world_space_point =
        screen_to_world_transform * projective_screen_space_point;

    Eigen::Vector3d const world_space_point =
        (projective_world_space_point / projective_world_space_point.w()).head(3);

    return world_space_point;
};

sbs::common::ray_t unproject_ray(
    Eigen::Vector2d const& coords,
    Eigen::Vector4d const& viewport,
    Eigen::Matrix4d const& projection,
    Eigen::Matrix4d const& view)
{
    Eigen::Vector3d const win_source(coords.x(), coords.y(), 0.);
    Eigen::Vector3d const win_dest(coords.x(), coords.y(), 1.);

    Eigen::Vector3d const source = unproject(win_source, viewport, projection, view);
    Eigen::Vector3d const dest   = unproject(win_dest, viewport, projection, view);
    Eigen::Vector3d const dir    = dest - source;

    sbs::common::ray_t const ray{source, dir};
    return ray;
}

std::optional<std::tuple<std::uint32_t, double, double, double>>
pick(common::ray_t const& ray, common::shared_vertex_surface_mesh_i const& mesh)
{
    using return_type = std::optional<std::tuple<std::uint32_t, double, double, double>>;

    std::size_t const num_triangles = mesh.triangle_count();

    std::vector<std::tuple<std::uint32_t, double, double, double>> intersected_triangles{};

    for (std::size_t f = 0u; f < num_triangles; ++f)
    {
        auto const t  = mesh.triangle(f);
        auto const v1 = mesh.vertex(t.vertices[0u]);
        auto const v2 = mesh.vertex(t.vertices[1u]);
        auto const v3 = mesh.vertex(t.vertices[2u]);

        auto const& a = common::point_t{v1.position};
        auto const& b = common::point_t{v2.position};
        auto const& c = common::point_t{v3.position};

        common::triangle_t const triangle{a, b, c};
        auto const intersection = common::intersect_twoway(ray, triangle);

        if (!intersection.has_value())
            continue;

        auto const [u, v, w] = common::barycentric_coordinates(a, b, c, intersection.value());

        intersected_triangles.push_back(std::make_tuple(static_cast<std::uint32_t>(f), u, v, w));
    }

    if (intersected_triangles.empty())
        return {};

    std::sort(
        intersected_triangles.begin(),
        intersected_triangles.end(),
        [&ray, &mesh](auto const& intersection1, auto const& intersection2) {
            auto const& [f1, u1, v1, w1] = intersection1;
            auto const triangle1         = mesh.triangle(f1);
            auto const vertex1_t1        = mesh.vertex(triangle1.vertices[0u]);
            auto const vertex2_t1        = mesh.vertex(triangle1.vertices[1u]);
            auto const vertex3_t1        = mesh.vertex(triangle1.vertices[2u]);

            common::triangle_t const triangle_primitive1{
                Eigen::Vector3d{vertex1_t1.position},
                Eigen::Vector3d{vertex2_t1.position},
                Eigen::Vector3d{vertex3_t1.position}};

            auto const& [f2, u2, v2, w2] = intersection2;
            auto const triangle2         = mesh.triangle(f2);
            auto const vertex1_t2        = mesh.vertex(triangle2.vertices[0u]);
            auto const vertex2_t2        = mesh.vertex(triangle2.vertices[1u]);
            auto const vertex3_t2        = mesh.vertex(triangle2.vertices[2u]);

            common::triangle_t const triangle_primitive2{
                Eigen::Vector3d{vertex1_t2.position},
                Eigen::Vector3d{vertex2_t2.position},
                Eigen::Vector3d{vertex3_t2.position}};

            common::point_t const p1 = u1 * triangle_primitive1.a() + v1 * triangle_primitive1.b() +
                                       w1 * triangle_primitive1.c();
            common::point_t const p2 = u2 * triangle_primitive2.a() + v2 * triangle_primitive2.b() +
                                       w2 * triangle_primitive2.c();

            return (p1 - ray.p).squaredNorm() < (p2 - ray.p).squaredNorm();
        });

    return intersected_triangles.front();
}

std::optional<std::uint32_t>
pick_vertex(common::ray_t const& ray, common::shared_vertex_surface_mesh_i const& mesh)
{
    auto const picked_triangle = pick(ray, mesh);
    if (!picked_triangle.has_value())
        return {};

    auto const& [f, u, v, w] = picked_triangle.value();
    std::uint32_t vi         = mesh.triangle(f).vertices[0u];
    if (v > u && v > w)
        vi = mesh.triangle(f).vertices[1u];
    if (w > u && w > v)
        vi = mesh.triangle(f).vertices[2u];

    return vi;
};

} // namespace rendering
} // namespace sbs