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

/* Author: Ioan Sucan */

#include "geometric_shapes/bodies.h"
#include "geometric_shapes/body_operations.h"
#include "geometric_shapes/check_isometry.h"

#include <console_bridge/console.h>

extern "C" {
#ifdef GEOMETRIC_SHAPES_HAVE_QHULL_2011
#include <libqhull/libqhull.h>
#include <libqhull/mem.h>
#include <libqhull/qset.h>
#include <libqhull/geom.h>
#include <libqhull/merge.h>
#include <libqhull/poly.h>
#include <libqhull/io.h>
#include <libqhull/stat.h>
#else
#include <qhull/qhull.h>
#include <qhull/mem.h>
#include <qhull/qset.h>
#include <qhull/geom.h>
#include <qhull/merge.h>
#include <qhull/poly.h>
#include <qhull/io.h>
#include <qhull/stat.h>
#endif
}

#include <boost/math/constants/constants.hpp>
#include <limits>
#include <cstdio>
#include <cmath>  // std::fmin, std::fmax
#include <algorithm>
#include <Eigen/Geometry>
#include <unordered_map>
#include <mutex>

namespace bodies
{
namespace detail
{
static const double ZERO = 1e-9;

/** \brief Compute the square of the distance between a ray and a point
    Note: this requires 'dir' to be normalized */
static inline double distanceSQR(const Eigen::Vector3d& p, const Eigen::Vector3d& origin, const Eigen::Vector3d& dir)
{
  Eigen::Vector3d a = p - origin;
  double d = dir.normalized().dot(a);
  return a.squaredNorm() - d * d;
}

// temp structure for intersection points (used for ordering them)
struct intersc
{
  intersc(const Eigen::Vector3d& _pt, const double _tm) : pt(_pt), time(_tm)
  {
  }

  Eigen::Vector3d pt;
  double time;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

// define order on intersection points
struct interscOrder
{
  bool operator()(const intersc& a, const intersc& b) const
  {
    return a.time < b.time;
  }
};

/**
 * \brief Take intersections points in ipts and add them to intersections, filtering duplicates.
 * \param ipts The source list of intersections (will be modified (sorted)).
 * \param intersections The output list of intersection points.
 * \param count The maximum count of returned intersection points. 0 = return all points.
 */
void filterIntersections(std::vector<detail::intersc>& ipts, EigenSTL::vector_Vector3d* intersections,
                         const size_t count)
{
  if (intersections == nullptr || ipts.empty())
    return;

  std::sort(ipts.begin(), ipts.end(), interscOrder());
  const auto n = count > 0 ? std::min<size_t>(count, ipts.size()) : ipts.size();

  for (const auto& p : ipts)
  {
    if (intersections->size() == n)
      break;
    if (!intersections->empty() && p.pt.isApprox(intersections->back(), ZERO))
      continue;
    intersections->push_back(p.pt);
  }
}

// HACK: The global map g_triangle_for_plane_ is needed for ABI compatibility with the melodic version of
// geometric_shapes; in newer releases, it should instead be added as a member to ConvexMesh::MeshData.
std::unordered_map<const ConvexMesh*, std::map<size_t, size_t>> g_triangle_for_plane_;
std::mutex g_triangle_for_plane_mutex;  //!< Lock this mutex every time you work with g_triangle_for_plane_.
static std::map<size_t, size_t>& getTriangleForPlane(const ConvexMesh* mesh)
{
  std::lock_guard<std::mutex> lock(g_triangle_for_plane_mutex);
  auto it = g_triangle_for_plane_.find(mesh);
  if (it == detail::g_triangle_for_plane_.end())
    return detail::g_triangle_for_plane_.emplace(mesh, std::map<size_t, size_t>()).first->second;
  else
    return it->second;
}
}  // namespace detail

inline Eigen::Vector3d normalize(const Eigen::Vector3d& dir)
{
  const double norm = dir.squaredNorm();
#if EIGEN_VERSION_AT_LEAST(3, 3, 0)
  return ((norm - 1) > 1e-9) ? (dir / Eigen::numext::sqrt(norm)) : dir;
#else  // used in kinetic
  return ((norm - 1) > 1e-9) ? (dir / sqrt(norm)) : dir;
#endif
}
}  // namespace bodies

void bodies::Body::setDimensions(const shapes::Shape* shape)
{
  setDimensionsDirty(shape);
  updateInternalData();
}

bool bodies::Body::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
                                     Eigen::Vector3d& result) const
{
  BoundingSphere bs;
  computeBoundingSphere(bs);
  for (unsigned int i = 0; i < max_attempts; ++i)
  {
    result = Eigen::Vector3d(rng.uniformReal(bs.center.x() - bs.radius, bs.center.x() + bs.radius),
                             rng.uniformReal(bs.center.y() - bs.radius, bs.center.y() + bs.radius),
                             rng.uniformReal(bs.center.z() - bs.radius, bs.center.z() + bs.radius));
    if (containsPoint(result))
      return true;
  }
  return false;
}

bool bodies::Sphere::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const
{
  return (center_ - p).squaredNorm() <= radius2_;
}

void bodies::Sphere::useDimensions(const shapes::Shape* shape)  // radius
{
  radius_ = static_cast<const shapes::Sphere*>(shape)->radius;
}

std::vector<double> bodies::Sphere::getDimensions() const
{
  std::vector<double> d(1, radius_);
  return d;
}

void bodies::Sphere::updateInternalData()
{
  const auto tmpRadiusU = radius_ * scale_ + padding_;
  if (tmpRadiusU < 0)
    throw std::runtime_error("Sphere radius must be non-negative.");
  radiusU_ = tmpRadiusU;
  radius2_ = radiusU_ * radiusU_;
  center_ = pose_.translation();
}

std::shared_ptr<bodies::Body> bodies::Sphere::cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const
{
  auto s = std::allocate_shared<Sphere>(Eigen::aligned_allocator<Sphere>());
  s->radius_ = radius_;
  s->padding_ = padding;
  s->scale_ = scale;
  s->pose_ = pose;
  s->updateInternalData();
  return s;
}

double bodies::Sphere::computeVolume() const
{
  return 4.0 * boost::math::constants::pi<double>() * radiusU_ * radiusU_ * radiusU_ / 3.0;
}

void bodies::Sphere::computeBoundingSphere(BoundingSphere& sphere) const
{
  sphere.center = center_;
  sphere.radius = radiusU_;
}

void bodies::Sphere::computeBoundingCylinder(BoundingCylinder& cylinder) const
{
  cylinder.pose = pose_;
  cylinder.radius = radiusU_;
  cylinder.length = 2.0 * radiusU_;
}

void bodies::Sphere::computeBoundingBox(bodies::AABB& bbox) const
{
  bbox.setEmpty();

  // it's a sphere, so we do not rotate the bounding box
  Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
  transform.translation() = getPose().translation();

  bbox.extendWithTransformedBox(transform, Eigen::Vector3d(2 * radiusU_, 2 * radiusU_, 2 * radiusU_));
}

bool bodies::Sphere::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
                                       Eigen::Vector3d& result) const
{
  for (unsigned int i = 0; i < max_attempts; ++i)
  {
    const double minX = center_.x() - radiusU_;
    const double maxX = center_.x() + radiusU_;
    const double minY = center_.y() - radiusU_;
    const double maxY = center_.y() + radiusU_;
    const double minZ = center_.z() - radiusU_;
    const double maxZ = center_.z() + radiusU_;
    // we are sampling in a box; the probability of success after 20 attempts is 99.99996% given the ratio of box volume
    // to sphere volume
    for (int j = 0; j < 20; ++j)
    {
      result = Eigen::Vector3d(rng.uniformReal(minX, maxX), rng.uniformReal(minY, maxY), rng.uniformReal(minZ, maxZ));
      if (containsPoint(result))
        return true;
    }
  }
  return false;
}

bool bodies::Sphere::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                                   EigenSTL::vector_Vector3d* intersections, unsigned int count) const
{
  // this is faster than always calling dir.normalized() in case the vector is already unit
  const Eigen::Vector3d dirNorm = normalize(dir);

  if (detail::distanceSQR(center_, origin, dirNorm) > radius2_)
    return false;

  bool result = false;

  Eigen::Vector3d cp = origin - center_;
  double dpcpv = cp.dot(dirNorm);

  Eigen::Vector3d w = cp - dpcpv * dirNorm;
  Eigen::Vector3d Q = center_ + w;
  double x = radius2_ - w.squaredNorm();

  if (fabs(x) < detail::ZERO)
  {
    w = Q - origin;
    double dpQv = w.dot(dirNorm);
    if (dpQv > detail::ZERO)
    {
      if (intersections)
        intersections->push_back(Q);
      result = true;
    }
  }
  else if (x > 0.0)
  {
    x = sqrt(x);
    w = dirNorm * x;
    Eigen::Vector3d A = Q - w;
    Eigen::Vector3d B = Q + w;
    w = A - origin;
    double dpAv = w.dot(dirNorm);
    w = B - origin;
    double dpBv = w.dot(dirNorm);

    if (dpAv > detail::ZERO)
    {
      result = true;
      if (intersections)
      {
        intersections->push_back(A);
        if (count == 1)
          return result;
      }
    }

    if (dpBv > detail::ZERO)
    {
      result = true;
      if (intersections)
        intersections->push_back(B);
    }
  }
  return result;
}

bool bodies::Cylinder::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const
{
  Eigen::Vector3d v = p - center_;
  double pH = v.dot(normalH_);

  if (fabs(pH) > length2_)
    return false;

  double pB1 = v.dot(normalB1_);
  double remaining = radius2_ - pB1 * pB1;

  if (remaining < 0.0)
    return false;
  else
  {
    double pB2 = v.dot(normalB2_);
    return pB2 * pB2 <= remaining;
  }
}

void bodies::Cylinder::useDimensions(const shapes::Shape* shape)  // (length, radius)
{
  length_ = static_cast<const shapes::Cylinder*>(shape)->length;
  radius_ = static_cast<const shapes::Cylinder*>(shape)->radius;
}

std::vector<double> bodies::Cylinder::getDimensions() const
{
  std::vector<double> d(2);
  d[0] = radius_;
  d[1] = length_;
  return d;
}

void bodies::Cylinder::updateInternalData()
{
  const auto tmpRadiusU = radius_ * scale_ + padding_;
  if (tmpRadiusU < 0)
    throw std::runtime_error("Cylinder radius must be non-negative.");
  const auto tmpLength2 = scale_ * length_ / 2.0 + padding_;
  if (tmpLength2 < 0)
    throw std::runtime_error("Cylinder length must be non-negative.");
  radiusU_ = tmpRadiusU;
  length2_ = tmpLength2;
  radius2_ = radiusU_ * radiusU_;
  center_ = pose_.translation();
  radiusBSqr_ = length2_ * length2_ + radius2_;
  radiusB_ = sqrt(radiusBSqr_);

  ASSERT_ISOMETRY(pose_);
  Eigen::Matrix3d basis = pose_.linear();
  normalB1_ = basis.col(0);
  normalB2_ = basis.col(1);
  normalH_ = basis.col(2);

  double tmp = -normalH_.dot(center_);
  d1_ = tmp + length2_;
  d2_ = tmp - length2_;
}

bool bodies::Cylinder::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int /* max_attempts */,
                                         Eigen::Vector3d& result) const
{
  // sample a point on the base disc of the cylinder
  double a = rng.uniformReal(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
  double r = rng.uniformReal(-radiusU_, radiusU_);
  double x = cos(a) * r;
  double y = sin(a) * r;

  // sample e height
  double z = rng.uniformReal(-length2_, length2_);

  result = pose_ * Eigen::Vector3d(x, y, z);
  return true;
}

std::shared_ptr<bodies::Body> bodies::Cylinder::cloneAt(const Eigen::Isometry3d& pose, double padding,
                                                        double scale) const
{
  auto c = std::allocate_shared<Cylinder>(Eigen::aligned_allocator<Cylinder>());
  c->length_ = length_;
  c->radius_ = radius_;
  c->padding_ = padding;
  c->scale_ = scale;
  c->pose_ = pose;
  c->updateInternalData();
  return c;
}

double bodies::Cylinder::computeVolume() const
{
  return 2.0 * boost::math::constants::pi<double>() * radius2_ * length2_;
}

void bodies::Cylinder::computeBoundingSphere(BoundingSphere& sphere) const
{
  sphere.center = center_;
  sphere.radius = radiusB_;
}

void bodies::Cylinder::computeBoundingCylinder(BoundingCylinder& cylinder) const
{
  cylinder.pose = pose_;
  cylinder.radius = radiusU_;
  cylinder.length = 2 * length2_;
}

void bodies::Cylinder::computeBoundingBox(bodies::AABB& bbox) const
{
  bbox.setEmpty();

  // method taken from http://www.iquilezles.org/www/articles/diskbbox/diskbbox.htm

  const auto a = normalH_;
  const auto e = radiusU_ * (Eigen::Vector3d::Ones() - a.cwiseProduct(a) / a.dot(a)).cwiseSqrt();
  const auto pa = center_ + length2_ * normalH_;
  const auto pb = center_ - length2_ * normalH_;

  bbox.extend(pa - e);
  bbox.extend(pa + e);
  bbox.extend(pb - e);
  bbox.extend(pb + e);
}

bool bodies::Cylinder::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                                     EigenSTL::vector_Vector3d* intersections, unsigned int count) const
{
  // this is faster than always calling dir.normalized() in case the vector is already unit
  const Eigen::Vector3d dirNorm = normalize(dir);

  if (detail::distanceSQR(center_, origin, dirNorm) > radiusBSqr_)
    return false;

  std::vector<detail::intersc> ipts;

  // intersect bases
  double tmp = normalH_.dot(dirNorm);
  if (fabs(tmp) > detail::ZERO)
  {
    double tmp2 = -normalH_.dot(origin);
    double t1 = (tmp2 - d1_) / tmp;

    if (t1 > 0.0)
    {
      Eigen::Vector3d p1(origin + dirNorm * t1);
      Eigen::Vector3d v1(p1 - center_);
      v1 = v1 - normalH_.dot(v1) * normalH_;
      if (v1.squaredNorm() < radius2_ + detail::ZERO)
      {
        if (intersections == nullptr)
          return true;

        detail::intersc ip(p1, t1);
        ipts.push_back(ip);
      }
    }

    double t2 = (tmp2 - d2_) / tmp;
    if (t2 > 0.0)
    {
      Eigen::Vector3d p2(origin + dirNorm * t2);
      Eigen::Vector3d v2(p2 - center_);
      v2 = v2 - normalH_.dot(v2) * normalH_;
      if (v2.squaredNorm() < radius2_ + detail::ZERO)
      {
        if (intersections == nullptr)
          return true;

        detail::intersc ip(p2, t2);
        ipts.push_back(ip);
      }
    }
  }

  if (ipts.size() < 2)
  {
    // intersect with infinite cylinder
    Eigen::Vector3d VD(normalH_.cross(dirNorm));
    Eigen::Vector3d ROD(normalH_.cross(origin - center_));
    double a = VD.squaredNorm();
    double b = 2.0 * ROD.dot(VD);
    double c = ROD.squaredNorm() - radius2_;
    double d = b * b - 4.0 * a * c;
    if (d >= 0.0 && fabs(a) > detail::ZERO)
    {
      d = sqrt(d);
      double e = -a * 2.0;
      double t1 = (b + d) / e;
      double t2 = (b - d) / e;

      if (t1 > 0.0)
      {
        Eigen::Vector3d p1(origin + dirNorm * t1);
        Eigen::Vector3d v1(center_ - p1);

        if (fabs(normalH_.dot(v1)) < length2_ + detail::ZERO)
        {
          if (intersections == nullptr)
            return true;

          detail::intersc ip(p1, t1);
          ipts.push_back(ip);
        }
      }

      if (t2 > 0.0)
      {
        Eigen::Vector3d p2(origin + dirNorm * t2);
        Eigen::Vector3d v2(center_ - p2);

        if (fabs(normalH_.dot(v2)) < length2_ + detail::ZERO)
        {
          if (intersections == nullptr)
            return true;
          detail::intersc ip(p2, t2);
          ipts.push_back(ip);
        }
      }
    }
  }

  if (ipts.empty())
    return false;

  // If a ray hits exactly the boundary between a side and a base, it is reported twice.
  // We want to only return the intersection once, thus we need to filter them.
  detail::filterIntersections(ipts, intersections, count);
  return true;
}

bool bodies::Box::samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int /* max_attempts */,
                                    Eigen::Vector3d& result) const
{
  result = pose_ * Eigen::Vector3d(rng.uniformReal(-length2_, length2_), rng.uniformReal(-width2_, width2_),
                                   rng.uniformReal(-height2_, height2_));
  return true;
}

bool bodies::Box::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const
{
  const Eigen::Vector3d aligned = (pose_.linear().transpose() * (p - center_)).cwiseAbs();
  return aligned[0] <= length2_ && aligned[1] <= width2_ && aligned[2] <= height2_;
}

void bodies::Box::useDimensions(const shapes::Shape* shape)  // (x, y, z) = (length, width, height)
{
  const double* size = static_cast<const shapes::Box*>(shape)->size;
  length_ = size[0];
  width_ = size[1];
  height_ = size[2];
}

std::vector<double> bodies::Box::getDimensions() const
{
  std::vector<double> d(3);
  d[0] = length_;
  d[1] = width_;
  d[2] = height_;
  return d;
}

void bodies::Box::updateInternalData()
{
  double s2 = scale_ / 2.0;
  const auto tmpLength2 = length_ * s2 + padding_;
  const auto tmpWidth2 = width_ * s2 + padding_;
  const auto tmpHeight2 = height_ * s2 + padding_;

  if (tmpLength2 < 0 || tmpWidth2 < 0 || tmpHeight2 < 0)
    throw std::runtime_error("Box dimensions must be non-negative.");

  length2_ = tmpLength2;
  width2_ = tmpWidth2;
  height2_ = tmpHeight2;

  center_ = pose_.translation();

  radius2_ = length2_ * length2_ + width2_ * width2_ + height2_ * height2_;
  radiusB_ = sqrt(radius2_);

  ASSERT_ISOMETRY(pose_);
  Eigen::Matrix3d basis = pose_.linear();
  normalL_ = basis.col(0);
  normalW_ = basis.col(1);
  normalH_ = basis.col(2);

  // rotation is intentionally not applied, the corners are used in intersectsRay()
  const Eigen::Vector3d tmp(length2_, width2_, height2_);
  corner1_ = center_ - tmp;
  corner2_ = center_ + tmp;
}

std::shared_ptr<bodies::Body> bodies::Box::cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const
{
  auto b = std::allocate_shared<Box>(Eigen::aligned_allocator<Box>());
  b->length_ = length_;
  b->width_ = width_;
  b->height_ = height_;
  b->padding_ = padding;
  b->scale_ = scale;
  b->pose_ = pose;
  b->updateInternalData();
  return b;
}

double bodies::Box::computeVolume() const
{
  return 8.0 * length2_ * width2_ * height2_;
}

void bodies::Box::computeBoundingSphere(BoundingSphere& sphere) const
{
  sphere.center = center_;
  sphere.radius = radiusB_;
}

void bodies::Box::computeBoundingCylinder(BoundingCylinder& cylinder) const
{
  double a, b;

  if (length2_ > width2_ && length2_ > height2_)
  {
    cylinder.length = length2_ * 2.0;
    a = width2_;
    b = height2_;
    Eigen::Isometry3d rot(Eigen::AngleAxisd(90.0f * (M_PI / 180.0f), Eigen::Vector3d::UnitY()));
    cylinder.pose = pose_ * rot;
  }
  else if (width2_ > height2_)
  {
    cylinder.length = width2_ * 2.0;
    a = height2_;
    b = length2_;
    cylinder.radius = sqrt(height2_ * height2_ + length2_ * length2_);
    Eigen::Isometry3d rot(Eigen::AngleAxisd(90.0f * (M_PI / 180.0f), Eigen::Vector3d::UnitX()));
    cylinder.pose = pose_ * rot;
  }
  else
  {
    cylinder.length = height2_ * 2.0;
    a = width2_;
    b = length2_;
    cylinder.pose = pose_;
  }
  cylinder.radius = sqrt(a * a + b * b);
}

void bodies::Box::computeBoundingBox(bodies::AABB& bbox) const
{
  bbox.setEmpty();

  bbox.extendWithTransformedBox(getPose(), 2 * Eigen::Vector3d(length2_, width2_, height2_));
}

bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                                EigenSTL::vector_Vector3d* intersections, unsigned int count) const
{
  // this is faster than always calling dir.normalized() in case the vector is already unit
  const Eigen::Vector3d dirNorm = normalize(dir);

  // Brian Smits. Efficient bounding box intersection. Ray tracing news 15(1), 2002

  // The implemented method only works for axis-aligned boxes. So we treat ours as such, cancel its rotation, and
  // rotate the origin and dir instead. corner1_ and corner2_ are corners with canceled rotation.
  const Eigen::Matrix3d invRot = pose_.linear().transpose();
  const Eigen::Vector3d o(invRot * (origin - center_) + center_);
  const Eigen::Vector3d d(invRot * dirNorm);

  Eigen::Vector3d tmpTmin, tmpTmax;
  tmpTmin = (corner1_ - o).cwiseQuotient(d);
  tmpTmax = (corner2_ - o).cwiseQuotient(d);

  // In projection to each axis, if the ray has positive direction, it goes from min corner (corner1_) to max corner
  // (corner2_). If its direction is negative, the first intersection is at max corner and then at min corner.
  for (size_t i = 0; i < 3; ++i)
  {
    if (d[i] < 0)
      std::swap(tmpTmin[i], tmpTmax[i]);
  }

  // tmin and tmax are such values of t in "p = o + t * d" in which the line intersects the box faces.
  // The box is viewed projected from all three directions, values of t are computed for each of the projections,
  // and a final constraint on tmin and tmax is updated by each of these projections. If tmin > tmax, there is no
  // intersection between the line and the box.

  double tmin, tmax;
  // use fmax/fmin to handle NaNs which can sneak in when dividing by d in tmpTmin and tmpTmax
  tmin = std::fmax(tmpTmin.x(), std::fmax(tmpTmin.y(), tmpTmin.z()));
  tmax = std::fmin(tmpTmax.x(), std::fmin(tmpTmax.y(), tmpTmax.z()));

  // tmin > tmax, there is no intersection between the line and the box
  if (tmax - tmin < -detail::ZERO)
    return false;

  // As we're doing intersections with a ray and not a line, cases where tmax is negative mean that the intersection is
  // with the opposite ray and not the one we are working with.
  if (tmax < 0)
    return false;

  if (intersections)
  {
    if (tmax - tmin > detail::ZERO)
    {
      // tmax > tmin, we have two distinct intersection points
      if (tmin > detail::ZERO)
      {
        // tmin > 0, both intersections lie on the ray
        intersections->push_back(tmin * dirNorm + origin);
        if (count == 0 || count > 1)
          intersections->push_back(tmax * dirNorm + origin);
      }
      else
      {
        // tmin <= 0 && tmax >= 0, the first intersection point is on the opposite ray and the second one on the correct
        // ray - this means origin of the ray lies inside the box and we should only report one intersection.
        intersections->push_back(tmax * dirNorm + origin);
      }
    }
    else
    {
      // tmax == tmin, there is exactly one intersection at a corner or edge
      intersections->push_back(tmax * dirNorm + origin);
    }
  }

  return true;
}

bool bodies::ConvexMesh::containsPoint(const Eigen::Vector3d& p, bool /* verbose */) const
{
  if (!mesh_data_)
    return false;
  if (bounding_box_.containsPoint(p))
  {
    // Transform the point to the "base space" of this mesh
    Eigen::Vector3d ip(i_pose_ * p);
    return isPointInsidePlanes(ip);
  }
  else
    return false;
}

void bodies::ConvexMesh::correctVertexOrderFromPlanes()
{
  for (unsigned int i = 0; i < mesh_data_->triangles_.size(); i += 3)
  {
    Eigen::Vector3d d1 =
        mesh_data_->vertices_[mesh_data_->triangles_[i]] - mesh_data_->vertices_[mesh_data_->triangles_[i + 1]];
    Eigen::Vector3d d2 =
        mesh_data_->vertices_[mesh_data_->triangles_[i]] - mesh_data_->vertices_[mesh_data_->triangles_[i + 2]];
    // expected computed normal from triangle vertex order
    Eigen::Vector3d tri_normal = d1.cross(d2);
    tri_normal.normalize();
    // actual plane normal
    Eigen::Vector3d normal(mesh_data_->planes_[mesh_data_->plane_for_triangle_[i / 3]].x(),
                           mesh_data_->planes_[mesh_data_->plane_for_triangle_[i / 3]].y(),
                           mesh_data_->planes_[mesh_data_->plane_for_triangle_[i / 3]].z());
    bool same_dir = tri_normal.dot(normal) > 0;
    if (!same_dir)
    {
      std::swap(mesh_data_->triangles_[i], mesh_data_->triangles_[i + 1]);
    }
  }
}

void bodies::ConvexMesh::useDimensions(const shapes::Shape* shape)
{
  mesh_data_ = std::allocate_shared<MeshData>(Eigen::aligned_allocator<MeshData>());
  const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(shape);

  double maxX = -std::numeric_limits<double>::infinity(), maxY = -std::numeric_limits<double>::infinity(),
         maxZ = -std::numeric_limits<double>::infinity();
  double minX = std::numeric_limits<double>::infinity(), minY = std::numeric_limits<double>::infinity(),
         minZ = std::numeric_limits<double>::infinity();

  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
  {
    double vx = mesh->vertices[3 * i];
    double vy = mesh->vertices[3 * i + 1];
    double vz = mesh->vertices[3 * i + 2];

    if (maxX < vx)
      maxX = vx;
    if (maxY < vy)
      maxY = vy;
    if (maxZ < vz)
      maxZ = vz;

    if (minX > vx)
      minX = vx;
    if (minY > vy)
      minY = vy;
    if (minZ > vz)
      minZ = vz;
  }

  if (maxX < minX)
    maxX = minX = 0.0;
  if (maxY < minY)
    maxY = minY = 0.0;
  if (maxZ < minZ)
    maxZ = minZ = 0.0;

  mesh_data_->box_size_ = Eigen::Vector3d(maxX - minX, maxY - minY, maxZ - minZ);

  mesh_data_->box_offset_ = Eigen::Vector3d((minX + maxX) / 2.0, (minY + maxY) / 2.0, (minZ + maxZ) / 2.0);

  mesh_data_->planes_.clear();
  mesh_data_->triangles_.clear();
  mesh_data_->vertices_.clear();
  mesh_data_->mesh_radiusB_ = 0.0;
  mesh_data_->mesh_center_ = Eigen::Vector3d();

  double xdim = maxX - minX;
  double ydim = maxY - minY;
  double zdim = maxZ - minZ;

  double pose1;
  double pose2;

  unsigned int off1;
  unsigned int off2;

  /* compute bounding cylinder */
  double cyl_length;
  double maxdist = -std::numeric_limits<double>::infinity();
  if (xdim > ydim && xdim > zdim)
  {
    off1 = 1;
    off2 = 2;
    pose1 = mesh_data_->box_offset_.y();
    pose2 = mesh_data_->box_offset_.z();
    cyl_length = xdim;
  }
  else if (ydim > zdim)
  {
    off1 = 0;
    off2 = 2;
    pose1 = mesh_data_->box_offset_.x();
    pose2 = mesh_data_->box_offset_.z();
    cyl_length = ydim;
  }
  else
  {
    off1 = 0;
    off2 = 1;
    pose1 = mesh_data_->box_offset_.x();
    pose2 = mesh_data_->box_offset_.y();
    cyl_length = zdim;
  }

  /* compute convex hull */
  coordT* points = (coordT*)calloc(mesh->vertex_count * 3, sizeof(coordT));
  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
  {
    points[3 * i + 0] = (coordT)mesh->vertices[3 * i + 0];
    points[3 * i + 1] = (coordT)mesh->vertices[3 * i + 1];
    points[3 * i + 2] = (coordT)mesh->vertices[3 * i + 2];

    double dista = mesh->vertices[3 * i + off1] - pose1;
    double distb = mesh->vertices[3 * i + off2] - pose2;
    double dist = sqrt(((dista * dista) + (distb * distb)));
    if (dist > maxdist)
      maxdist = dist;
  }
  mesh_data_->bounding_cylinder_.radius = maxdist;
  mesh_data_->bounding_cylinder_.length = cyl_length;

  static FILE* null = fopen("/dev/null", "w");

  char flags[] = "qhull Tv Qt";
  int exitcode = qh_new_qhull(3, mesh->vertex_count, points, true, flags, null, null);

  if (exitcode != 0)
  {
    CONSOLE_BRIDGE_logWarn("Convex hull creation failed");
    qh_freeqhull(!qh_ALL);
    int curlong, totlong;
    qh_memfreeshort(&curlong, &totlong);
    return;
  }

  int num_facets = qh num_facets;

  int num_vertices = qh num_vertices;
  mesh_data_->vertices_.reserve(num_vertices);
  Eigen::Vector3d sum(0, 0, 0);

  // necessary for FORALLvertices
  std::map<unsigned int, unsigned int> qhull_vertex_table;
  vertexT* vertex;
  FORALLvertices
  {
    Eigen::Vector3d vert(vertex->point[0], vertex->point[1], vertex->point[2]);
    qhull_vertex_table[vertex->id] = mesh_data_->vertices_.size();
    sum += vert;
    mesh_data_->vertices_.push_back(vert);
  }

  mesh_data_->mesh_center_ = sum / (double)(num_vertices);
  for (unsigned int j = 0; j < mesh_data_->vertices_.size(); ++j)
  {
    double dist = (mesh_data_->vertices_[j] - mesh_data_->mesh_center_).squaredNorm();
    if (dist > mesh_data_->mesh_radiusB_)
      mesh_data_->mesh_radiusB_ = dist;
  }

  mesh_data_->mesh_radiusB_ = sqrt(mesh_data_->mesh_radiusB_);
  mesh_data_->triangles_.reserve(num_facets);

  // HACK: only needed for ABI compatibility with melodic
  std::map<size_t, size_t>& triangle_for_plane = detail::getTriangleForPlane(this);
  triangle_for_plane.clear();

  // neccessary for qhull macro
  facetT* facet;
  FORALLfacets
  {
    Eigen::Vector4d planeEquation(facet->normal[0], facet->normal[1], facet->normal[2], facet->offset);
    if (!mesh_data_->planes_.empty())
    {
      // filter equal planes - assuming same ones follow each other
      if ((planeEquation - mesh_data_->planes_.back()).cwiseAbs().maxCoeff() > 1e-6)  // max diff to last
        mesh_data_->planes_.push_back(planeEquation);
    }
    else
    {
      mesh_data_->planes_.push_back(planeEquation);
    }

    // Needed by FOREACHvertex_i_
    int vertex_n, vertex_i;
    FOREACHvertex_i_((*facet).vertices)
    {
      mesh_data_->triangles_.push_back(qhull_vertex_table[vertex->id]);
    }

    mesh_data_->plane_for_triangle_[(mesh_data_->triangles_.size() - 1) / 3] = mesh_data_->planes_.size() - 1;
    triangle_for_plane[mesh_data_->planes_.size() - 1] = (mesh_data_->triangles_.size() - 1) / 3;
  }
  qh_freeqhull(!qh_ALL);
  int curlong, totlong;
  qh_memfreeshort(&curlong, &totlong);
}

std::vector<double> bodies::ConvexMesh::getDimensions() const
{
  return std::vector<double>();
}

void bodies::ConvexMesh::computeScaledVerticesFromPlaneProjections()
{
  // compute the scaled vertices, if needed
  if (padding_ == 0.0 && scale_ == 1.0)
  {
    scaled_vertices_ = &mesh_data_->vertices_;
    return;
  }

  if (!scaled_vertices_storage_)
    scaled_vertices_storage_.reset(new EigenSTL::vector_Vector3d());
  scaled_vertices_ = scaled_vertices_storage_.get();
  scaled_vertices_storage_->resize(mesh_data_->vertices_.size());
  // project vertices along the vertex - center line to the scaled and padded plane
  // take the average of all tri's planes around that vertex as the result
  // is not unique

  // First figure out, which tris are connected to each vertex
  std::map<unsigned int, std::vector<unsigned int>> vertex_to_tris;
  for (unsigned int i = 0; i < mesh_data_->triangles_.size() / 3; ++i)
  {
    vertex_to_tris[mesh_data_->triangles_[3 * i + 0]].push_back(i);
    vertex_to_tris[mesh_data_->triangles_[3 * i + 1]].push_back(i);
    vertex_to_tris[mesh_data_->triangles_[3 * i + 2]].push_back(i);
  }

  for (unsigned int i = 0; i < mesh_data_->vertices_.size(); ++i)
  {
    Eigen::Vector3d v(mesh_data_->vertices_[i] - mesh_data_->mesh_center_);
    EigenSTL::vector_Vector3d projected_vertices;
    for (unsigned int t : vertex_to_tris[i])
    {
      const Eigen::Vector4d& plane = mesh_data_->planes_[mesh_data_->plane_for_triangle_[t]];
      Eigen::Vector3d plane_normal(plane.x(), plane.y(), plane.z());
      double d_scaled_padded =
          scale_ * plane.w() - (1 - scale_) * mesh_data_->mesh_center_.dot(plane_normal) - padding_;

      // intersect vert - center with scaled/padded plane equation
      double denom = v.dot(plane_normal);
      if (fabs(denom) < 1e-3)
        continue;
      double lambda = (-mesh_data_->mesh_center_.dot(plane_normal) - d_scaled_padded) / denom;
      Eigen::Vector3d vert_on_plane = v * lambda + mesh_data_->mesh_center_;
      projected_vertices.push_back(vert_on_plane);
    }
    if (projected_vertices.empty())
    {
      double l = v.norm();
      scaled_vertices_storage_->at(i) =
          mesh_data_->mesh_center_ + v * (scale_ + (l > detail::ZERO ? padding_ / l : 0.0));
    }
    else
    {
      Eigen::Vector3d sum(0, 0, 0);
      for (const Eigen::Vector3d& vertex : projected_vertices)
      {
        sum += vertex;
      }
      sum /= projected_vertices.size();
      scaled_vertices_storage_->at(i) = sum;
    }
  }
}

void bodies::ConvexMesh::updateInternalData()
{
  if (!mesh_data_)
    return;
  Eigen::Isometry3d pose = pose_;
  pose.translation() = Eigen::Vector3d(pose_ * mesh_data_->box_offset_);

  shapes::Box box_shape(mesh_data_->box_size_.x(), mesh_data_->box_size_.y(), mesh_data_->box_size_.z());
  bounding_box_.setPoseDirty(pose);
  // The real effect of padding will most likely be smaller due to the mesh padding algorithm, but in "worst case" it
  // can inflate the primitive bounding box by the padding_ value.
  bounding_box_.setPaddingDirty(padding_);
  bounding_box_.setScaleDirty(scale_);
  bounding_box_.setDimensionsDirty(&box_shape);
  bounding_box_.updateInternalData();

  i_pose_ = pose_.inverse();
  center_ = pose_ * mesh_data_->mesh_center_;
  radiusB_ = mesh_data_->mesh_radiusB_ * scale_ + padding_;
  radiusBSqr_ = radiusB_ * radiusB_;

  // compute the scaled vertices, if needed
  if (padding_ == 0.0 && scale_ == 1.0)
    scaled_vertices_ = &mesh_data_->vertices_;
  else
  {
    if (!scaled_vertices_storage_)
      scaled_vertices_storage_.reset(new EigenSTL::vector_Vector3d());
    scaled_vertices_ = scaled_vertices_storage_.get();
    scaled_vertices_storage_->resize(mesh_data_->vertices_.size());
    for (unsigned int i = 0; i < mesh_data_->vertices_.size(); ++i)
    {
      Eigen::Vector3d v(mesh_data_->vertices_[i] - mesh_data_->mesh_center_);
      double l = v.norm();
      scaled_vertices_storage_->at(i) =
          mesh_data_->mesh_center_ + v * (scale_ + (l > detail::ZERO ? padding_ / l : 0.0));
    }
  }
}
const std::vector<unsigned int>& bodies::ConvexMesh::getTriangles() const
{
  static const std::vector<unsigned int> empty;
  return mesh_data_ ? mesh_data_->triangles_ : empty;
}

const EigenSTL::vector_Vector3d& bodies::ConvexMesh::getVertices() const
{
  static const EigenSTL::vector_Vector3d empty;
  return mesh_data_ ? mesh_data_->vertices_ : empty;
}

const EigenSTL::vector_Vector3d& bodies::ConvexMesh::getScaledVertices() const
{
  return scaled_vertices_ ? *scaled_vertices_ : getVertices();
}

const EigenSTL::vector_Vector4d& bodies::ConvexMesh::getPlanes() const
{
  static const EigenSTL::vector_Vector4d empty;
  return mesh_data_ ? mesh_data_->planes_ : empty;
}

std::shared_ptr<bodies::Body> bodies::ConvexMesh::cloneAt(const Eigen::Isometry3d& pose, double padding,
                                                          double scale) const
{
  auto m = std::allocate_shared<ConvexMesh>(Eigen::aligned_allocator<ConvexMesh>());
  m->mesh_data_ = mesh_data_;
  m->padding_ = padding;
  m->scale_ = scale;
  m->pose_ = pose;
  m->updateInternalData();
  return m;
}

void bodies::ConvexMesh::computeBoundingSphere(BoundingSphere& sphere) const
{
  sphere.center = center_;
  sphere.radius = radiusB_;
}

void bodies::ConvexMesh::computeBoundingCylinder(BoundingCylinder& cylinder) const
{
  // the padding contibution might be smaller in reality, but we want to get it right for the worst case
  cylinder.length = mesh_data_ ? mesh_data_->bounding_cylinder_.length * scale_ + 2 * padding_ : 0.0;
  cylinder.radius = mesh_data_ ? mesh_data_->bounding_cylinder_.radius * scale_ + padding_ : 0.0;
  // need to do rotation correctly to get pose, which bounding box does
  BoundingCylinder cyl;
  bounding_box_.computeBoundingCylinder(cyl);
  cylinder.pose = cyl.pose;
}

void bodies::ConvexMesh::computeBoundingBox(bodies::AABB& bbox) const
{
  bbox.setEmpty();

  bounding_box_.computeBoundingBox(bbox);
}

bool bodies::ConvexMesh::isPointInsidePlanes(const Eigen::Vector3d& point) const
{
  unsigned int numplanes = mesh_data_->planes_.size();
  const std::map<size_t, size_t>& triangle_for_plane = detail::getTriangleForPlane(this);
  for (unsigned int i = 0; i < numplanes; ++i)
  {
    const Eigen::Vector4d& plane = mesh_data_->planes_[i];
    Eigen::Vector3d plane_vec(plane.x(), plane.y(), plane.z());
    // w() needs to be recomputed from a scaled vertex as normally it refers to the unscaled plane
    // we also cannot simply subtract padding_ from it, because padding of the points on the plane causes a different
    // effect than adding padding along this plane's normal (padding effect is direction-dependent)
    const auto scaled_point_on_plane = scaled_vertices_->at(mesh_data_->triangles_[3 * triangle_for_plane.at(i)]);
    const double w_scaled_padded = -plane_vec.dot(scaled_point_on_plane);
    const double dist = plane_vec.dot(point) + w_scaled_padded - detail::ZERO;
    if (dist > 0.0)
      return false;
  }
  return true;
}

unsigned int bodies::ConvexMesh::countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const
{
  unsigned int numvertices = mesh_data_->vertices_.size();
  unsigned int result = 0;
  for (unsigned int i = 0; i < numvertices; ++i)
  {
    Eigen::Vector3d plane_vec(planeNormal.x(), planeNormal.y(), planeNormal.z());
    double dist = plane_vec.dot(mesh_data_->vertices_[i]) + planeNormal.w() - 1e-6;
    if (dist > 0.0)
      result++;
  }
  return result;
}

double bodies::ConvexMesh::computeVolume() const
{
  double volume = 0.0;
  if (mesh_data_)
    for (unsigned int i = 0; i < mesh_data_->triangles_.size() / 3; ++i)
    {
      const Eigen::Vector3d& v1 = mesh_data_->vertices_[mesh_data_->triangles_[3 * i + 0]];
      const Eigen::Vector3d& v2 = mesh_data_->vertices_[mesh_data_->triangles_[3 * i + 1]];
      const Eigen::Vector3d& v3 = mesh_data_->vertices_[mesh_data_->triangles_[3 * i + 2]];
      volume += v1.x() * v2.y() * v3.z() + v2.x() * v3.y() * v1.z() + v3.x() * v1.y() * v2.z() -
                v1.x() * v3.y() * v2.z() - v2.x() * v1.y() * v3.z() - v3.x() * v2.y() * v1.z();
    }
  return fabs(volume) / 6.0;
}

bool bodies::ConvexMesh::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
                                       EigenSTL::vector_Vector3d* intersections, unsigned int count) const
{
  // this is faster than always calling dir.normalized() in case the vector is already unit
  const Eigen::Vector3d dirNorm = normalize(dir);

  if (!mesh_data_)
    return false;
  if (detail::distanceSQR(center_, origin, dirNorm) > radiusBSqr_)
    return false;
  if (!bounding_box_.intersectsRay(origin, dirNorm))
    return false;

  // transform the ray into the coordinate frame of the mesh
  Eigen::Vector3d orig(i_pose_ * origin);
  Eigen::Vector3d dr(i_pose_.linear() * dirNorm);

  std::vector<detail::intersc> ipts;

  bool result = false;

  // for each triangle
  const auto nt = mesh_data_->triangles_.size() / 3;
  for (size_t i = 0; i < nt; ++i)
  {
    Eigen::Vector3d vec(mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].x(),
                        mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].y(),
                        mesh_data_->planes_[mesh_data_->plane_for_triangle_[i]].z());

    const double tmp = vec.dot(dr);
    if (fabs(tmp) > detail::ZERO)
    {
      // planes_[...].w() corresponds to the unscaled mesh, so we need to compute it ourselves
      const double w_scaled_padded = vec.dot(scaled_vertices_->at(mesh_data_->triangles_[3 * i]));
      const double t = -(vec.dot(orig) + w_scaled_padded) / tmp;
      if (t > 0.0)
      {
        const auto i3 = 3 * i;
        const auto v1 = mesh_data_->triangles_[i3 + 0];
        const auto v2 = mesh_data_->triangles_[i3 + 1];
        const auto v3 = mesh_data_->triangles_[i3 + 2];

        const Eigen::Vector3d& a = scaled_vertices_->at(v1);
        const Eigen::Vector3d& b = scaled_vertices_->at(v2);
        const Eigen::Vector3d& c = scaled_vertices_->at(v3);

        Eigen::Vector3d cb(c - b);
        Eigen::Vector3d ab(a - b);

        // intersection of the plane defined by the triangle and the ray
        Eigen::Vector3d P(orig + dr * t);

        // check if it is inside the triangle
        Eigen::Vector3d pb(P - b);
        Eigen::Vector3d c1(cb.cross(pb));
        Eigen::Vector3d c2(cb.cross(ab));
        if (c1.dot(c2) < 0.0)
          continue;

        Eigen::Vector3d ca(c - a);
        Eigen::Vector3d pa(P - a);
        Eigen::Vector3d ba(-ab);

        c1 = ca.cross(pa);
        c2 = ca.cross(ba);
        if (c1.dot(c2) < 0.0)
          continue;

        c1 = ba.cross(pa);
        c2 = ba.cross(ca);

        if (c1.dot(c2) < 0.0)
          continue;

        result = true;
        if (intersections)
        {
          detail::intersc ip(origin + dirNorm * t, t);
          ipts.push_back(ip);
        }
        else
          break;
      }
    }
  }

  if (result && intersections)
  {
    // If a ray hits exactly the boundary between two triangles, it is reported twice;
    // We only want return the intersection once; thus we need to filter them.
    detail::filterIntersections(ipts, intersections, count);
  }

  return result;
}

bodies::ConvexMesh::~ConvexMesh()
{
  // HACK: only needed for ABI compatibility with melodic
  {
    std::lock_guard<std::mutex> lock(detail::g_triangle_for_plane_mutex);
    detail::g_triangle_for_plane_.erase(this);
  }
}

bodies::BodyVector::BodyVector()
{
}

bodies::BodyVector::BodyVector(const std::vector<shapes::Shape*>& shapes, const EigenSTL::vector_Isometry3d& poses,
                               double padding)
{
  for (unsigned int i = 0; i < shapes.size(); i++)
    addBody(shapes[i], poses[i], padding);
}

bodies::BodyVector::~BodyVector()
{
  clear();
}

void bodies::BodyVector::clear()
{
  for (auto& body : bodies_)
    delete body;
  bodies_.clear();
}

void bodies::BodyVector::addBody(Body* body)
{
  bodies_.push_back(body);
  BoundingSphere sphere;
  body->computeBoundingSphere(sphere);
}

void bodies::BodyVector::addBody(const shapes::Shape* shape, const Eigen::Isometry3d& pose, double padding)
{
  bodies::Body* body = bodies::createBodyFromShape(shape);
  body->setPoseDirty(pose);
  body->setPaddingDirty(padding);
  body->updateInternalData();
  addBody(body);
}

std::size_t bodies::BodyVector::getCount() const
{
  return bodies_.size();
}

void bodies::BodyVector::setPose(unsigned int i, const Eigen::Isometry3d& pose)
{
  if (i >= bodies_.size())
  {
    CONSOLE_BRIDGE_logError("There is no body at index %u", i);
    return;
  }

  bodies_[i]->setPose(pose);
}

const bodies::Body* bodies::BodyVector::getBody(unsigned int i) const
{
  if (i >= bodies_.size())
  {
    CONSOLE_BRIDGE_logError("There is no body at index %u", i);
    return nullptr;
  }
  else
    return bodies_[i];
}

bool bodies::BodyVector::containsPoint(const Eigen::Vector3d& p, std::size_t& index, bool verbose) const
{
  for (std::size_t i = 0; i < bodies_.size(); ++i)
    if (bodies_[i]->containsPoint(p, verbose))
    {
      index = i;
      return true;
    }
  return false;
}

bool bodies::BodyVector::containsPoint(const Eigen::Vector3d& p, bool verbose) const
{
  std::size_t dummy;
  return containsPoint(p, dummy, verbose);
}

bool bodies::BodyVector::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, std::size_t& index,
                                       EigenSTL::vector_Vector3d* intersections, unsigned int count) const
{
  for (std::size_t i = 0; i < bodies_.size(); ++i)
    if (bodies_[i]->intersectsRay(origin, dir, intersections, count))
    {
      index = i;
      return true;
    }
  return false;
}


#include <boost/geometry/strategies/agnostic/hull_graham_andrew.hpp>


#include <unordered_map>

#include <boost/algorithm/string/replace.hpp>

#include "binocular-camera.hpp"
#include "camera.hpp"
#include "triangulation.hpp"

#include "perceive/calibration/find-F.hpp"
#include "perceive/geometry/fitting-planes.hpp"
#include "perceive/geometry/projective/camera.hpp"
#include "perceive/graphics/bresenham.hpp"
#include "perceive/graphics/tiny-string.hpp"
#include "perceive/io/perceive-assets.hpp"
#include "perceive/optimization/golden-section-search.hpp"
#include "perceive/optimization/levenberg-marquardt.hpp"
#include "perceive/optimization/nelder-mead.hpp"
#include "perceive/utils/create-cv-remap.hpp"
#include "perceive/utils/eigen-helpers.hpp"
#include "perceive/utils/file-system.hpp"
#include "perceive/utils/md5.hpp"
#include "perceive/utils/tick-tock.hpp"

#define This BinocularCamera

namespace perceive
{
// ------------------------------------------------------ Inscribed Rect 2 Hulls

static AABB inscribed_rect_two_hulls(const std::vector<Vector2>& chull0,
                                     const std::vector<Vector2>& chull1,
                                     const real delta,
                                     const bool do_trace) noexcept
{
   auto find_bounding_roi = [&](const auto& hull) -> AABB {
      // Find the bounding rectangle
      auto aabb = AABB::minmax();
      for(const auto& X : hull) aabb.union_point(X);
      return aabb;
   };

   array<vector<Vector2>, 2> hull{{chull0, chull1}};
   array<AABB, 2> bounds;
   std::transform(cbegin(hull), cend(hull), begin(bounds), find_bounding_roi);
   array<vector<Vector2>, 2> isects;

   auto hull_hull_line_bounds = [&](const real y) {
      auto l = to_homgen_line(Vector2{0.0, y}, Vector2{1.0, y});
      for(size_t i = 0; i < 2; ++i) {
         hull_line_intersections(cbegin(hull[i]), cend(hull[i]), l, isects[i]);
         if(isects[i].size() < 2) return Vector2::nan();
      }
      auto cmp = [&](const auto& a, const auto& b) { return a.x < b.x; };
      auto [l0, r0]
          = std::minmax_element(cbegin(isects[0]), cend(isects[0]), cmp);
      auto [l1, r1]
          = std::minmax_element(cbegin(isects[1]), cend(isects[1]), cmp);
      return Vector2(std::max(l0->x, l1->x), std::min(r0->x, r1->x));
   };

   auto calc_aabb = [&](const real y0, const real y1) -> AABB {
      auto xx0 = hull_hull_line_bounds(y0);
      auto xx1 = hull_hull_line_bounds(y1);
      AABB aabb;
      aabb.left                       = std::max(xx0(0), xx1(0));
      aabb.right                      = std::min(xx0(1), xx1(1));
      std::tie(aabb.top, aabb.bottom) = std::minmax(y0, y1);

      if(do_trace) {
         cout << format(" (-) y0={}, y1={}, xx0={}, xx1={}, aabb={}",
                        y0,
                        y1,
                        str(xx0),
                        str(xx1),
                        str(aabb))
              << endl;
      }

      if(!aabb.is_finite()) aabb = AABB(0.0, 0.0, 0.0, 0.0);
      return aabb;
   };

   auto extend_aabb = [&](bool direction, real y0, real y1, real delta) {
      auto aabb0 = calc_aabb(y0, y1);
      while(true) {
         y0         = direction ? (y0 - delta) : y0;
         y1         = direction ? y1 : (y1 + delta);
         auto aabb1 = calc_aabb(y0, y1);

         if(do_trace) {
            cout << format(" <*> y0 = {}, y1 = {}, "
                           "area = [{}, {}], 0={}, 1={}",
                           y0,
                           y1,
                           aabb0.area(),
                           aabb1.area(),
                           str(aabb0),
                           str(aabb1))
                 << endl;

            fgetc(stdin);
         }

         if(aabb0.area() > aabb1.area()) break;
         aabb0 = aabb1;
      }
      return aabb0;
   };

   const auto C = 0.5 * (bounds[0].center(), bounds[1].center());

   auto aabb = extend_aabb(true, C.y, C.y, delta);
   auto ret  = extend_aabb(false, aabb.top, aabb.bottom, delta);

   if(ret.area() <= std::fabs(delta)) {
      LOG_ERR("failed to get an inscribed rectangle");
      cout << format("bounds0 = {}", str(bounds[0])) << endl;
      cout << format("bounds1 = {}", str(bounds[1])) << endl;
      cout << format("C0 = {}", str(bounds[0].center())) << endl;
      cout << format("C1 = {}", str(bounds[1].center())) << endl;
      cout << format("C     = {}", str(C)) << endl;
      cout << format("delta = {}", delta) << endl;
      cout << format("aabb  = {}", str(aabb)) << endl;
      cout << format("ret   = {}", str(ret)) << endl;
      // FATAL(format("aborting due to previous errors"));
   }

   return ret;
}

// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
// --                               Camera Info                               --
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------

bool BinocularCameraInfo::operator==(
    const BinocularCameraInfo& o) const noexcept
{
#define NORM_TEST(v) (fabs(v - o.v) < 1e-9)
#define TEST(v) (v == o.v)
   return true && TEST(camera_id) && ((t - o.t).norm() < 1e-9)
          && ((q.normalised() - o.q.normalised()).norm() < 1e-9)
          && NORM_TEST(baseline) && TEST(M[0]) && TEST(M[1]);
#undef TEST
#undef NORM_TEST
}

string BinocularCameraInfo::to_json_string() const noexcept
{
   auto aa = q.to_axis_angle();
   auto tt = t.normalised();
   return format(R"V0G0N(
{{
   "camera-id": "{}",
   "rotation-axis": [{}, {}, {}],
   "rotation-angle": {},
   "t": [{}, {}, {}],
   "baseline": {},
   "sensor0": {},
   "sensor1": {}
}}
)V0G0N",
                 camera_id,
                 str_precise(aa(0)),
                 str_precise(aa(1)),
                 str_precise(aa(2)),
                 str_precise(aa(3)),
                 str_precise(tt(0)),
                 str_precise(tt(1)),
                 str_precise(tt(2)),
                 str_precise(baseline),
                 indent(M[0].to_json_string(), 14),
                 indent(M[1].to_json_string(), 14));
}

void load(BinocularCameraInfo& data, const string& fname) noexcept(false)
{
   read(data, file_get_contents(fname));
}

void save(const BinocularCameraInfo& data, const string& fname) noexcept(false)
{
   std::string s;
   write(data, s);
   file_put_contents(fname, s);
}

void read(BinocularCameraInfo& data, const std::string& in) noexcept(false)
{
   const auto root = parse_json(in);
   read(data, root);
}

void write(const BinocularCameraInfo& data, std::string& out) noexcept(false)
{
   out = data.to_json_string();
}

void read(BinocularCameraInfo& data, const Json::Value& node) noexcept(false)
{
   std::string err = "";

   auto from_json = [&](const Json::Value& root) {
      Vector3 axis;
      real theta{0.0};
      json_load(get_key(root, "camera-id"), data.camera_id);
      json_load(get_key(root, "rotation-axis"), axis);
      json_load(get_key(root, "rotation-angle"), theta);
      json_load(get_key(root, "t"), data.t);
      json_load(get_key(root, "baseline"), data.baseline);
      const auto s0 = time_thunk(
          [&]() { data.M[0].from_json(get_key(root, "sensor0")); });
      const auto s1 = time_thunk(
          [&]() { data.M[1].from_json(get_key(root, "sensor1")); });
      // TRACE(format("s0 = {}s, s1 = {}s", s0, s1));
      data.q.from_axis_angle(axis, theta);
   };

   try {
      const auto s0 = time_thunk([&]() { from_json(node); });
      // TRACE(format("from-json took {}s", s0));
   } catch(std::logic_error& e) {
      err = strlen(e.what()) == 0 ? "logic error" : e.what();
   } catch(std::runtime_error& e) {
      err = strlen(e.what()) == 0 ? "runtime error" : e.what();
   } catch(std::exception& e) {
      // JSON parse error
      err = strlen(e.what()) == 0 ? "exception" : e.what();
   } catch(...) {
      err = format("unknown error");
   }

   if(err != "") throw std::runtime_error(err);

   {
      const auto [bcam_id, suffix] = extract_suffix(data.camera_id, '_');
      if(!string_is_uppercase(bcam_id)) {
         const string new_bcam_id = string_to_uppercase(bcam_id);
         const string full_id     = new_bcam_id + suffix;
         WARN(format("Camera id is not uppercase. BOO!!!. Changing '{}' -> "
                     "'{}'. Suffix is '{}', and full camera-id is '{}'",
                     bcam_id,
                     new_bcam_id,
                     suffix,
                     full_id));
         data.camera_id = std::move(full_id);
      }
   }
}

void write(const BinocularCameraInfo& data, Json::Value& node) noexcept(false)
{
   node = parse_json(data.to_json_string());
}

// --------------------------------------------------------- Euclidean Transform

EuclideanTransform BinocularCameraInfo::euclid_transform() const noexcept
{
   EuclideanTransform et;
   et.scale       = 1.0;
   et.translation = C1();          //
   et.rotation    = q.conjugate(); // Cam to world
   return et;
}

void BinocularCameraInfo::set_from_et(const EuclideanTransform& et01) noexcept
{
   q        = et01.rotation.conjugate();
   baseline = et01.translation.norm();
   t        = -q.apply(et01.translation).normalised();

   if(true) {
      if(!q.is_finite() or !std::isfinite(baseline) or !t.is_finite()) {
         cout << "et01: " << str(et01) << endl;
         cout << "q:    " << q.to_readable_str() << endl;
         cout << "t:    " << str(t) << endl;
         cout << "base: " << baseline << endl;
         FATAL("kBAM!");
      }
   }
}

EuclideanTransform
BinocularCameraInfo::make_et1(const EuclideanTransform& et0) const noexcept
{
   return (et0.inverse() * euclid_transform().inverse()).inverse();
}

// ------------------------------------------------------------ Rectifying Ideal

Vector2 BinocularCameraInfo::rectify_ideal(int cam,
                                           const Vector2& U) const noexcept
{
   const Matrix3r R     = quaternion_to_rot3x3(q);
   const Matrix3r R_inv = R.inverse();
   const Vector3 t      = this->t.normalised();
   const auto baseline  = this->baseline;

   const auto C0 = this->C0(); // Camera center 0
   const auto C1 = this->C1(); // Camera center 1

   // The XYZ co-ordinates of the new camera
   const Vector3 z0 = to_vec3(Vector3r(R.block(2, 0, 1, 3).transpose()));
   const Vector3 X  = (C1 - C0).normalised();
   const Vector3 Y  = cross(z0, X).normalised();
   const Vector3 Z  = cross(X, Y).normalised();

   Matrix3r Rn;
   Rn << X(0), X(1), X(2), Y(0), Y(1), Y(2), Z(0), Z(1), Z(2);

   Matrix3r HR[2];
   HR[0] = Rn;
   HR[1] = Rn * R_inv;

   Expects(cam == CAM0 or cam == CAM1);

   unsigned width = 800, height = 600;

   AABB aabb;
   const auto K
       = well_calibrated_region(M[0], M[1], HR[0], HR[1], width, height, aabb);

   Matrix3r KHR[2];
   KHR[0] = K * HR[0];
   KHR[1] = K * HR[1];

   return homgen_P2_to_R2(to_vec3(KHR[cam] * Vector3r(U.x, U.y, 1.0)));
}

// -------------------------------------------------------------------- Solve 3d

Vector3
BinocularCameraInfo::solve3d_from_distorted(const Vector2& d0,
                                            const Vector2& d1) const noexcept
{
   Camera cam0;
   Camera cam1;
   cam0.set_intrinsic(10.0, 10.0, 1.0, 1.0, Vector2(0.0, 0.0));
   cam1.set_intrinsic(10.0, 10.0, 1.0, 1.0, Vector2(0.0, 0.0));
   cam0.set_extrinsic(Matrix3r::Identity(), Vector3(0.0, 0.0, 0.0));
   Matrix3r R = quaternion_to_rot3x3(q);
   cam1.set_extrinsic(R, t * baseline);

   Matrix3r E = calibration::make_essential_matrix(q, t);

   const auto u0   = M[0].undistort(d0);
   const auto u1   = M[1].undistort(d1);
   const auto ray0 = to_vec3(cam0.back_project(u0));
   const auto ray1 = to_vec3(cam1.back_project(u1));

   const auto r0 = homgen_R2_to_P2(M[0].undistort(d0)).normalised();
   const auto r1
       = q.inverse_rotate(homgen_R2_to_P2(M[1].undistort(d1)).normalised());
   const auto C0 = cam0.C();
   const auto C1 = cam1.C();
   const auto X  = intersect_rays_2(C0, C0 + ray0, C1, C1 + ray1);
   if(false) {
      INFO("solve3d-from-distorted");
      cout << format("working fmt = {}, {}",
                     str(M[0].working_format()),
                     str(M[1].working_format()))
           << endl;
      cout << format("D = {}, {}", str(d0), str(d1)) << endl;
      cout << format("R = {}, {}", str(r0), str(r1)) << endl;
      cout << format("R = {}, {}", str(ray0), str(ray1)) << endl;
      cout << format("C = {}, {}", str(C0), str(C1)) << endl;
      cout << format("e = {}", 400.0 * calibration::xFl_lFx(E, u0, u1)) << endl;
      cout << format("X = {}", str(X)) << endl;
   }
   return X;
}

real BinocularCameraInfo::reproj_err_from_distorted(
    const Vector2& d0,
    const Vector2& d1) const noexcept
{
   const auto C0   = this->C0();
   const auto C1   = this->C1();
   const auto X    = solve3d_from_distorted(d0, d1);
   const auto ray0 = (X - C0).normalised();
   const auto ray1 = (q.rotate((X - C1))).normalised();
   const auto v0   = M[0].distort(homgen_P2_to_R2(ray0));
   const auto v1   = M[1].distort(homgen_P2_to_R2(ray1));
   const auto err  = 0.5 * ((d0 - v0).norm() + (d1 - v1).norm());
   if(false) {
      INFO("reproj-err-from-distorted");
      cout << format("D = {}, {}", str(d0), str(d1)) << endl;
      cout << format("X = {}", str(X)) << endl;
      cout << format("R = {}, {}", str(ray0), str(ray1)) << endl;
      cout << format("V = {}, {}", str(v0), str(v1)) << endl;
      cout << format("e = {}", err) << endl;
   }
   return err;
}

Vector2 BinocularCameraInfo::project(int cam, const Vector3& X) const noexcept
{
   return M[size_t(cam)].distort(homgen_P2_to_R2(to_ray(cam, X)));
   // if(cam == CAM0) return M[0].distort(homgen_P2_to_R2(X - C0()));
   // return M[1].distort(homgen_P2_to_R2(q.rotate(X - C1())));
}

Vector3 BinocularCameraInfo::to_ray(int cam, const Vector3& X) const noexcept
{
   if(cam == CAM0) return (X - C0()).normalised();
   return (q.rotate(X - C1())).normalised();
}

// --------------------------------------------------------- plane ray intersect
// Find 'X' on plane 'p3' that intersects with ray from distorted
// point 'D' in cam 'cam'. 'et0' is the extrinsic transform for
// Cam0. The transform for Cam1 is calculated from 'q', 't', 'baseline', and
// 'et0'.
Vector3
BinocularCameraInfo::plane_ray_intersect(const int cam,
                                         const EuclideanTransform& et0,
                                         const Plane& p3,
                                         const Vector2& D) const noexcept
{
   // Convert to a ray
   auto cam_ray = homgen_R2_to_P2(M[size_t(cam)].undistort(D)).normalised();
   if(cam == CAM1) cam_ray = q.inverse_apply(cam_ray); // cam1 to cam0
   const auto ray = et0.rotation.rotate(cam_ray);      // cam0 to world

   // Camera center
   const auto C = et0.apply(this->C(cam));
   return plane_ray_intersection(p3, C, C + ray);
}

// --------------------------------------------------- print debug triangulation
/// Print debug info on triangulating 3D points P and Q
/// If 'K' is zero, then points [p0, q0] (left image) and
/// [p1, q1] (right image) are considered distorted, and are
/// undistorted using bcam_info.M[cam_ind]. Otherwise 'K'
/// is used to turn them into rays. If the points are already
/// ideal, then set 'K' to the identity matrix.
void print_debug_triangulation(const string& label,
                               const BinocularCameraInfo& bcam_info,
                               const Vector2& p0,
                               const Vector2& q0,
                               const Vector2& p1,
                               const Vector2& q1,
                               const Matrix3r K) noexcept
{
   const bool use_K     = !(K == Matrix3r::Zero());
   const Matrix3r K_inv = use_K ? K.inverse() : K;

   auto undistort = [&](int cam_id, const Vector2& D) -> Vector3 {
      Expects(cam_id >= 0 and cam_id < 2);
      if(use_K) return to_vec3(K_inv * Vector3r(D.x, D.y, 1.0)).normalised();
      const Vector2 U = bcam_info.M[size_t(cam_id)].undistort(D);
      return Vector3(U.x, U.y, 1.0).normalised();
   };

   auto undistort_rotate = [&](int cam_id, const Vector2& D) -> Vector3 {
      const auto ray = undistort(cam_id, D);
      return (cam_id == 0) ? ray : bcam_info.q.conjugate().rotate(ray);
   };

   // Rays
   const Vector3 rp0 = undistort_rotate(0, p0);
   const Vector3 rq0 = undistort_rotate(0, q0);
   const Vector3 rp1 = undistort_rotate(1, p1);
   const Vector3 rq1 = undistort_rotate(1, q1);

   // Cam centres
   const Vector3 C0 = bcam_info.C0();
   const Vector3 C1 = bcam_info.C1();

   // Triangulate
   const Vector3 P = intersect_rays_2(C0, C0 + rp0, C1, C1 + rp1);
   const Vector3 Q = intersect_rays_2(C0, C0 + rq0, C1, C1 + rq1);

   // Errors
   const real P_err = intersect_rays_2_plane_err(C0, C0 + rp0, C1, C1 + rp1);
   const real Q_err = intersect_rays_2_plane_err(C0, C0 + rq0, C1, C1 + rq1);

   // Reproject
   auto project = [&](int cam_id, const Vector3& X) -> Vector2 {
      const auto R = (cam_id == 0) ? X : bcam_info.q.rotate(X - C1);
      if(use_K) return homgen_P2_to_R2(to_vec3(K * to_vec3r(R)));
      return bcam_info.M[size_t(cam_id)].distort(homgen_P2_to_R2(R));
   };

   const auto a0 = project(0, P);
   const auto a1 = project(1, P);
   const auto b0 = project(0, Q);
   const auto b1 = project(1, Q);

   // Now some nice output
   cout << ANSI_COLOUR_WHITE << ANSI_COLOUR_GREEN_BG << string(60, '-')
        << format("  Triangulation Report '{}' ", label) << ANSI_COLOUR_RESET
        << endl;

   cout << format("   p0 = {},  p1 = {}", str(p0), str(p1)) << endl;
   cout << format("   q0 = {},  q1 = {}", str(q0), str(q1)) << endl;
   cout << endl;
   cout << format("  rp0 = {}, rp1 = {}", str(rp0), str(rp1)) << endl;
   cout << format("  rq0 = {}, rq1 = {}", str(rq0), str(rq1)) << endl;
   cout << endl;
   cout << format("   C0 = {}, C1 = {}", str(C0), str(C1)) << endl;
   cout << format("   C0->C1 rot = {}", bcam_info.q.to_readable_str()) << endl;
   if(!use_K) {
      for(auto i = 0; i < 2; ++i)
         cout << format("   Cam[{}] working format: {}",
                        i,
                        str(bcam_info.M[size_t(i)].working_format()))
              << endl;
   }
   cout << endl;
   cout << format("   P = {}  (err = {}mm)", str(P), P_err * 1000.0) << endl;
   cout << format("   Q = {}  (err = {}mm)", str(Q), Q_err * 1000.0) << endl;
   cout << format("   |P - Q| = {}", (P - Q).norm()) << endl;
   cout << endl;
   cout << format("   Reprojections:") << endl;
   cout << format("   p0 |{} - {}| = {}\n", str(p0), str(a0), (p0 - a0).norm());
   cout << format("   p1 |{} - {}| = {}\n", str(p1), str(a1), (p1 - a1).norm());
   cout << format("   q0 |{} - {}| = {}\n", str(q0), str(b0), (q0 - b0).norm());
   cout << format("   q1 |{} - {}| = {}\n", str(q1), str(b1), (q1 - b1).norm());
   cout << "." << endl; // end
}

// ---------------------------------------------------- print debug p3 intersect
void print_debug_p3_intersect(const string& label,
                              const BinocularCameraInfo& bcam_info,
                              const Plane& p3,
                              const Vector2& p0,
                              const Vector2& q0,
                              const Vector2& p1,
                              const Vector2& q1,
                              const Matrix3r K) noexcept
{
   const bool use_K     = !(K == Matrix3r::Zero());
   const Matrix3r K_inv = use_K ? K.inverse() : K;

   auto undistort = [&](int cam_id, const Vector2& D) -> Vector3 {
      Expects(cam_id >= 0 and cam_id < 2);
      if(use_K) return to_vec3(K_inv * Vector3r(D.x, D.y, 1.0)).normalised();
      const Vector2 U = bcam_info.M[size_t(cam_id)].undistort(D);
      return Vector3(U.x, U.y, 1.0).normalised();
   };

   auto undistort_rotate = [&](int cam_id, const Vector2& D) -> Vector3 {
      const auto ray = undistort(cam_id, D);
      return (cam_id == 0) ? ray : bcam_info.q.conjugate().rotate(ray);
   };

   // Rays
   const Vector3 rp0 = undistort_rotate(0, p0);
   const Vector3 rq0 = undistort_rotate(0, q0);
   const Vector3 rp1 = undistort_rotate(1, p1);
   const Vector3 rq1 = undistort_rotate(1, q1);

   // Cam centres
   const Vector3 C0 = bcam_info.C0();
   const Vector3 C1 = bcam_info.C1();

   // Plane-ray intersections
   const Vector3 P0 = plane_ray_intersection(p3, C0, C0 + rp0);
   const Vector3 Q0 = plane_ray_intersection(p3, C0, C0 + rq0);
   const Vector3 P1 = plane_ray_intersection(p3, C1, C1 + rp1);
   const Vector3 Q1 = plane_ray_intersection(p3, C1, C1 + rq1);
   const Vector3 P  = 0.5 * (P1 + P0);
   const Vector3 Q  = 0.5 * (Q1 + Q0);

   // Reproject
   auto project = [&](int cam_id, const Vector3& X) -> Vector2 {
      const auto R = (cam_id == 0) ? X : bcam_info.q.rotate(X - C1);
      if(use_K) return homgen_P2_to_R2(to_vec3(K * to_vec3r(R)));
      return bcam_info.M[size_t(cam_id)].distort(homgen_P2_to_R2(R));
   };

   const auto a0 = project(0, P);
   const auto a1 = project(1, P);
   const auto b0 = project(0, Q);
   const auto b1 = project(1, Q);

   // Now some nice output
   cout << ANSI_COLOUR_WHITE << ANSI_COLOUR_RED_BG << string(60, '-')
        << format("  Plane Intersect Report '{}' ", label) << ANSI_COLOUR_RESET
        << endl;

   cout << format("   p0 = {},  p1 = {}", str(p0), str(p1)) << endl;
   cout << format("   q0 = {},  q1 = {}", str(q0), str(q1)) << endl;
   cout << endl;
   cout << format("  rp0 = {}, rp1 = {}", str(rp0), str(rp1)) << endl;
   cout << format("  rq0 = {}, rq1 = {}", str(rq0), str(rq1)) << endl;
   cout << endl;
   cout << format("   C0 = {}, C1 = {}", str(C0), str(C1)) << endl;
   cout << format("   C0->C1 rot = {}", bcam_info.q.to_readable_str()) << endl;
   if(!use_K) {
      for(auto i = 0; i < 2; ++i)
         cout << format("   Cam[{}] working format: {}",
                        i,
                        str(bcam_info.M[size_t(i)].working_format()))
              << endl;
   }
   cout << endl;
   cout << format("   p3 = {}", str(p3)) << endl;
   cout << format("   P0 = {}, P1 = {}", str(P0), str(P1)) << endl;
   cout << format("   Q0 = {}, Q1 = {}", str(Q0), str(Q1)) << endl;
   cout << endl;
   cout << format("   P  = {},  err = {}", str(P), (P1 - P0).norm()) << endl;
   cout << format("   Q  = {},  err = {}", str(Q), (Q1 - Q0).norm()) << endl;
   cout << format("   |P - Q| = {}", (P - Q).norm()) << endl;
   cout << endl;
   cout << format("   Reprojections:") << endl;
   cout << format("   p0 |{} - {}| = {}\n", str(p0), str(a0), (p0 - a0).norm());
   cout << format("   p1 |{} - {}| = {}\n", str(p1), str(a1), (p1 - a1).norm());
   cout << format("   q0 |{} - {}| = {}\n", str(q0), str(b0), (q0 - b0).norm());
   cout << format("   q1 |{} - {}| = {}\n", str(q1), str(b1), (q1 - b1).norm());
   cout << "." << endl; // end
}

// ----------------------------------------- Estimate Bino Cam Euclid Transforms
// 'e0' and 'e1' are estimated euclidean transforms for each camera.
// The are averaged, and then the bino-cam-info is used to calculate
// the output transformtions.
//
std::pair<EuclideanTransform, EuclideanTransform>
estimate_bino_camera_euclid_transforms(const BinocularCameraInfo& info,
                                       const EuclideanTransform& e0,
                                       const EuclideanTransform& e1)
{
   EuclideanTransform el, er;

   const auto t_av = 0.5 * (e0.translation + e1.translation);
   const auto q_av = average_rotations(e0.rotation, e1.rotation);

   // Vector that travels from cam-center 0 to cam-center 1, from
   // the point of view of the camera
   const auto C01       = -info.q.conjugate().rotate(info.t).normalised();
   const auto direction = q_av.rotate(C01);
   const auto aa        = info.q.to_axis_angle(); // From C0 to C1... halve
   const auto q = axis_angle_to_quaternion(Vector4{aa.xyz(), aa.w * 0.5});

   el.translation = t_av - 0.5 * info.baseline * direction;
   er.translation = t_av + 0.5 * info.baseline * direction;
   el.rotation    = q_av * q.conjugate();
   er.rotation    = q_av * q;
   el.scale = er.scale = 1.0;

   return std::make_pair(el, er);
}

// ----------------------------------------------- transfer point between images

Vector2
transfer_point_between_images(const Vector2& x0,
                              const Plane& p3_et0,           // et0 coords
                              const EuclideanTransform& et0, // world->cam
                              const EuclideanTransform& et1, // world->cam
                              const CachingUndistortInverse& cu0,
                              const CachingUndistortInverse& cu1) noexcept
{
   const auto U = homgen_R2_to_P2(cu0.undistort(x0));
   const auto W
       = et0.inverse_apply(plane_ray_intersection(p3_et0, Vector3{}, U));
   return cu1.distort(homgen_P2_to_R2(et1.apply(W)));
}

// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
// --                             Binocular Camera                            --
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------

// ---------------------------------------------------------------- Construction

This::BinocularCamera()
{
   for(auto i = 0; i < 2; ++i) {
      HR_[i] = KRR_[i] = KR_[i] = KRR_inv_[i] = Kn_[i] = Matrix3r::Zero();
   }
}

// ------------------------------------------------------------------------ Init

void This::init(const Quaternion& q,
                const Vector3& t,
                const real baseline,
                const unsigned distorted_w,
                const unsigned distorted_h,
                const Matrix3r& K,
                const unsigned width,
                const unsigned height,
                const bool use_calib_roi)
{
   w_ = width;
   h_ = height;
   set_working_format(distorted_w, distorted_h);

   q_        = q;
   R_        = quaternion_to_rot3x3(q);
   R_inv_    = R_.inverse();
   t_        = t.normalised();
   baseline_ = baseline;

   C_[0] = Vector3(0.0, 0.0, 0.0);
   C_[1] = -to_vec3(R_inv_ * to_vec3r(t_ * baseline_));

   // The XYZ co-ordinates of the new camera
   const Vector3 z0 = to_vec3(Vector3r(R_.block(2, 0, 1, 3).transpose()));
   const Vector3 X  = (C_[1] - C_[0]).normalised();
   const Vector3 Y  = cross(z0, X).normalised();
   const Vector3 Z  = cross(X, Y).normalised();

   Matrix3r Rn;
   Rn << X(0), X(1), X(2), Y(0), Y(1), Y(2), Z(0), Z(1), Z(2);

   rect_q_ = rot3x3_to_quaternion(Rn);

   HR_[0] = Rn;
   HR_[1] = Rn * R_inv_;

   if(use_calib_roi) {
      AABB aabb;
      const auto K = well_calibrated_region(
          M_[0], M_[1], HR_[0], HR_[1], width, height, aabb);

      if(!is_finite(K)) {
         LOG_ERR("attempt to initialize a binocular camera with erroneous "
                 "calibration region of interest:");
         cout << M_[0].to_string() << endl;
         cout << M_[1].to_string() << endl;
         cout << format("width-height = [{}x{}]", width, height) << endl;
         cout << "calculated sensor roi = " << str(aabb) << endl;
         cout << "HR[0] = " << endl << HR_[0] << endl << endl;
         cout << "HR[1] = " << endl << HR_[1] << endl << endl;
         cout << "K = " << endl << K << endl << endl;
         cout << to_vec3(K * Vector3r(aabb.left, aabb.top, 1.0)) << endl;
         cout << to_vec3(K * Vector3r(aabb.right, aabb.top, 1.0)) << endl;
         cout << to_vec3(K * Vector3r(aabb.right, aabb.bottom, 1.0)) << endl;
         cout << to_vec3(K * Vector3r(aabb.left, aabb.bottom, 1.0)) << endl;
         FATAL("Aborting due to previous errors");
      }

      Kn_[0] = Kn_[1] = K;

   } else {
      if(!is_finite(K)) {
         LOG_ERR("attempt to initialize a binocular camera with erroneous 'K' "
                 "matrix:");
         cout << K << endl << endl;
         FATAL("Aborting due to previous errors");
      }
      Kn_[0] = Kn_[1] = K;
   }

   KR_[0] = Kn_[0] * HR_[0];
   KR_[1] = Kn_[1] * HR_[1];

   KRR_[0]     = KR_[0];
   KRR_[1]     = KR_[1] * R_;
   KRR_inv_[0] = KRR_[0].inverse();
   KRR_inv_[1] = KRR_[1].inverse();

   {
      bool has_error   = false;
      auto test_matrix = [&](const auto& matrix, const string_view name) {
         if(is_finite(matrix)) return;
         LOG_ERR(format("attempt to initialize a binocular camera with "
                        "invalid data. Matrix '{}' = ",
                        name));
         cout << matrix << endl << endl;
      };

      test_matrix(Rn, "Rn");
      test_matrix(HR_[0], "HR_[0]");
      test_matrix(HR_[1], "HR_[1]");
      test_matrix(Kn_[0], "Kn_[0]");
      test_matrix(Kn_[1], "Kn_[1]");
      test_matrix(KR_[0], "KR_[0]");
      test_matrix(KR_[1], "KR_[1]");
      test_matrix(KRR_[0], "KRR_[0]");
      test_matrix(KRR_[1], "KRR_[1]");
      test_matrix(KRR_inv_[0], "KRR_inv_[0]");
      test_matrix(KRR_inv_[1], "KRR_inv_[1]");

      if(has_error) FATAL("Aborting due to previous errors");
   }
}

void This::init(const DistortionModel& M0,
                const DistortionModel& M1,
                const Quaternion& q,
                const Vector3& t,
                const real baseline,
                const unsigned distorted_w,
                const unsigned distorted_h,
                const Matrix3r& K,
                const unsigned width,
                const unsigned height,
                const bool use_calib_roi)
{
   if(false) {
      INFO("bcam init:");
      cout << format("wh = {}x{}", width, height) << endl;
      cout << "K = " << endl << K << endl << endl;
   }

   {
      This tmp;
      *this = tmp; // reinitialize all data
   }

   M_[0] = M0;
   M_[1] = M1;

   for(auto i = 0; i < 2; ++i) {
      cu_[i].init(M_[i]);
      M_[i].finalize();
   }

   if(false) {
      auto s = time_thunk([&]() {
         cu_[0].init(M_[0]);
         cu_[1].init(M_[1]);
      });
      // INFO(format("cu.init took {} seconds", s));

      auto test_it
          = [&](const DistortionModel& M,
                const CachingUndistortInverse& cu) { // Compare distort...
               const auto U  = M.undistort(Vector2(237, 84));
               const auto D1 = M.distort(U);
               const auto D2 = cu.distort(U);
               cout << format("<{}> U = {}, |{} - {}| = {}",
                              M.sensor_id(),
                              str(U),
                              str(D1),
                              str(D2),
                              (D1 - D2).norm())
                    << endl;
            };
      test_it(M_[0], cu(0));
      test_it(M_[1], cu(1));
   }

   this->init(q,
              t,
              baseline,
              distorted_w,
              distorted_h,
              K,
              width,
              height,
              use_calib_roi);
}

void This::init(const BinocularCameraInfo& info,
                const unsigned distorted_w,
                const unsigned distorted_h,
                const Matrix3r& K,
                const unsigned width,
                const unsigned height,
                const bool use_calib_roi)
{
   init(info.M[0],
        info.M[1],
        info.q,
        info.t.normalised(),
        info.baseline,
        distorted_w,
        distorted_h,
        K,
        width,
        height,
        use_calib_roi);
}

void This::set_baseline(real baseline)
{
   baseline_ = baseline;
   C_[1]     = -to_vec3(R_inv_ * to_vec3r(t_ * baseline_));
}

void This::set_working_format(const unsigned distorted_w,
                              const unsigned distorted_h)
{
   for(auto i = 0; i < 2; ++i) {
      M_[i].set_working_format(distorted_w, distorted_h);
      M_[i].finalize();
      cu_[i].set_working_format(distorted_w, distorted_h);
   }
}

// -------------------------------------------------------------------- Make Et1

// If 'et0' is transform for cam0, this returns transform for cam1
// NOTE: the euclid transform 'et0' gives 'eye-coords' => 'world-coords'.
// Thus, et0.inverse(X) gives 'world=>eye', which is used for projecting to
// camera images.
EuclideanTransform This::make_et1(const EuclideanTransform& et0) const noexcept
{
   auto et01        = EuclideanTransform();
   et01.translation = C(1);
   et01.scale       = 1.0;
   et01.rotation    = q().conjugate();
   return (et0.inverse() * et01.inverse()).inverse();
}

// --------------------------------------------------------------------- Solve3d

Vector3 This::solve3d(real x0, real x1, real y) const noexcept
{
   return solve3d(x0, y, x1, y);
}

Vector3 This::solve3d(const Vector2& x0, const Vector2& x1) const noexcept
{
   return solve3d(x0.x, x0.y, x1.x, x1.y);
}

Vector3 This::solve3d(real x0, real y0, real x1, real y1) const noexcept
{
   if(false && feedback_)
      INFO(format("Solve3d: {}, {}, {}, {}", x0, y0, x1, y1));

   Vector3r o0 = KRR_inv_[0] * Vector3r(x0, y0, 1.0);
   Vector3r o1 = KRR_inv_[1] * Vector3r(x1, y1, 1.0);
   return intersect_rays_2(
       C_[0], C_[0] + to_vec3(o0), C_[1], C_[1] + to_vec3(o1));
}

real This::solve3d_3d_err(real x0, real y0, real x1, real y1) const noexcept
{
   Vector3r o0 = KRR_inv_[0] * Vector3r(x0, y0, 1.0);
   Vector3r o1 = KRR_inv_[1] * Vector3r(x1, y1, 1.0);
   return intersect_rays_2_plane_err(C_[0],
                                     C_[0] + to_vec3(o0).normalised(),
                                     C_[1],
                                     C_[1] + to_vec3(o1).normalised());
}

real This::reproj_err_from_distorted(const Vector2& d0,
                                     const Vector2& d1) const noexcept
{
   const auto X = solve3d_from_distorted(d0, d1);
   return (d0 - project(CAM0, X)).norm() + (d1 - project(CAM1, X)).norm();

   const auto u0 = to_vec3r(homgen_R2_to_P2(model(0).undistort(d0)));
   const auto u1 = to_vec3r(homgen_R2_to_P2(model(1).undistort(d1)));
   const auto r0 = homgen_P2_to_R2(to_vec3(KR(CAM0) * u0));
   const auto r1 = homgen_P2_to_R2(to_vec3(KR(CAM1) * u1));
   return reproj_err(r0, r1);
}

real This::reproj_err(const Vector2& x0, const Vector2& x1) const noexcept
{
   const auto X = solve3d(x0, x1);
   return (x0 - project(CAM0, X)).norm() + (x1 - project(CAM1, X)).norm();
}

real This::reproj_err(real x0, real y0, real x1, real y1) const noexcept
{
   return reproj_err(Vector2(x0, y0), Vector2(x1, y1));
}

Vector3 This::solve3d_from_distorted(const Vector2& x0,
                                     const Vector2& x1) const noexcept
{
   return solve3d_from_undistorted(M_[0].undistort(x0), M_[1].undistort(x1));
}

Vector3 This::solve3d_from_undistorted(const Vector2& u0,
                                       const Vector2& u1) const noexcept
{
   return solve3d_from_rays(homgen_R2_to_P2(u0).normalized(),
                            homgen_R2_to_P2(u1).normalized());
}

// Solve 3D from rays
Vector3 This::solve3d_from_rays(const Vector3& ray0,
                                const Vector3& ray1) const noexcept
{
   const auto Rray1 = to_vec3(R_inv_ * to_vec3r(ray1));
   return intersect_rays(C_[0], C_[0] + ray0, C_[1], C_[1] + Rray1);
}

// ---------------------------------------------------------------- Back-project

Vector3 This::to_ray(int cam_ind, const Vector2& distorted) const noexcept
{
   Expects(cam_ind == CAM0 || cam_ind == CAM1);
   return homgen_R2_to_P2(M_[cam_ind].undistort(distorted)).normalized();
}

Vector3 This::to_ray(int cam_ind, const Vector3r& D) const noexcept
{
   Expects(cam_ind == CAM0 || cam_ind == CAM1);
   return to_vec3(M_[cam_ind].undistort(D)).normalized();
}

Vector3 This::rectified_to_ray(int cam_ind, const Vector2& x) const noexcept
{
   Expects(cam_ind == CAM0 || cam_ind == CAM1);
   return rectified_to_ray(cam_ind, Vector3r(x.x, x.y, 1.0));
}

Vector3 This::rectified_to_ray(int cam_ind, const Vector3r& x) const noexcept
{
   Expects(cam_ind == CAM0 || cam_ind == CAM1);
   return to_vec3(KRR_inv_[cam_ind] * x).normalized();
}

// --------------------------------------------------------------------- Project

Vector2 This::project(int cam_ind, const Vector3& X) const
{
   Expects(cam_ind == CAM0 || cam_ind == CAM1);
   Vector3r Xr;
   if(cam_ind == CAM0)
      Xr = normalized_P2(to_vec3r(X - C(CAM0)));
   else
      Xr = normalized_P2(R_ * to_vec3r(X - C(CAM1)));
   const auto U = Vector2{Xr(0), Xr(1)};

   return cu(cam_ind).distort(U);
   // if(cu(cam_ind).in_bounds(U)) return cu(cam_ind).distort(U);
   // return M_[cam_ind].distort(U);
}

Vector2 This::project_to_rectified(int cam_ind, const Vector3& X) const
{
   Expects(cam_ind == CAM0 || cam_ind == CAM1);
   return homgen_P2_to_R2(to_vec3(KRR_[cam_ind] * to_vec3r(X - C(cam_ind))));
}

// ------------------------------------------------------------------- Disparity

real This::disparity(const Vector3& X) const
{
   auto xl = project_to_rectified(CAM0, X);
   auto xr = project_to_rectified(CAM1, X);
   return xl.x - xr.x;
}

// -------------------------------------------------------------- Epipolar Curve

// Epipolar "ellipses"... actually quadric beziers
// 'X' is a pixel in the left camera,
// 'min/max-dist' give the limits of the bezier
// 'A', 'B', and 'C' are the three control points of the output bezier
void This::epipolar_curve(const Vector2& x,
                          real min_dist,
                          real max_dist,
                          Vector2& a,
                          Vector2& b,
                          Vector2& c) const
{
   a = b = c = x;

   // First, must back-project 'X'
   const auto Xr = this->to_ray(0, x); // Already normalized
   assert(fabs(Xr.norm() - 1.0) < 1e-6);

   a = project(CAM1, C(CAM0) + min_dist * Xr);
   c = project(CAM1, C(CAM0) + max_dist * Xr);

   Vector2 a1 = project(CAM1, C(CAM0) + (min_dist + 1e-5) * Xr);
   Vector2 c1 = project(CAM1, C(CAM0) + (max_dist - 1e-5) * Xr);

   // 'b' is the intersection of the line [a-a1] and [c-c1]
   b = homgen_P2_to_R2(cross(to_homgen_line(a, a1), to_homgen_line(c, c1)));
}

// ------------------------------------------------------------------- get_mapxy

void This::get_mapxy(cv::Mat mapx[2], cv::Mat mapy[2]) const
{
   for(unsigned i = 0; i < 2; ++i) { get_sensor_mapxy(i, mapx[i], mapy[i]); }
}

// ------------------------------------------------------------ get_sensor_mapxy

void This::get_sensor_mapxy(unsigned sensor_num,
                            cv::Mat& mapx,
                            cv::Mat& mapy) const
{
   Expects(sensor_num < 2);

   static const Matrix3r H = Matrix3r::Identity();
   const auto fmt          = M_[sensor_num].working_format();
   const auto fx           = real(w_) / real(fmt.x);
   const auto fy           = real(h_) / real(fmt.y);

   // get_cached_mapxy(w_, h_, H, cu_[i], KR_[i], mapx[i], mapy[i]);

   auto f = [&](const Vector2& x) {
      assert(x.is_finite());
      const auto d = cu(int(sensor_num)).distort(x);
      assert(d.is_finite());
      return d;
   };

   create_cv_remap(w_, h_, H, f, KR_[sensor_num], mapx, mapy);
}

// ------------------------------------------------------------------- to-string

std::string This::to_string() const
{
   return format(R"V0G0N(
BinocularCamera
   A-dims:      {}x{}
   out-dims:    {}x{}
   C0:          {}
   C1:          {}
   rot:         {} deg about {}
   baseline:    {}
   M0:          {}
   M1:          {}
)V0G0N",
                 M_width(),
                 M_height(),
                 w_,
                 h_,
                 C_[0].to_string(),
                 C_[1].to_string(),
                 to_degrees(q_.to_axis_angle().w),
                 q_.to_axis_angle().xyz().to_string(),
                 baseline_,
                 indent(M_[0].to_string(), 16),
                 indent(M_[1].to_string(), 16));
}

// ------------------------------------------------------ well-calibrated region

Matrix3r well_calibrated_region(const DistortionModel& M0,
                                const DistortionModel& M1,
                                const Matrix3r& H0,
                                const Matrix3r& H1,
                                const unsigned width,
                                const unsigned height,
                                AABB& inscribed_rect)
{
   auto D_to_X = [](const auto& M, const auto& HR, const Vector2& D) {
      auto W = M.calib_to_working_format(D);
      auto R = homgen_R2_to_P2(M.undistort(W));
      auto X = homgen_P2_to_R2(to_vec3(HR * to_vec3r(R)));

      if(false) {
         cout << format("# ------------") << endl
              << format("     D = {}", str(D)) << endl
              << format("     W = {}", str(W)) << endl
              << format("     R = {}", str(R)) << endl
              << format("     X = {}", str(X)) << endl
              << endl;
      }

      return X;
   };

   auto find_hull = [&](const auto& M, const auto& HR) {
      // This is the distorted hull, generated during calibration
      const auto& distort_hull = M.calib_hull();

      auto d_to_x = [&](const Vector2& D) { return D_to_X(M, HR, D); };

      // Find the undistorted and rectified hull
      vector<Vector2> hull;
      hull.resize(distort_hull.size());

      std::transform(
          cbegin(distort_hull), cend(distort_hull), begin(hull), d_to_x);

      return hull;
   };

   auto hull0 = find_hull(M0, H0);
   auto hull1 = find_hull(M1, H1);

   AABB aabb;
   {
      static std::mutex padlock;
      lock_guard<decltype(padlock)> lock(padlock);
      // INFO(format("HULL ON {}, {}", M0.sensor_id(), M1.sensor_id()));
      const bool do_trace = false; // (M0.sensor_id() == "STR00042"s);
      aabb = inscribed_rect_two_hulls(hull0, hull1, 1e-3, do_trace);
   }

   Matrix3r K = Matrix3r::Identity();
   K(0, 0)    = width / aabb.width();
   K(1, 1)    = height / aabb.height();
   K(0, 2)    = -aabb.left * width / aabb.width();
   K(1, 2)    = -aabb.top * height / aabb.height();

   inscribed_rect = aabb;

   {
      auto generate_hull = [&](const auto& M,
                               const auto& HR,
                               const string& fname) {
         ARGBImage argb;
         argb.resize(unsigned(M.calib_format().x),
                     unsigned(M.calib_format().y));
         argb.zero();
         const auto gt_hull = M.calib_hull();
         for(auto i = 0u; i < gt_hull.size(); ++i) {
            const auto A = gt_hull[i];
            const auto B = gt_hull[(i + 1) % gt_hull.size()];
            plot_line_AA(A, B, [&](int x, int y, float a) {
               if(argb.in_bounds(x, y))
                  argb(x, y) = blend(k_white, argb(x, y), a);
            });

            const auto X = D_to_X(M, HR, A);
            render_string(
                argb, format("X = {}", str(X)), to_pt2(A), k_yellow, k_black);
         }
         argb.save(fname);
         INFO(format("Hull for {} saved to {}", M.sensor_id(), fname));
      };

      if(aabb.area() < 1e-3) {
         LOG_ERR("Failed to create an inscripted AABB for two camera sensors.");
         cout << "multiview failed to find an overlapping well-calibrated "
                 "region "
                 " between the two sensors -- considering their relative "
                 "rotation";

         cout << format("hull0 = [{}]",
                        implode(cbegin(hull0), cend(hull0), ", "))
              << endl;
         cout << format("hull1 = [{}]",
                        implode(cbegin(hull1), cend(hull1), ", "))
              << endl;
         cout << format("aabb  = {}, area = {}", str(aabb), aabb.area())
              << endl;

         generate_hull(M0, H0, format("/tmp/hull-{}.png", M0.sensor_id()));
         generate_hull(M1, H1, format("/tmp/hull-{}.png", M1.sensor_id()));

         FATAL("aborting due to previous errors");
      }
   }

   return K;
}

// ------------------------------------------------------- rectify >-> distorted

Vector2 This::rectify_distorted(int cam_ind, const Vector2& D) const noexcept
{
   const auto U = cu(cam_ind).undistort(D);
   if(cam_ind == 0) return rectify_ideal(cam_ind, U);
   const auto Y = q().inverse_apply(homgen_R2_to_P2(U));
   return rectify_ideal(cam_ind, homgen_P2_to_R2(Y));
}

Vector2 This::rectify_ideal(int cam_ind, const Vector2& U) const noexcept
{
   return homgen_P2_to_R2(
       to_vec3(KRR_[cam_ind] * to_vec3r(homgen_R2_to_P2(U))));
}

Vector2 This::unrectify_to_ideal(int cam_ind, const Vector2& R) const noexcept
{
   return homgen_P2_to_R2(
       to_vec3(KRR_inv_[cam_ind] * to_vec3r(homgen_R2_to_P2(R))));
}

Vector2 This::unrectify_to_distorted(int cam_ind,
                                     const Vector2& R) const noexcept
{
   const auto U = unrectify_to_ideal(cam_ind, R);
   if(cam_ind == 0) return cu(cam_ind).distort(U);
   const auto Y = q().apply(homgen_R2_to_P2(U));
   return cu(cam_ind).distort(homgen_P2_to_R2(Y));
}

// --------------------------------------------------------- plane ray intersect

// Takes distorted point 'D' in camera 'cam-ind', and finds the
// plane-ray intersection with plane 'p3'
Vector3 This::plane_ray_intersect(int cam_ind,
                                  const EuclideanTransform& extrinsic,
                                  const Plane& p3,
                                  const Vector2& D) const noexcept
{
   // Convert to a ray
   auto cam_ray = homgen_R2_to_P2(model(cam_ind).undistort(D)).normalised();
   if(cam_ind == CAM1) cam_ray = q().inverse_apply(cam_ray); // cam1 to cam0
   const auto ray = extrinsic.rotation.rotate(cam_ray);      // cam0 to world

   // Camera center
   const auto C = extrinsic.apply(this->C(cam_ind));

   return plane_ray_intersection(p3, C, C + ray);
}

Vector2 This::backproject_to_distorted(int cam_ind, const Vector3& X)
{
   if(cam_ind == CAM0) return cu(cam_ind).distort(homgen_P2_to_R2(X));

   auto Y = q().apply(X - C(cam_ind));
   return cu(cam_ind).distort(homgen_P2_to_R2(Y));
}

// ------------------------------------------------------------------- fit plane
// @param bcam_info Binocular Camera
// @param Xs Contiguous array of image points in Cam0
// @param Ys Corresponding contiguous array of points in Cam1
// @param N Number of points. Xs[N-1] and Ys[N-1] must be valid.
// @param K If non-zero, then points are turned to rays with K.inverse(),
//          otherwise bcam_info.M[cam_id].undistort is used.
// Note: Call print_debug_p3_intersect to examine result.
Plane fit_plane(const BinocularCameraInfo& bcam_info,
                const Vector2* xx,
                const Vector2* yy,
                const unsigned N,
                const bool feedback,
                const Matrix3r K) noexcept
{
   const bool use_K          = !(K == Matrix3r::Zero());
   const Matrix3r K_inv      = K.inverse();
   const bool super_feedback = false;

   // Cam centres
   array<Vector3, 2> Cs = {bcam_info.C0(), bcam_info.C1()};

   auto to_ray = [&](int cam_id, const Vector2& x) {
      Expects(cam_id >= 0 and cam_id < 2);
      Vector3 ray;
      if(use_K)
         ray = to_vec3(K_inv * Vector3r(x.x, x.y, 1.0)).normalised();
      else {
         const Vector2 U = bcam_info.M[size_t(cam_id)].undistort(x);
         ray             = Vector3(U.x, U.y, 1.0).normalised();
      }
      return (cam_id == 0) ? ray : bcam_info.q.conjugate().rotate(ray);
   };

   auto intersect = [&](const Vector2& x, const Vector2& y) {
      return intersect_rays_2(
          Cs[0], Cs[0] + to_ray(0, x), Cs[1], Cs[1] + to_ray(1, y));
   };

   auto resolve_p3 = [&](int cam_id, const Vector2& x, const Plane& p3) {
      return plane_ray_intersection(
          p3, Cs[size_t(cam_id)], Cs[size_t(cam_id)] + to_ray(cam_id, x));
   };

   auto project = [&](int cam_id, const Vector3& X) {
      const auto R
          = (cam_id == 0) ? X : bcam_info.q.rotate(X - Cs[size_t(cam_id)]);
      return (use_K) ? homgen_P2_to_R2(to_vec3(K * to_vec3r(R)))
                     : bcam_info.M[size_t(cam_id)].distort(homgen_P2_to_R2(R));
   };

   auto calc_err = [&](const Plane& p3) {
      Expects(p3.is_finite());
      real err = 0.0;
      for(auto i = 0u; i < N; ++i) {
         const auto A  = resolve_p3(0, xx[i], p3);
         const auto B  = resolve_p3(1, yy[i], p3);
         const auto b0 = project(0, B);
         const auto a1 = project(1, A);
         err += 0.5 * ((b0 - xx[i]).norm() + (a1 - yy[i]).norm());
      }
      return err / real(N);
   };

   auto find_d = [&](const Plane& p3) {
      Plane p = p3;
      auto f  = [&](double x) -> double {
         p.w = x;
         return calc_err(p);
      };
      return golden_section_search(f, -1e3, 1e3, 1e-3);
   };

   // Get initial plane estimate
   std::vector<Vector3> Xs(N);
   for(auto i = 0u; i < N; ++i) Xs[i] = intersect(xx[i], yy[i]);
   Plane p3 = fit_plane(&Xs[0], N);

   auto pack = [&](real* X) {
      const auto s3 = cartesian_to_spherical(p3.xyz());
      *X++          = s3.x;
      *X++          = s3.y;
   };

   auto unpack = [&](const real* X) {
      Vector3 s3;
      s3.x     = *X++;
      s3.y     = *X++;
      s3.z     = 1.0;
      p3.xyz() = spherical_to_cartesian(s3);
      p3.w     = find_d(p3);
      Expects(p3.is_finite());
   };

   int counter             = 0;
   real best_score         = std::numeric_limits<real>::max();
   const unsigned n_params = 2;
   vector<real> best_params(n_params);
   auto fn = [&](const real* X) -> real {
      unpack(X);
      const real ret = calc_err(p3);
      if(ret < best_score) {
         best_score = ret;
         std::copy(X, X + n_params, begin(best_params));
         if(super_feedback) {
            cout << format("   #{:8d} :: {:10.7f} :: p3 = {}",
                           counter,
                           best_score,
                           str(p3))
                 << endl;
         }
      }
      counter++;
      return ret;
   };

   vector<real> start(n_params);
   auto do_refine = [&](bool use_nelder_mead) {
      vector<real> xmin(n_params);
      real ynewlo   = dNAN;
      real ystartlo = dNAN;
      real reqmin   = 1e-2;
      real diffstep = 0.1;
      int kcount    = 10000; // max interations
      int icount = 0, numres = 0, ifault = 0;
      const char* method = nullptr;

      if(!use_nelder_mead) {
         method = "levenberg-marquardt";
         levenberg_marquardt(fn,
                             n_params,
                             &start[0],
                             &xmin[0],
                             reqmin,
                             diffstep,
                             4,
                             kcount / 100,
                             icount,
                             ifault);
         ynewlo = fn(&xmin[0]);

      } else {
         method = "nelder-mead";

         vector<real> step(n_params);
         auto X = &step[0];
         for(auto i = 0; i < 2; ++i) *X++ = 2.0 * one_degree(); //
         //*X++ = 0.01;                                     // 1cm

         nelder_mead(fn,
                     n_params,
                     &start[0],
                     &xmin[0],
                     ynewlo,
                     reqmin,
                     &step[0],
                     10,
                     10 * kcount,
                     icount,
                     numres,
                     ifault);
      }
   };

   pack(&start[0]);
   const real ystartlo = fn(&start[0]);
   auto now            = tick();
   for(auto i = 0; i < 1; ++i) {
      do_refine((i % 2) == 0);
      std::copy(cbegin(best_params), cend(best_params), begin(start));
      for(size_t i = 0; i < 2; ++i) // pertube rotation
         start[i] += 0.001 * (uniform() * one_degree() - 0.5 * one_degree());
      // start[2] += 0.001 * (2.0 * uniform() - 1.0);
   }
   const real ynewlo    = fn(&best_params[0]);
   const real elapsed_s = tock(now);

   unpack(&best_params[0]);

   const bool success = ynewlo < 10.0; // TODO, should be in degrees

   if(feedback) { // Print feedback
      std::stringstream ss{""};
      ss << endl;
      ss << format("Feedback fittin plane (binocular)") << endl;
      ss << format("    elapsed time:         {}s", elapsed_s) << endl;
      ss << format("    initial-score:        {}", ystartlo) << endl;
      ss << format("    final-score:          {}", ynewlo) << endl;
      ss << format("    p3:                   {}", str(p3)) << endl;
      cout << ss.str();
   }

   return p3;
}

} // namespace perceive