/**
 * $Id$
 *
 * Software License Agreement (GNU General Public License)
 *
 *  Copyright (C) 2015:
 *
 *    Johann Prankl, prankl@acin.tuwien.ac.at
 *    Aitor Aldoma, aldoma@acin.tuwien.ac.at
 *
 *      Automation and Control Institute
 *      Vienna University of Technology
 *      Gusshausstraße 25-29
 *      1170 Vienn, Austria
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * @author Johann Prankl, Aitor Aldoma
 *
 */

#ifndef KP_BA_COST_FUNCTIONS_HPP
#define KP_BA_COST_FUNCTIONS_HPP

#include <Eigen/Dense>
#include <ceres/rotation.h>
#include <v4r/common/impl/Vector.hpp>
#include <v4r/keypoints/impl/invPose.hpp>

namespace v4r
{

template<class T>
inline void print_values(const std::string &txt, const T &val1, const T &val2, const T &val3, const T &val4)
{
    //Do nothing
}

template<>
inline void print_values<double>(const std::string &txt, const double &val1, const double &val2, const double &val3, const double &val4)
{
  std::cout<<txt<<" "<<val1<<" "<<val2<<" "<<val3<<" "<<val4<<std::endl;
}

/**
 * Apply camera intrinsics to the normalized point to get image coordinates.
 * This applies the radial lens distortion to a point which is in normalized
 * camera coordinates (i.e. the principal point is at (0, 0)) to get image
 * coordinates in pixels. Templated for use with autodifferentiation.
 */
template <typename T>
inline void applyRadialDistortionCameraIntrinsics(const T &focal_length_x,
                                                  const T &focal_length_y,
                                                  const T &principal_point_x,
                                                  const T &principal_point_y,
                                                  const T &k1,
                                                  const T &k2,
                                                  const T &k3,
                                                  const T &p1,
                                                  const T &p2,
                                                  const T &normalized_x,
                                                  const T &normalized_y,
                                                  T *image_x,
                                                  T *image_y) {
  T x = normalized_x;
  T y = normalized_y;

  // Apply distortion to the normalized points to get (xd, yd).
  T r2 = x*x + y*y;
  T r4 = r2 * r2;
  T r6 = r4 * r2;
  T r_coeff = (T(1) + k1*r2 + k2*r4 + k3*r6);
  T xd = x * r_coeff + T(2)*p1*x*y + p2*(r2 + T(2)*x*x);
  T yd = y * r_coeff + T(2)*p2*x*y + p1*(r2 + T(2)*y*y);

  // Apply focal length and principal point to get the final image coordinates.
  *image_x = focal_length_x * xd + principal_point_x;
  *image_y = focal_length_y * yd + principal_point_y;
}

/**
 * Apply camera intrinsics to the normalized point to get image coordinates.
 * Templated for use with autodifferentiation.
 */
template <typename T>
inline void applyCameraIntrinsics(const T &focal_length_x,
                                  const T &focal_length_y,
                                  const T &principal_point_x,
                                  const T &principal_point_y,
                                  const T &normalized_x,
                                  const T &normalized_y,
                                  T *image_x,
                                  T *image_y) {
  // Apply focal length and principal point to get the final image coordinates.
  *image_x = focal_length_x * normalized_x + principal_point_x;
  *image_y = focal_length_y * normalized_y + principal_point_y;
}

/**
 * Cost functor which computes reprojection error of 3D point X
 * on camera defined by angle-axis rotation and it's translation
 * (which are in the same block due to optimization reasons).
 * This functor uses a radial distortion model.
 */
struct RadialDistortionReprojectionError {
  RadialDistortionReprojectionError(const double &_observed_x, const double &_observed_y)
      : observed_x(_observed_x), observed_y(_observed_y) {}

  template <typename T>
  bool operator()(const T* const intrinsics,
                  const T* const R_t,  // Rotation denoted by angle axis
                                       // followed with translation
                  const T* const X,    // Point coordinates 3x1.
                  T* residuals) const {
    // Unpack the intrinsics.
    const T& focal_length_x    = intrinsics[0];
    const T& focal_length_y    = intrinsics[1];
    const T& principal_point_x = intrinsics[2];
    const T& principal_point_y = intrinsics[3];
    const T& k1                = intrinsics[4];
    const T& k2                = intrinsics[5];
    const T& k3                = intrinsics[6];
    const T& p1                = intrinsics[7];
    const T& p2                = intrinsics[8];

    // Compute projective coordinates: x = RX + t.
    T x[3];

    ceres::AngleAxisRotatePoint(R_t, X, x);
    x[0] += R_t[3];
    x[1] += R_t[4];
    x[2] += R_t[5];

    // Compute normalized coordinates: x /= x[2].
    T xn = x[0] / x[2];
    T yn = x[1] / x[2];

    T predicted_x, predicted_y;

    // Apply distortion to the normalized points to get (xd, yd).
    applyRadialDistortionCameraIntrinsics(focal_length_x,
                                          focal_length_y,
                                          principal_point_x,
                                          principal_point_y,
                                          k1, k2, k3,
                                          p1, p2,
                                          xn, yn,
                                          &predicted_x,
                                          &predicted_y);

    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - T(observed_x);
    residuals[1] = predicted_y - T(observed_y);

    return true;
  }

  const double observed_x;
  const double observed_y;
};

/**
 * Cost functor which computes reprojection error of 3D point X
 * on camera defined by angle-axis rotation and it's translation
 * (which are in the same block due to optimization reasons).
 */
struct ReprojectionError {
  ReprojectionError(const double &_observed_x, const double &_observed_y)
      : observed_x(_observed_x), observed_y(_observed_y) {}

  template <typename T>
  bool operator()(const T* const intrinsics,
                  const T* const R_t,  // Rotation denoted by angle axis
                                       // followed with translation
                  const T* const X,    // Point coordinates 3x1.
                  T* residuals) const {
    // Unpack the intrinsics.
    const T& focal_length_x    = intrinsics[0];
    const T& focal_length_y    = intrinsics[1];
    const T& principal_point_x = intrinsics[2];
    const T& principal_point_y = intrinsics[3];

    // Compute projective coordinates: x = RX + t.
    T x[3];

    ceres::AngleAxisRotatePoint(R_t, X, x);
    x[0] += R_t[3];
    x[1] += R_t[4];
    x[2] += R_t[5];

    // Compute normalized coordinates: x /= x[2].
    T xn = x[0] / x[2];
    T yn = x[1] / x[2];

    T predicted_x, predicted_y;

    // Apply distortion to the normalized points to get (xd, yd).
    applyCameraIntrinsics(focal_length_x, focal_length_y, principal_point_x, principal_point_y,
                          xn, yn, &predicted_x, &predicted_y);

    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - T(observed_x);
    residuals[1] = predicted_y - T(observed_y);

    return true;
  }

  const double observed_x;
  const double observed_y;
};

/**
 * This functor uses a radial distortion model.
 */
struct RadialDistortionReprojectionAndDepthError {
  RadialDistortionReprojectionAndDepthError(const double &_observed_x, const double &_observed_y, const double &_inv_depth, const double &_depth_err_weight)
      : observed_x(_observed_x), observed_y(_observed_y), inv_depth(_inv_depth),
        depth_err_weight(_depth_err_weight) {}

  template <typename T>
  bool operator()(const T* const intrinsics,
                  const T* const R_t,  // Rotation denoted by angle axis
                                       // followed with translation
                  const T* const X,    // Point coordinates 3x1.
                  T* residuals) const {
    // Unpack the intrinsics.
    const T& focal_length_x    = intrinsics[0];
    const T& focal_length_y    = intrinsics[1];
    const T& principal_point_x = intrinsics[2];
    const T& principal_point_y = intrinsics[3];
    const T& k1                = intrinsics[4];
    const T& k2                = intrinsics[5];
    const T& k3                = intrinsics[6];
    const T& p1                = intrinsics[7];
    const T& p2                = intrinsics[8];

    // Compute projective coordinates: x = RX + t.
    T x[3];

    ceres::AngleAxisRotatePoint(R_t, X, x);
    x[0] += R_t[3];
    x[1] += R_t[4];
    x[2] += R_t[5];

    // Compute normalized coordinates: x /= x[2].
    T xn = x[0] / x[2];
    T yn = x[1] / x[2];

    T predicted_x, predicted_y;

    // Apply distortion to the normalized points to get (xd, yd).
    applyRadialDistortionCameraIntrinsics(focal_length_x,
                                          focal_length_y,
                                          principal_point_x,
                                          principal_point_y,
                                          k1, k2, k3,
                                          p1, p2,
                                          xn, yn,
                                          &predicted_x,
                                          &predicted_y);

    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - T(observed_x);
    residuals[1] = predicted_y - T(observed_y);
    residuals[2] = T(depth_err_weight)*(T(1.)/x[2] - T(inv_depth));

    return true;
  }

  const double observed_x;
  const double observed_y;
  const double inv_depth;
  const double depth_err_weight;
};

/**
 * Cost functor which computes reprojection and a RGBD-depth error of 3D point X
 * on camera defined by angle-axis rotation and it's translation
 * (which are in the same block due to optimization reasons).
 */
struct ReprojectionAndDepthError {
  ReprojectionAndDepthError(const double &_observed_x, const double &_observed_y, const double &_inv_depth,
        const double &_depth_err_weight)
      : observed_x(_observed_x), observed_y(_observed_y), inv_depth(_inv_depth),
        depth_err_weight(_depth_err_weight) {}

  template <typename T>
  bool operator()(const T* const intrinsics,
                  const T* const R_t,  // Rotation denoted by angle axis
                                       // followed with translation
                  const T* const X,    // Point coordinates 3x1.
                  T* residuals) const {
    // Unpack the intrinsics.
    const T& focal_length_x    = intrinsics[0];
    const T& focal_length_y    = intrinsics[1];
    const T& principal_point_x = intrinsics[2];
    const T& principal_point_y = intrinsics[3];

    // Compute projective coordinates: x = RX + t.
    T x[3];

    ceres::AngleAxisRotatePoint(R_t, X, x);
    x[0] += R_t[3];
    x[1] += R_t[4];
    x[2] += R_t[5];

    // Compute normalized coordinates: x /= x[2].
    T xn = x[0] / x[2];
    T yn = x[1] / x[2];

    T predicted_x, predicted_y;

    // Apply distortion to the normalized points to get (xd, yd).
    applyCameraIntrinsics(focal_length_x, focal_length_y, principal_point_x, principal_point_y,
                          xn, yn, &predicted_x, &predicted_y);

    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - T(observed_x);
    residuals[1] = predicted_y - T(observed_y);
    residuals[2] = T(depth_err_weight)*(T(1.)/x[2] - T(inv_depth));

//    print_values("dx,dy,dd: ",residuals[0],residuals[1],residuals[2],T(0));

    return true;
  }

  const double observed_x;
  const double observed_y;
  const double inv_depth;
  const double depth_err_weight;
};

/**
 * Cost functor which computes reprojection and a RGBD-depth error of 3D point X
 * on camera defined by angle-axis rotation and it's translation
 * (which are in the same block due to optimization reasons).
 */
struct ReprojectionErrorGlobalPoseCamViewData {
  ReprojectionErrorGlobalPoseCamViewData(const double &_observed_x, const double &_observed_y)
      : observed_x(_observed_x), observed_y(_observed_y) {}

  template <typename T>
  bool operator()(const T* const intrinsics,
                  const T* const Rt0,   // Rotation denoted by angle axis followed with translation (...to keyframe)
                  const T* const Rt1,   // ..to proj. frame
                  const T* const X0,    // Point coordinates 3x1. (in keyframe coordinates)
                  T* residuals) const {
    // Unpack the intrinsics.
    const T& focal_length_x    = intrinsics[0];
    const T& focal_length_y    = intrinsics[1];
    const T& principal_point_x = intrinsics[2];
    const T& principal_point_y = intrinsics[3];

    // transform point to global coordinates
    T xg[3];
    T invRt0[6];
    v4r::invPose6(Rt0, &Rt0[3], invRt0, &invRt0[3]);
    ceres::AngleAxisRotatePoint(invRt0, X0, xg);
    xg[0] += invRt0[3];
    xg[1] += invRt0[4];
    xg[2] += invRt0[5];

    // Compute projective coordinates: x = RX + t.
    T x[3];

    ceres::AngleAxisRotatePoint(Rt1, xg, x);
    x[0] += Rt1[3];
    x[1] += Rt1[4];
    x[2] += Rt1[5];

    // Compute normalized coordinates: x /= x[2].
    T xn = x[0] / x[2];
    T yn = x[1] / x[2];

    T predicted_x, predicted_y;

    // Apply distortion to the normalized points to get (xd, yd).
    applyCameraIntrinsics(focal_length_x, focal_length_y, principal_point_x, principal_point_y, xn, yn, &predicted_x, &predicted_y);

    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - T(observed_x);
    residuals[1] = predicted_y - T(observed_y);

    //print_values("dx,dy,dd: ",residuals[0],residuals[1],residuals[2],T(0));

    return true;
  }

  const double observed_x;
  const double observed_y;
};

/**
 * Cost functor which computes reprojection and a RGBD-depth error of 3D point X
 * on camera defined by angle-axis rotation and it's translation
 * (which are in the same block due to optimization reasons).
 */
struct ReprojectionErrorGlobalPoseDeltaPoseCamViewData {
  ReprojectionErrorGlobalPoseDeltaPoseCamViewData(const double &_observed_x, const double &_observed_y)
      : observed_x(_observed_x), observed_y(_observed_y) {}

  template <typename T>
  bool operator()(const T* const intrinsics,
                  const T* const Rt0,   // Rotation denoted by angle axis followed with translation (...to keyframe)
                  const T* const Rt1,   // ..to proj. frame
                  const T* const Rtd,   // ..to proj. frame
                  const T* const X0,    // Point coordinates 3x1. (in keyframe coordinates)
                  T* residuals) const {

    // Unpack the intrinsics.
    const T& focal_length_x    = intrinsics[0];
    const T& focal_length_y    = intrinsics[1];
    const T& principal_point_x = intrinsics[2];
    const T& principal_point_y = intrinsics[3];

    // transform point to cloud pose
    T xd[3];
    T invRtd[6];
    v4r::invPose6(Rtd, &Rtd[3], invRtd, &invRtd[3]);
    ceres::AngleAxisRotatePoint(invRtd, X0, xd);
    xd[0] += invRtd[3];
    xd[1] += invRtd[4];
    xd[2] += invRtd[5];

    // transform point to global coordinates
    T xg[3];
    T invRt0[6];
    v4r::invPose6(Rt0, &Rt0[3], invRt0, &invRt0[3]);
    ceres::AngleAxisRotatePoint(invRt0, xd, xg);
    xd[0] += invRt0[3];
    xd[1] += invRt0[4];
    xd[2] += invRt0[5];

    // Compute projective coordinates: x = RX + t.
    T x[3];
    ceres::AngleAxisRotatePoint(Rt1, xg, x);
    x[0] += Rt1[3];
    x[1] += Rt1[4];
    x[2] += Rt1[5];

    // Compute projective coordinates: x = RX + t.
    T xrgb[3];
    ceres::AngleAxisRotatePoint(Rtd, xg, xrgb);
    xrgb[0] += Rtd[3];
    xrgb[1] += Rtd[4];
    xrgb[2] += Rtd[5];

    // Compute normalized coordinates: x /= x[2].
    T xn = xrgb[0] / xrgb[2];
    T yn = xrgb[1] / xrgb[2];

    T predicted_x, predicted_y;

    // Apply distortion to the normalized points to get (xd, yd).
    applyCameraIntrinsics(focal_length_x, focal_length_y, principal_point_x, principal_point_y, xn, yn, &predicted_x, &predicted_y);

    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - T(observed_x);
    residuals[1] = predicted_y - T(observed_y);

    //print_values("dx,dy,dd: ",residuals[0],residuals[1],residuals[2],T(0));

    return true;
  }

  const double observed_x;
  const double observed_y;
};

/**
 * This functor uses a radial distortion model.
 */
struct RadialDistortionReprojectionErrorGlobalPoseCamViewData {
  RadialDistortionReprojectionErrorGlobalPoseCamViewData(const double &_observed_x, const double &_observed_y)
      : observed_x(_observed_x), observed_y(_observed_y) {}

  template <typename T>
  bool operator()(const T* const intrinsics,
                  const T* const Rt0,   // Rotation denoted by angle axis followed with translation (...to keyframe)
                  const T* const Rt1,   // ..to proj. frame
                  const T* const X0,    // Point coordinates 3x1.
                  T* residuals) const {
    // Unpack the intrinsics.
    const T& focal_length_x    = intrinsics[0];
    const T& focal_length_y    = intrinsics[1];
    const T& principal_point_x = intrinsics[2];
    const T& principal_point_y = intrinsics[3];
    const T& k1                = intrinsics[4];
    const T& k2                = intrinsics[5];
    const T& k3                = intrinsics[6];
    const T& p1                = intrinsics[7];
    const T& p2                = intrinsics[8];

    // transform point to global coordinates
    T xg[3];
    T invRt0[6];
    v4r::invPose6(Rt0, &Rt0[3], invRt0, &invRt0[3]);
    ceres::AngleAxisRotatePoint(invRt0, X0, xg);
    xg[0] += invRt0[3];
    xg[1] += invRt0[4];
    xg[2] += invRt0[5];

    // Compute projective coordinates: x = RX + t.
    T x[3];

    ceres::AngleAxisRotatePoint(Rt1, xg, x);
    x[0] += Rt1[3];
    x[1] += Rt1[4];
    x[2] += Rt1[5];

    // Compute normalized coordinates: x /= x[2].
    T xn = x[0] / x[2];
    T yn = x[1] / x[2];

    T predicted_x, predicted_y;

    // Apply distortion to the normalized points to get (xd, yd).
    applyRadialDistortionCameraIntrinsics(focal_length_x, focal_length_y, principal_point_x, principal_point_y, k1, k2, k3, p1, p2, xn, yn, &predicted_x, &predicted_y);

    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - T(observed_x);
    residuals[1] = predicted_y - T(observed_y);

    return true;
  }

  const double observed_x;
  const double observed_y;
};

/**
 * This functor uses a radial distortion model.
 */
struct RadialDistortionReprojectionErrorGlobalPoseDeltaPoseCamViewData {
  RadialDistortionReprojectionErrorGlobalPoseDeltaPoseCamViewData(const double &_observed_x, const double &_observed_y)
      : observed_x(_observed_x), observed_y(_observed_y) {}

  template <typename T>
  bool operator()(const T* const intrinsics,
                  const T* const Rt0,   // Rotation denoted by angle axis followed with translation (...to keyframe)
                  const T* const Rt1,   // ..to proj. frame
                  const T* const Rtd,   // ..cloud->RGB delta pose
                  const T* const X0,    // Point coordinates 3x1.
                  T* residuals) const {
    // Unpack the intrinsics.
    const T& focal_length_x    = intrinsics[0];
    const T& focal_length_y    = intrinsics[1];
    const T& principal_point_x = intrinsics[2];
    const T& principal_point_y = intrinsics[3];
    const T& k1                = intrinsics[4];
    const T& k2                = intrinsics[5];
    const T& k3                = intrinsics[6];
    const T& p1                = intrinsics[7];
    const T& p2                = intrinsics[8];

    // transform point to cloud pose
    T xd[3];
    T invRtd[6];
    v4r::invPose6(Rtd, &Rtd[3], invRtd, &invRtd[3]);
    ceres::AngleAxisRotatePoint(invRtd, X0, xd);
    xd[0] += invRtd[3];
    xd[1] += invRtd[4];
    xd[2] += invRtd[5];

    // transform point to global coordinates
    T xg[3];
    T invRt0[6];
    v4r::invPose6(Rt0, &Rt0[3], invRt0, &invRt0[3]);
    ceres::AngleAxisRotatePoint(invRt0, xd, xg);
    xg[0] += invRt0[3];
    xg[1] += invRt0[4];
    xg[2] += invRt0[5];

    // Compute projective coordinates: x = RX + t.
    T x[3];
    ceres::AngleAxisRotatePoint(Rt1, xg, x);
    x[0] += Rt1[3];
    x[1] += Rt1[4];
    x[2] += Rt1[5];

    // Compute projective coordinates: x = RX + t.
    T xrgb[3];
    ceres::AngleAxisRotatePoint(Rtd, x, xrgb);
    xrgb[0] += Rtd[3];
    xrgb[1] += Rtd[4];
    xrgb[2] += Rtd[5];

    // Compute normalized coordinates: x /= x[2].
    T xn = xrgb[0] / xrgb[2];
    T yn = xrgb[1] / xrgb[2];

    T predicted_x, predicted_y;

    // Apply distortion to the normalized points to get (xd, yd).
    applyRadialDistortionCameraIntrinsics(focal_length_x, focal_length_y, principal_point_x, principal_point_y, k1, k2, k3, p1, p2, xn, yn, &predicted_x, &predicted_y);

    // The error is the difference between the predicted and observed position.
    residuals[0] = predicted_x - T(observed_x);
    residuals[1] = predicted_y - T(observed_y);

    return true;
  }

  const double observed_x;
  const double observed_y;
};

/**
 * This functor uses a point to plane error model
 */
struct PointToPlaneErrorGlobalPoseCamViewData {
  PointToPlaneErrorGlobalPoseCamViewData(const Eigen::Vector3d &_pt0, const Eigen::Vector3d &_n0, const  Eigen::Vector3d &_pt1, const double &_w)
      : pt0(_pt0), n0(_n0), pt1(_pt1), error_weight(_w) {}

  template <typename T>
  bool operator()(const T* const Rt0,   // Rotation denoted by angle axis followed with translation (...to keyframe)
                  const T* const Rt1,   // ..to proj. frame
                  T* residuals) const {

    Eigen::Matrix<T,3,1> pt0e, n0e, pt0g, n0g, pt1t, n1t, pt1e;

    pt0e[0] = T(pt0[0]);
    pt0e[1] = T(pt0[1]);
    pt0e[2] = T(pt0[2]);
    n0e[0] = T(n0[0]);
    n0e[1] = T(n0[1]);
    n0e[2] = T(n0[2]);
    pt1e[0] = T(pt1[0]);
    pt1e[1] = T(pt1[1]);
    pt1e[2] = T(pt1[2]);

    // transform point to global coordinates
    T invRt0[6];
    v4r::invPose6(Rt0, &Rt0[3], invRt0, &invRt0[3]);
    ceres::AngleAxisRotatePoint(invRt0, &n0e[0], &n0g[0]);
    ceres::AngleAxisRotatePoint(invRt0, &pt0e[0], &pt0g[0]);
    pt0g[0] += invRt0[3];
    pt0g[1] += invRt0[4];
    pt0g[2] += invRt0[5];

    // transform to view 1: x = RX + t.
    ceres::AngleAxisRotatePoint(Rt1, &n0g[0], &n1t[0]);
    ceres::AngleAxisRotatePoint(Rt1, &pt0g[0], &pt1t[0]);
    pt1t[0] += Rt1[3];
    pt1t[1] += Rt1[4];
    pt1t[2] += Rt1[5];

    // compute the point to plane distance
    //residuals[0] = T(error_weight) * /*(T(2.)/(pt1e[2]+pt1t[2])) **/ (pt1e-pt1t).dot(n1t);
    T weight = T(error_weight) * (T(2.)/(v4r::sqr(pt1e[2])+v4r::sqr(pt1t[2])));
    Eigen::Matrix<T,3,1> diff;
    diff = pt1e-pt1t;
    residuals[0] = weight * diff[0]*n1t[0];
    residuals[1] = weight * diff[1]*n1t[1];
    residuals[2] = weight * diff[2]*n1t[2];

    return true;
  }

  const Eigen::Vector3d pt0;
  const Eigen::Vector3d n0;
  const Eigen::Vector3d pt1;
  const double error_weight;
};

/**
 * This functor uses a point to plane error model
 */
struct PointToPlaneErrorGlobalPoseCamViewDataOptiPt1 {
  PointToPlaneErrorGlobalPoseCamViewDataOptiPt1(const Eigen::Vector3d &_pt0, const Eigen::Vector3d &_n0, const double &_w)
      : pt0(_pt0), n0(_n0), error_weight(_w) {}

  template <typename T>
  bool operator()(const T* const Rt0,   // Rotation denoted by angle axis followed with translation (...to keyframe)
                  const T* const Rt1,   // ..to proj. frame
                  const T* const X1,
                  T* residuals) const {

    Eigen::Matrix<T,3,1> pt0e, n0e, pt0g, n0g, pt1t, n1t, pt1e;

    pt0e[0] = T(pt0[0]);
    pt0e[1] = T(pt0[1]);
    pt0e[2] = T(pt0[2]);
    n0e[0] = T(n0[0]);
    n0e[1] = T(n0[1]);
    n0e[2] = T(n0[2]);
    pt1e[0] = X1[0];
    pt1e[1] = X1[1];
    pt1e[2] = X1[2];

    // transform point to global coordinates
    T invRt0[6];
    v4r::invPose6(Rt0, &Rt0[3], invRt0, &invRt0[3]);
    ceres::AngleAxisRotatePoint(invRt0, &n0e[0], &n0g[0]);
    ceres::AngleAxisRotatePoint(invRt0, &pt0e[0], &pt0g[0]);
    pt0g[0] += invRt0[3];
    pt0g[1] += invRt0[4];
    pt0g[2] += invRt0[5];

    // transform to view 1: x = RX + t.
    ceres::AngleAxisRotatePoint(Rt1, &n0g[0], &n1t[0]);
    ceres::AngleAxisRotatePoint(Rt1, &pt0g[0], &pt1t[0]);
    pt1t[0] += Rt1[3];
    pt1t[1] += Rt1[4];
    pt1t[2] += Rt1[5];

    // compute the point to plane distance
    //residuals[0] = T(error_weight) * /*(T(2.)/(pt1e[2]+pt1t[2])) **/ (pt1e-pt1t).dot(n1t);
    T weight = T(error_weight) * (T(2.)/(v4r::sqr(pt1e[2])+v4r::sqr(pt1t[2])));
    Eigen::Matrix<T,3,1> diff;
    diff = pt1e-pt1t;
    residuals[0] = weight * diff[0]*n1t[0];
    residuals[1] = weight * diff[1]*n1t[1];
    residuals[2] = weight * diff[2]*n1t[2];

    return true;
  }

  const Eigen::Vector3d pt0;
  const Eigen::Vector3d n0;
  const double error_weight;
};

/**
 * This functor uses a point to plane error model
 */
struct PointToPlaneErrorGlobalPoseDeltaPoseCamViewDataOptiPt1 {
  PointToPlaneErrorGlobalPoseDeltaPoseCamViewDataOptiPt1(const Eigen::Vector3d &_pt0, const Eigen::Vector3d &_n0, const double &_w)
      : pt0(_pt0), n0(_n0), error_weight(_w) {}

  template <typename T>
  bool operator()(const T* const Rtd,   // ..to rgb. frame
                  const T* const X1,
                  T* residuals) const {

    Eigen::Matrix<T,3,1> pt0e, n0e, pt1t, n1t, pt1e;

    pt0e[0] = T(pt0[0]);
    pt0e[1] = T(pt0[1]);
    pt0e[2] = T(pt0[2]);
    n0e[0] = T(n0[0]);
    n0e[1] = T(n0[1]);
    n0e[2] = T(n0[2]);
    pt1e[0] = X1[0];
    pt1e[1] = X1[1];
    pt1e[2] = X1[2];

    // transform to view 1: x = RX + t.
    ceres::AngleAxisRotatePoint(Rtd, &n0e[0], &n1t[0]);
    ceres::AngleAxisRotatePoint(Rtd, &pt0e[0], &pt1t[0]);
    pt1t[0] += Rtd[3];
    pt1t[1] += Rtd[4];
    pt1t[2] += Rtd[5];

    // compute the point to plane distance
    //residuals[0] = T(error_weight) * /*(T(2.)/(pt1e[2]+pt1t[2])) **/ (pt1e-pt1t).dot(n1t);
    T weight = T(error_weight) * (T(2.)/(v4r::sqr(pt1e[2])+v4r::sqr(pt1t[2])));
    Eigen::Matrix<T,3,1> diff;
    diff = pt1e-pt1t;
    residuals[0] = weight * diff[0]*n1t[0];
    residuals[1] = weight * diff[1]*n1t[1];
    residuals[2] = weight * diff[2]*n1t[2];

    return true;
  }

  const Eigen::Vector3d pt0;
  const Eigen::Vector3d n0;
  const double error_weight;
};

}

#endif


// Copyright 2005 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#include <boost/graph/page_rank.hpp>
#include <boost/python.hpp>

namespace boost { namespace graph { namespace python {

using boost::python::object;

struct python_done
{
  explicit python_done(object callable) : callable(callable) { }

  template<typename RankMap, typename Graph>
  bool operator()(RankMap rank, const Graph& g)
  {
    using boost::python::extract;
    return extract<bool>(callable(rank, ref(g)));
  }

private:
  object callable;
};

template<typename Graph>
void
page_rank
  (const Graph& g,
   vector_property_map<
     float,
     typename property_map<Graph, vertex_index_t>::const_type>& rank,
   object done_or_iterations,
   float damping)
{
  using boost::python::extract;

  int iterations = 20;
  if (done_or_iterations != object()) {
    extract<int> iters(done_or_iterations);
    if (iters.check())
      iterations = iters;
    else {
      boost::graph::page_rank(g, rank, python_done(done_or_iterations),
                              damping);
      return;
    }
  }

  boost::graph::page_rank(g, rank, boost::graph::n_iterations(iterations),
                          damping);
}

} } } // end namespace boost::graph::python


//
// Created by bear on 6/27/20.
//
#include "util.h"

#include <iostream>

#include <boost/units/quantity.hpp>
#include <boost/units/io.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/prefixes.hpp>
#include <boost/units/systems/cgs.hpp>

namespace zuut {
    template<class T1, class T2>
    static decltype(auto)
    work(const T1 &F, const T2 &dx) { return F * dx; }

    TEST(BoostTest, units) {
        using namespace boost::units;
        using namespace boost::units::si;
        auto F(newton);

        // Create our own constants for the mks system
        const auto centimeter = 1.0 * centi * meter; // not used
        const auto centimeters = 1.0 * centi * meters; // not used
        const auto nanometers = 1.0 * nano * meter;

        std::cout << "Using the si (mks meter/kilogram/second) system " << std::endl;
        auto dist1(1.5 * meters);
        auto dist2 = 2.0 * nanometers;
        auto dist3 = 150 * nano * meters;
        auto dist4 = 0.002 * nanometers;
        auto dist5 = dist2 + dist3 + dist4;
        // ERROR can't do a : auto dist6 = dist1 + dist5; w/o a type conversion

#define show(x)  /*clazzname(x) << " " << */ #x << " = " << x
        std::cout << show(dist1) << std::endl;
        std::cout << show(dist2) << std::endl;
        std::cout << show(dist3) << std::endl;
        std::cout << show(dist4) << std::endl;
        std::cout << show(dist5) << std::endl;

        // cgs (centimeter-gram-second system)
        std::cout << "Using the cgs (centimeter/gram/second) system " << std::endl;
        quantity<cgs::length> cm_dist1 = static_cast<quantity<cgs::length>>(dist1);
        auto cm_dist2 = 2.0 * cgs::centimeter;
        auto cm_dist3 = 150 * cgs::centimeter;
        auto cm_dist4(3 * cgs::centimeter);
        std::cout << show(cm_dist1) << std::endl;
        std::cout << show(cm_dist2) << std::endl;
        std::cout << show(cm_dist3) << std::endl;
        std::cout << show(cm_dist4) << std::endl;

        std::cout << "Other units system " << std::endl;
        auto E(work(F, dist1));
        std::cout << show(E) << std::endl;
#undef show
    }
}


// Copyright 2018 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/core/lightweight_test.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <boost/histogram/axis.hpp>
#include <boost/histogram/axis/traits.hpp>
#include "std_ostream.hpp"
#include "throw_exception.hpp"
#include "utility_axis.hpp"

using namespace boost::histogram::axis;

int main() {
  // value_type
  {
    BOOST_TEST_TRAIT_SAME(traits::value_type<integer<int>>, int);
    BOOST_TEST_TRAIT_SAME(traits::value_type<category<int>>, int);
    BOOST_TEST_TRAIT_SAME(traits::value_type<regular<double>>, double);
  }

  // is_continuous
  {
    BOOST_TEST_TRAIT_TRUE((traits::is_continuous<regular<>>));
    BOOST_TEST_TRAIT_FALSE((traits::is_continuous<integer<int>>));
    BOOST_TEST_TRAIT_FALSE((traits::is_continuous<category<int>>));
    BOOST_TEST_TRAIT_TRUE((traits::is_continuous<integer<double>>));
  }

  // is_reducible
  {
    struct not_reducible {};
    struct reducible {
      reducible(const reducible&, index_type, index_type, unsigned);
    };

    BOOST_TEST_TRAIT_TRUE((traits::is_reducible<reducible>));
    BOOST_TEST_TRAIT_FALSE((traits::is_reducible<not_reducible>));

    BOOST_TEST_TRAIT_TRUE((traits::is_reducible<regular<>>));
    BOOST_TEST_TRAIT_TRUE((traits::is_reducible<variable<>>));
    BOOST_TEST_TRAIT_TRUE((traits::is_reducible<circular<>>));
    BOOST_TEST_TRAIT_TRUE((traits::is_reducible<integer<>>));
    BOOST_TEST_TRAIT_FALSE((traits::is_reducible<category<>>));
  }

  // static_is_inclusive
  {
    struct empty {};
    struct with_opts_not_inclusive {
      static constexpr unsigned options() { return option::underflow | option::overflow; }
      static constexpr bool inclusive() { return false; }
    };

    BOOST_TEST_TRAIT_FALSE((traits::static_is_inclusive<empty>));
    BOOST_TEST_TRAIT_FALSE((traits::static_is_inclusive<with_opts_not_inclusive>));

    BOOST_TEST_TRAIT_TRUE((traits::static_is_inclusive<regular<>>));
    BOOST_TEST_TRAIT_FALSE(
        (traits::static_is_inclusive<
            regular<double, boost::use_default, boost::use_default, option::growth_t>>));
    BOOST_TEST_TRAIT_FALSE(
        (traits::static_is_inclusive<regular<double, boost::use_default,
                                             boost::use_default, option::circular_t>>));

    BOOST_TEST_TRAIT_TRUE((traits::static_is_inclusive<variable<>>));
    BOOST_TEST_TRAIT_FALSE((traits::static_is_inclusive<
                            variable<double, boost::use_default, option::growth_t>>));
    BOOST_TEST_TRAIT_FALSE((traits::static_is_inclusive<
                            variable<double, boost::use_default, option::circular_t>>));

    BOOST_TEST_TRAIT_TRUE((traits::static_is_inclusive<integer<int>>));
    BOOST_TEST_TRAIT_TRUE((
        traits::static_is_inclusive<integer<int, boost::use_default, option::growth_t>>));
    BOOST_TEST_TRAIT_TRUE((traits::static_is_inclusive<
                           integer<int, boost::use_default, option::circular_t>>));

    BOOST_TEST_TRAIT_TRUE((traits::static_is_inclusive<integer<double>>));
    BOOST_TEST_TRAIT_FALSE((traits::static_is_inclusive<
                            integer<double, boost::use_default, option::growth_t>>));
    BOOST_TEST_TRAIT_FALSE((traits::static_is_inclusive<
                            integer<double, boost::use_default, option::circular_t>>));

    BOOST_TEST_TRAIT_TRUE((traits::static_is_inclusive<category<int>>));
    BOOST_TEST_TRAIT_TRUE((traits::static_is_inclusive<
                           category<int, boost::use_default, option::growth_t>>));
    BOOST_TEST_TRAIT_FALSE(
        (traits::static_is_inclusive<category<int, boost::use_default, option::none_t>>));
  }

  // index, rank, value, width
  {
    auto a = integer<>(1, 3);
    BOOST_TEST_EQ(traits::index(a, 1), 0);
    BOOST_TEST_EQ(traits::rank(a), 1);
    BOOST_TEST_EQ(traits::value(a, 0), 1);
    BOOST_TEST_EQ(traits::width(a, 0), 0);
    BOOST_TEST_EQ(traits::width(a, 0), 0);

    auto b = integer<double>(1, 3);
    BOOST_TEST_EQ(traits::index(b, 1), 0);
    BOOST_TEST_EQ(traits::rank(b), 1);
    BOOST_TEST_EQ(traits::value(b, 0), 1);
    BOOST_TEST_EQ(traits::width(b, 0), 1);
    BOOST_TEST(traits::static_options<decltype(b)>::test(option::underflow));

    auto c = category<std::string>{"red", "blue"};
    BOOST_TEST_EQ(traits::index(c, "blue"), 1);
    BOOST_TEST_EQ(traits::rank(c), 1);
    BOOST_TEST_EQ(traits::value(c, 0), std::string("red"));
    BOOST_TEST_EQ(traits::width(c, 0), 0);

    struct D {
      index_type index(const std::tuple<int, double>& args) const {
        return static_cast<index_type>(std::get<0>(args) + std::get<1>(args));
      }
      index_type size() const { return 5u; }
    } d;
    BOOST_TEST_EQ(traits::index(d, std::make_tuple(1, 2.0)), 3.0);
    BOOST_TEST_EQ(traits::rank(d), 2u);

    variant<D, integer<>> v;
    v = a;
    BOOST_TEST_EQ(traits::rank(v), 1u);
    v = d;
    BOOST_TEST_EQ(traits::rank(v), 2u);
  }

  // static_options, options()
  {
    using A = integer<>;
    BOOST_TEST_EQ(traits::static_options<A>::test(option::growth), false);
    auto expected = option::underflow | option::overflow;
    auto a = A{};
    BOOST_TEST_EQ(traits::options(a), expected);
    BOOST_TEST_EQ(traits::options(static_cast<A&>(a)), expected);
    BOOST_TEST_EQ(traits::options(static_cast<const A&>(a)), expected);
    BOOST_TEST_EQ(traits::options(std::move(a)), expected);

    using B = integer<int, null_type, option::growth_t>;
    BOOST_TEST_EQ(traits::static_options<B>::test(option::growth), true);
    BOOST_TEST_EQ(traits::options(B{}), option::growth);

    struct growing {
      auto update(double) { return std::make_pair(0, 0); }
    };
    using C = growing;
    BOOST_TEST_EQ(traits::static_options<C>::test(option::growth), true);
    auto c = C{};
    BOOST_TEST_EQ(traits::options(c), option::growth);
    BOOST_TEST_EQ(traits::options(static_cast<C&>(c)), option::growth);
    BOOST_TEST_EQ(traits::options(static_cast<const C&>(c)), option::growth);
    BOOST_TEST_EQ(traits::options(std::move(c)), option::growth);

    struct notgrowing {
      auto index(double) { return 0; }
    };
    using D = notgrowing;
    BOOST_TEST_EQ(traits::static_options<D>::test(option::growth), false);
    auto d = D{};
    BOOST_TEST_EQ(traits::options(d), option::none);
    BOOST_TEST_EQ(traits::options(static_cast<D&>(d)), option::none);
    BOOST_TEST_EQ(traits::options(static_cast<const D&>(d)), option::none);
    BOOST_TEST_EQ(traits::options(std::move(d)), option::none);
  }

  // update
  {
    auto a = integer<int, null_type, option::growth_t>();
    BOOST_TEST_EQ(traits::update(a, 0), (std::pair<index_type, index_type>(0, -1)));
    BOOST_TEST_THROWS(traits::update(a, "foo"), std::invalid_argument);

    variant<integer<int, null_type, option::growth_t>, integer<>> v(a);
    BOOST_TEST_EQ(traits::update(v, 0), (std::pair<index_type, index_type>(0, 0)));
  }

  // metadata
  {
    struct None {};

    struct Const {
      const int& metadata() const { return m; };
      int m = 0;
    };

    struct Both {
      const int& metadata() const { return m; };
      int& metadata() { return m; };
      int m = 0;
    };

    None none;
    BOOST_TEST_TRAIT_SAME(decltype(traits::metadata(none)), null_type&);
    BOOST_TEST_TRAIT_SAME(decltype(traits::metadata(static_cast<None&>(none))),
                          null_type&);
    BOOST_TEST_TRAIT_SAME(decltype(traits::metadata(static_cast<const None&>(none))),
                          const null_type&);

    Const c;
    BOOST_TEST_EQ(traits::metadata(c), 0);
    BOOST_TEST_EQ(traits::metadata(static_cast<Const&>(c)), 0);
    BOOST_TEST_EQ(traits::metadata(static_cast<const Const&>(c)), 0);

    Both b;
    BOOST_TEST_EQ(traits::metadata(b), 0);
    BOOST_TEST_EQ(traits::metadata(static_cast<Both&>(b)), 0);
    BOOST_TEST_EQ(traits::metadata(static_cast<const Both&>(b)), 0);
  }

  return boost::report_errors();
}


#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
#include <boost/filesystem.hpp>
#include <iostream>
#include <perception_utils/vfh/vfh_pose_estimator.h>

/** \brief Returns closest poses of of objects in training data to the query object
    \param -q the path to the input point cloud
    \param -k the number of nearest neighbors to return
*/
int main (int argc, char **argv)
{
    //parse data directory
    std::string queryCloudName;
    pcl::console::parse_argument (argc, argv, "-q", queryCloudName);
    boost::filesystem::path queryCloudPath(queryCloudName);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PCDReader reader;
    if(reader.read(queryCloudPath.native(), *cloud) == -1)
        return -1;
    VFHPoseEstimator poseEstimator;

    float roll, pitch, yaw;
    poseEstimator.getPose(cloud, roll, pitch, yaw, true);
    std::cout << roll << " " << pitch << " " << yaw << std::endl;
    return 0;
}


/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
 ------ Copyright (C) 2010 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/

/*
 ------------------ Author: Chris Laurel  -------------------------------------------------
 ------------------ E-mail: (claurel@gmail.com) ----------------------------

 Based on code by Chris Laurel from Celestia (http://www.shatters.net/celestia)
 */

#include <Eigen/Core>
//#include "Eigen/src/Core/Map.h"
#include <QtCore>
#include <QDataStream>
#include <QIODevice>
#include "date.h"
#include "jplephemeris.h"

using namespace Eigen;
using namespace std;

typedef Eigen::Matrix< double, 3, 3 > 	MyMatrix3d;
typedef Eigen::Matrix< double, 3, 1 > 	MyVector3d;

static const unsigned int DE200RecordSize    =  826;
static const unsigned int DE405RecordSize    = 1018;
static const unsigned int DE406RecordSize    =  728;

static const unsigned int NConstants         =  400;
static const unsigned int ConstantNameLength =  6;

static const unsigned int MaxChebyshevCoeffs = 32;

static const int LabelSize = 84;

sta::JPLEphemeris::JPLEphemeris()
{
}

sta::JPLEphemeris::~JPLEphemeris()
{
}

unsigned int
sta::JPLEphemeris::getDENumber() const
{
    return DENum;
}

double
sta::JPLEphemeris::getStartDate() const
{
    return startDate;
}

double
sta::JPLEphemeris::getEndDate() const
{
    return endDate;
}

sta::Ephemeris::TimeInterval
sta::JPLEphemeris::validTimeInterval(const StaBody* /* body */) const
{
    return TimeInterval(sta::JdToMjd(startDate), sta::JdToMjd(endDate));
}

// Internal method to map an STA body id to a JPL ephemeris item
sta::JPLEphemItem
sta::JPLEphemeris::mapStaId(StaBodyId id) const
{
    switch (id)
    {
    case STA_SOLAR_SYSTEM_BARYCENTER:
        return JPLEph_SSB;
    case STA_EARTH_BARYCENTER:
        return JPLEph_EarthMoonBary;
    case STA_MERCURY:
        return JPLEph_Mercury;
    case STA_VENUS:
        return JPLEph_Venus;
    case STA_EARTH:
        return JPLEph_Earth;
    case STA_MARS:
        return JPLEph_Mars;
    case STA_JUPITER:
        return JPLEph_Jupiter;
    case STA_SATURN:
        return JPLEph_Saturn;
    case STA_URANUS:
        return JPLEph_Uranus;
    case STA_NEPTUNE:
        return JPLEph_Neptune;
    case STA_PLUTO:
        return JPLEph_Pluto;
    case STA_SUN:
        return JPLEph_Sun;
    case STA_MOON:
        return JPLEph_Moon;
    default:
        return JPLEph_Invalid;
    }
}

const QList<StaBodyId>&
sta::JPLEphemeris::bodyList() const
{
    return m_bodies;
}

const StaBody*
sta::JPLEphemeris::parentBody(const StaBody* body) const
{
    JPLEphemItem id = mapStaId(body->id());
    if (id == JPLEph_Invalid)
    {
        return NULL;
    }
    else
    {
        if (id == JPLEph_SSB)
            return NULL;
        else if (id == JPLEph_Moon || id == JPLEph_Earth)
            return STA_SOLAR_SYSTEM->lookup(STA_EARTH_BARYCENTER);
        else
            return STA_SOLAR_SYSTEM->lookup(STA_SOLAR_SYSTEM_BARYCENTER);
    }
}

const sta::CoordinateSystemType
sta::JPLEphemeris::coordinateSystem(const StaBody* /* body */) const
{
    return sta::COORDSYS_EME_J2000;
}

sta::StateVector
sta::JPLEphemeris::stateVector(const StaBody* body,
                               double mjd,
                               const StaBody* center,
                               sta::CoordinateSystemType coordSys) const
{
    JPLEphemItem id = mapStaId(body->id());
    if (id == JPLEph_Invalid || body == center)
    {
        return StateVector::zero();
    }

    StateVector state = getPlanetStateVector(id, MjdToJd(mjd));
    const StaBody* parent = parentBody(body);

    // If the parent object in the ephemeris is the same as the center, then no
    // extra work is necessary. Otherwise, we also need compute the position of
    // the parent with respect to the center.
    if (parent == center)
    {
        return state;
    }
    else
    {
        const StaBody* ssb = STA_SOLAR_SYSTEM->lookup(STA_SOLAR_SYSTEM_BARYCENTER);
        StateVector parentState = parent->stateVector(mjd, ssb, coordSys);
        StateVector centerState = center->stateVector(mjd, ssb, coordSys);

        return StateVector(parentState.position - centerState.position + state.position,
                           parentState.velocity - centerState.velocity + state.velocity);
    }
}

// Return the position of an object relative to the solar system barycenter
// or the Earth (in the case of the Moon) at a specified TDB Julian date tjd.
// If tjd is outside the span covered by the ephemeris it is clamped to a
// valid time.
sta::StateVector
sta::JPLEphemeris::getPlanetStateVector(JPLEphemItem planet, double tjd) const
{
    // Solar system barycenter is the origin
    if (planet == JPLEph_SSB)
    {
        return StateVector(MyVector3d(0.0, 0.0, 0.0), MyVector3d(0.0, 0.0, 0.0));
    }

    // The position of the Earth must be computed from the positions of the
    // Earth-Moon barycenter and Moon. This is handled automatically because
    // Earth's 'parent' in the ephemeris is the Earth-Moon barycenter.
    if (planet == JPLEph_Earth)
    {
        // Get the geocentric position of the Moon
        sta::StateVector moonState = getPlanetStateVector(JPLEph_Moon, tjd);

        double f = 1.0 / (earthMoonMassRatio + 1.0);
        return StateVector(-moonState.position * f, -moonState.velocity * f);
    }

    // Clamp time to [ startDate, endDate ]
    if (tjd < startDate)
    {
        tjd = startDate;
    }
    else if (tjd > endDate)
    {
        tjd = endDate;
    }

    // recordIndex is always >= 0:
    unsigned int recordIndex = (unsigned int) ((tjd - startDate) / daysPerInterval);
    // Make sure we don't go past the end of the array if t == endDate
    if (recordIndex >= records.size())
    {
        recordIndex = records.size() - 1;
    }
    const JPLEphRecord& record = records[recordIndex];

    Q_ASSERT(coeffInfo[planet].nGranules >= 1);
    Q_ASSERT(coeffInfo[planet].nGranules <= 32);
    Q_ASSERT(coeffInfo[planet].nCoeffs <= MaxChebyshevCoeffs);

    // u is the normalized time (in [-1, 1]) for interpolating
    // coeffs is a pointer to the Chebyshev coefficients
    double u = 0.0;
    const double* coeffs = NULL;
    unsigned int nCoeffs = coeffInfo[planet].nCoeffs;
    double velocityScale = 1.0;

    if (coeffInfo[planet].nGranules == 0xffffffff)
    {
        coeffs = record.coeffs + coeffInfo[planet].offset;
        u = 2.0 * (tjd - record.t0) / daysPerInterval - 1.0;
        velocityScale = 2.0 / daysPerInterval;
    }
    else
    {
        // This interval is subdivided into shorter subintervals (called granules)
        // Adjust the coefficient pointer accordingly.
        double daysPerGranule = daysPerInterval / coeffInfo[planet].nGranules;
        int granule = (int) ((tjd - record.t0) / daysPerGranule);
        double granuleStartDate = record.t0 + daysPerGranule * (double) granule;
        coeffs = record.coeffs + coeffInfo[planet].offset +
                 granule * coeffInfo[planet].nCoeffs * 3;
        u = 2.0 * (tjd - granuleStartDate) / daysPerGranule - 1.0;
        velocityScale = 2.0 / daysPerGranule;
    }

    // Calculate the Chebyshev polynomial terms and their derivatives
    //Matrix<double, MaxChebyshevCoeffs, 1> positionTerms;
    double positionTerms[MaxChebyshevCoeffs];
    positionTerms[0] = 1.0;
    positionTerms[1] = u;

    //Matrix<double, MaxChebyshevCoeffs, 1> velocityTerms;
    double velocityTerms[MaxChebyshevCoeffs];
    velocityTerms[0] = 0.0;
    velocityTerms[1] = 1.0;
    for (unsigned int i = 2; i < nCoeffs; i++)
    {
        positionTerms[i] = 2.0 * u * positionTerms[i - 1] - positionTerms[i - 2];
        velocityTerms[i] = 2.0 * u * velocityTerms[i - 1] - velocityTerms[i - 2] + 2.0 * positionTerms[i - 1];
    }

    // Note that Eigen uses column-major storage, so we need to transpose the coefficient
    // matrix.
    MyVector3d position = Map<MatrixXd>(coeffs, nCoeffs, 3).transpose() * Map<MatrixXd>(positionTerms, nCoeffs, 1);
    MyVector3d velocity = Map<MatrixXd>(coeffs, nCoeffs, 3).transpose() * Map<MatrixXd>(velocityTerms, nCoeffs, 1);

    return StateVector(position, velocity * (velocityScale / 86400.0));
}

sta::JPLEphemeris*
sta::JPLEphemeris::load(QIODevice* device)
{
    QDataStream in(device);
    in.setByteOrder(QDataStream::BigEndian);

    JPLEphemeris* eph = NULL;

    // Skip past three header labels
    in.skipRawData(LabelSize * 3);
    if (in.status() != QDataStream::Ok)
        return NULL;

    // Skip past the constant names
    in.skipRawData(NConstants * ConstantNameLength);
    if (in.status() != QDataStream::Ok)
        return NULL;

    eph = new JPLEphemeris();
    if (eph == NULL)
        return NULL;

    // Read the start time, end time, and time interval
    in >> eph->startDate;
    in >> eph->endDate;
    in >> eph->daysPerInterval;
    if (in.status() != QDataStream::Ok)
    {
        delete eph;
        return NULL;
    }

    // Number of constants with valid values; not useful for us
    quint32 nConstants;
    in >> nConstants;

    in >> eph->au;     // kilometers per astronomical unit
    in >> eph->earthMoonMassRatio;

    // Read the coefficient information for each item in the ephemeris
    for (unsigned int i = 0; i < JPLEph_NItems; i++)
    {
        in >> eph->coeffInfo[i].offset;
        in >> eph->coeffInfo[i].nCoeffs;
        in >> eph->coeffInfo[i].nGranules;

        eph->coeffInfo[i].offset -= 3;
    }

    if (in.status() != QDataStream::Ok)
    {
        delete eph;
        return NULL;
    }

    in >> eph->DENum;

    switch (eph->DENum)
    {
    case 200:
        eph->recordSize = DE200RecordSize;
        break;
    case 405:
        eph->recordSize = DE405RecordSize;
        break;
    case 406:
        eph->recordSize = DE406RecordSize;
        break;
    default:
        delete eph;
        return NULL;
    }

    in >> eph->librationCoeffInfo.offset;
    in >> eph->librationCoeffInfo.nCoeffs;
    in >> eph->librationCoeffInfo.nGranules;
    if (in.status() != QDataStream::Ok)
    {
        delete eph;
        return NULL;
    }

    // Skip past the rest of the record
    in.skipRawData(eph->recordSize * 8 - 2856);
    // The next record contains constant values (which we don't need)
    in.skipRawData(eph->recordSize * 8);
    if (in.status() != QDataStream::Ok)
    {
        delete eph;
        return NULL;
    }

    unsigned int nRecords = (unsigned int) ((eph->endDate - eph->startDate) / eph->daysPerInterval);
    eph->records.resize(nRecords);

    for (unsigned int i = 0; i < nRecords; i++)
    {
        in >> eph->records[i].t0;
        in >> eph->records[i].t1;

    	// Allocate coefficient array for this record; the first two
    	// 'coefficients' are actually the start and end time (t0 and t1)
        eph->records[i].coeffs = new double[eph->recordSize - 2];
        for (unsigned int j = 0; j < eph->recordSize - 2; j++)
        {
            in >> eph->records[i].coeffs[j];
        }

    	// Make sure that we read this record successfully
        if (in.status() != QDataStream::Ok)
    	{
    	    delete eph;
    	    return NULL;
    	}
    }

    eph->m_bodies << STA_SUN << STA_MERCURY << STA_VENUS << STA_EARTH << STA_MARS
                  << STA_JUPITER << STA_SATURN << STA_URANUS << STA_NEPTUNE << STA_PLUTO
                  << STA_EARTH_BARYCENTER << STA_MOON;

    return eph;
}


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <iostream>                  // for std::cout
#include <utility>                   // for std::pair
#include <algorithm>                 // for std::for_each
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
#include <stdint.h>
#include "c_t_2.h"
#include "tester.h"
#include <map>
#include <sys/time.h>

typedef boost::adjacency_list<> Graph;
typedef boost::erdos_renyi_iterator<boost::minstd_rand, Graph> ERGen;
typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
typedef boost::property_map<Graph, boost::vertex_index_t>::type IndexMap;
typedef boost::graph_traits<Graph> GraphTraits;
typedef boost::graph_traits<Graph>::vertex_iterator vertex_iter;

uint32_t size_walk = 20000;
uint32_t *id_array = (uint32_t *)malloc(sizeof(uint32_t)*size_walk);
uint32_t node_count = 0;
long long n_count = 100000;
Vertex vt;

float random_walk(Vertex vt, IndexMap index, Graph g) {
    struct timeval tval_before, tval_after, tval_result;
    gettimeofday(&tval_before, NULL);
    for (int i=0;i<size_walk;i++) {
        boost::graph_traits<Graph>::out_edge_iterator out_i, out_end;
        GraphTraits::edge_descriptor e;
        int e_degree = out_degree(vt, g);
        while (e_degree == 0) {
            int r = random_int(0, n_count-1);
            std::pair<vertex_iter, vertex_iter> vp;
            vp = vertices(g);
            vp.first = vp.first + r;
            vt = *vp.first;
            e_degree = out_degree(vt, g);
        }
        int random_edge = random_int(0, e_degree-1);
        boost::tie(out_i, out_end) = out_edges(vt, g);
        out_i = out_i + random_edge;
        e = *out_i;
        vt = target(e, g);
    }
    gettimeofday(&tval_after, NULL);
    timersub(&tval_after, &tval_before, &tval_result);
    //long int num_seconds = tval_result.tv_sec + tv_result.tv_usec;
    float time_elapsed = ((long int)tval_result.tv_usec + (long int)tval_result.tv_sec/1000000)/1000000.0;
    return time_elapsed;
}
float random_walk_c(val_t vt, cow_trie_p g) {
    struct timeval tval_before, tval_after, tval_result;
    gettimeofday(&tval_before, NULL);
    for (int i=0;i<size_walk;i++) {
        uint32_t e_degree = vt._.node.succ_ct;
        while (e_degree == 0) {
            int random_vertex = random_int(0, n_count-1);
            cow_trie_lookup(g, random_vertex, &vt);
            e_degree = vt._.node.succ_ct;
        }
        uint32_t random_edge = random_int(0, e_degree-1);
        uint32_t key = edge_list_values(vt._.node.succs)[random_edge];
        cow_trie_lookup(g, key, &vt);
        cow_trie_lookup(g, vt._.edge.succ, &vt);
    }
    gettimeofday(&tval_after, NULL);
    timersub(&tval_after, &tval_before, &tval_result);
    //long int num_seconds = tval_result.tv_sec + tv_result.tv_usec;
    float time_elapsed = ((long int)tval_result.tv_usec + (long int)tval_result.tv_sec/1000000)/1000000.0;
    return time_elapsed;
}

size_t traverse(cow_trie_p map) {
    size_t count = 0;
    count += map->size;
    for (int i=0;i<count_one_bits(map->child_bitmap);i++) {
        count += traverse(cow_trie_children(map)[i]);
    }
    return count;
}

int main() {
    GraphTraits::out_edge_iterator out_i, out_end;
    GraphTraits::edge_descriptor e;
    boost::graph_traits<Graph>::adjacency_iterator ai;
    boost::graph_traits<Graph>::adjacency_iterator ai_end;
    boost::minstd_rand gen;

    std::map<uint64_t, uint32_t> m;
    std::map<int_type, int_type> id_map;

    size_t count = 0;

    uint64_t *v_array = (uint64_t *)malloc(sizeof(uint64_t)*(n_count + n_count*3));
    // Create graph with 100 nodes and edges with probability 0.05
    double prob = n_count*n_count;
    prob = n_count*3/prob;
    Graph g(ERGen(gen, n_count, prob), ERGen(), n_count);

    std::cout << "done generating\n\n";

    IndexMap index = get(boost::vertex_index, g);
    cow_trie_p my_map = cow_trie_alloc(0, 0);
    my_map->ref_count = 1;
    my_map->child_bitmap = 0;
    my_map->value_bitmap = 0;
    uint32_t id = 0;

    std::cout << "beginning vertex iteration\n\n";
    std::pair<vertex_iter, vertex_iter> vp;
    for (vp = vertices(g); vp.first != vp.second; ++vp.first) {
        Vertex v = *vp.first;
        boost::tie(out_i, out_end) = out_edges(v, g);
        uint32_t deg = out_degree(v, g);
        uint32_t *succs = new uint32_t[deg];
        val_t my_val = cow_trie_create_val(0, deg, succs, 0, 0);
        int_type my_id = cow_trie_insert(my_map, my_val, &my_map);
        id_map[id] = my_id;
        ++id;
    }

    std::cout << "vertex iteration done, beginning edge iteration\n\n";
    boost::graph_traits<Graph>::edge_iterator ei, ei_end;
    boost::tie(ei, ei_end) = edges(g);
    std::cout << "id before is" << id << "\n\n";
    for (ei; ei != ei_end; ++ei) {
        val_t my_val = cow_trie_create_val(1, 0, 0, id_map[index[source(*ei, g)]], id_map[index[target(*ei, g)]]);
        int_type my_id = cow_trie_insert(my_map, my_val, &my_map);
        m[index[source(*ei, g)] << 32 | index[target(*ei, g)]] = id;
        ++id;
    }
    std::cout << "id is\n\n" << id << "\n\n";
    std::cout << "done edge iterating\n\n";
    std::cout << "putting in successors\n\n";
    int p = 0;
    for (vp = vertices(g); vp.first != vp.second; ++vp.first) {
        Vertex v = *vp.first;
        val_t my_val;
        cow_trie_lookup(my_map, p, &my_val);
        uint32_t *succs = edge_list_values(my_val._.node.succs);
        boost::tie(out_i, out_end) = out_edges(v, g);
        int eindex = 0;
        for (out_i; out_i != out_end; ++out_i) {
            e = *out_i;
            Vertex src = source(e, g), targ = target(e, g);
            uint32_t id = m[index[src] << 32 | index[targ]];
            succs[eindex] = id_map[id];
            ++eindex;
        }
    ++p;
    }
    val_t v;
    cow_trie_lookup(my_map, 0, &v);
    Vertex vtest = *vertices(g).first;
    std::cout << "beginning random traversal\n\n";
    float t1 = random_walk(vtest, index, g);
    printf("time for theirs %f", t1);
    float t2 = random_walk_c(v, my_map);
    printf("time for ours %f", t2);
    std::cout << "end random traversal\n\n";
}


/**
 * Copyright 2004-present Facebook. All Rights Reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include "source/calibration/FeatureMatcher.h"

#include <future>
#include <queue>

#include <boost/timer/timer.hpp>

#include <folly/Format.h>

#include "source/calibration/FeatureDetector.h"

DEFINE_bool(custom_zncc, false, "uses custom ZNCC formula for patch matching");
DEFINE_double(depth_max, 100.0, "max depth in m");
DEFINE_double(depth_min, 1.0, "min depth in m");
DEFINE_double(depth_samples, 1000, "number of depths to sample");
DEFINE_double(max_depth_for_remap, 50, "max depth to reproject features");
DEFINE_double(overlap_threshold, 0, "minimum overlap between matched images");
DEFINE_double(reprojected_corner_drift_tolerance, 0.5, "in pixels");
DEFINE_double(
    search_overlap,
    0.25,
    "overlap fraction between search windows at different disparities");
DEFINE_int32(search_radius, 100, "search radius in pixels");
DEFINE_double(
    zncc_delta_threshold,
    0.05,
    "minimum zncc score difference betwen best and second best potential matches for a corner");

using PixelType = uint8_t;
using Image = cv::Mat_<PixelType>;
using ImageId = std::string;

namespace fb360_dep {
namespace calibration {

struct BestMatch {
  int bestIdx;
  double bestScore;
  int secondBestIdx;
  double secondBestScore;

  BestMatch() {
    // Initialize scores to -1. zncc scores are between -1 and 1, higher is better
    // Initialize index to -1 indicating there is no match.
    bestIdx = -1;
    bestScore = -1;
    secondBestIdx = -1;
    secondBestScore = -1;
  }

  void updateCornerScore(const double newScore, const int newIdx) {
    if (newScore > bestScore) {
      // newScore is a better score for this corner
      if (bestIdx == newIdx) {
        // If this is the same index as the previous best don't update the second best
        bestScore = newScore;
      } else {
        secondBestIdx = bestIdx;
        secondBestScore = bestScore;
        bestIdx = newIdx;
        bestScore = newScore;
      }
    } else if (newScore > secondBestScore && bestIdx != newIdx) {
      secondBestScore = newScore;
      secondBestIdx = newIdx;
    }
  }

  // A corner is weak if its best match score falls below the score threshold
  // or if 2 (or more) highest match scores are close together (within
  // zncc_delta_threshold). Weak corners are rejected since the potiential matches
  // are nearly indistiguishable.
  bool isWeakCorner() const {
    return bestScore < FLAGS_match_score_threshold ||
        bestScore - secondBestScore < FLAGS_zncc_delta_threshold;
  }
};

double computeZncc(const Keypoint& corner0, const Keypoint& corner1) {
  CHECK_EQ(corner0.patch.size, corner1.patch.size);
  double zncc = 0;
  for (int x = 0; x < corner0.patch.cols; ++x) {
    for (int y = 0; y < corner0.patch.rows; ++y) {
      zncc += (corner0.patch(y, x) - corner0.avg) * (corner1.patch(y, x) - corner1.avg);
    }
  }

  if (FLAGS_custom_zncc) {
    /* Custom ZNCC formula:

      mean((p0 - p0.avg) * (p1 - p1.avg)) / p0.avg / p1.avg
      ---------------------------------------------
      max(p0.stddev / p0.avg, p1.stddev / p1.avg)^2
    */

    zncc /= corner0.patch.total();
    zncc /= (corner0.avg * corner1.avg); // finishes numerator calculation

    float denominator = std::max(corner0.std / corner0.avg, corner1.std / corner1.avg);
    zncc /= (denominator * denominator);
  } else {
    zncc /= (corner0.std * corner1.std * corner0.patch.total());
  }

  return zncc;
}

// Compute a box around the pixel in camera 1 corresponding to the specified point in camera 0
cv::Rect2f computeBox(
    const Camera& camera1,
    const Camera& camera0,
    const Camera::Vector2& pixel0,
    const double depth) {
  const Camera::Vector3 world = camera0.rig(pixel0, depth);
  const Camera::Vector2 pixel1 = camera1.pixel(world);
  return cv::Rect2f(
      pixel1.x() - FLAGS_search_radius,
      pixel1.y() - FLAGS_search_radius,
      2 * FLAGS_search_radius,
      2 * FLAGS_search_radius);
}

bool tooMuchOverlap(const cv::Rect2f& box, const cv::Rect2f& lastBox) {
  return (box & lastBox).area() > FLAGS_search_overlap * box.area();
}

// Compute what a corner in camera 0 looks like from camera 1. I.e.
// - find the point in camera 1 corresponding to the specified point in camera 0
// - then, for each pixel in a square around that point, read the corresponding pixel from camera 0
// Return false if camera 1 doesn't see the specified point
bool projectCorner(
    Image& projection1,
    const Camera& camera1,
    const Image& img0,
    const Camera& camera0,
    const Keypoint& corner0,
    const double depth0) {
  CHECK_EQ(img0.cols, camera0.resolution.x());
  CHECK_EQ(img0.rows, camera0.resolution.y());
  const Camera::Vector3 corner = camera0.rig(corner0.coords, depth0);
  Camera::Vector2 corner1;
  if (!camera1.sees(corner, corner1)) {
    return false;
  }
  const double depth1 = (corner - camera1.position).norm();

  CHECK_EQ(corner0.patch.cols, corner0.patch.rows);
  const int radius = corner0.patch.cols / 2;
  projection1.create(2 * radius + 1, 2 * radius + 1);
  for (int xOffset = -radius; xOffset <= radius; xOffset++) {
    for (int yOffset = -radius; yOffset <= radius; yOffset++) {
      const Camera::Vector2 pixel1 = {corner1.x() + xOffset, corner1.y() + yOffset};
      const Camera::Vector3 world = camera1.rig(pixel1, depth1);
      Camera::Vector2 pixel0;
      if (!camera0.sees(world, pixel0)) {
        return false;
      }
      projection1(yOffset + radius, xOffset + radius) = FLAGS_use_nearest
          ? img0(int(pixel0.y()), int(pixel0.x()))
          : cv_util::getPixelBilinear(img0, pixel0.x(), pixel0.y());
    }
  }
  return true;
}

bool hasCornerNearCenter(const Image& image) {
  const Camera::Vector2 center = 0.5 * Camera::Vector2(image.cols, image.rows);
  Camera::Vector2 best = center;
  for (const Camera::Vector2& corner : findScaledCorners(1, image, cv::Mat_<uint8_t>())) {
    Camera::Vector2 offset = corner - center;
    if (offset.dot(offset) < best.dot(best)) {
      best = offset;
    }
  }
  return best.dot(best) <= math_util::square(FLAGS_reprojected_corner_drift_tolerance);
}

bool getNextDepthSample(
    int& currentDepthSample,
    double& currentDisparity,
    cv::Rect2f& currentBox,
    const Camera& camera0,
    const Camera::Vector2& corner0Coords,
    const Camera& camera1) {
  for (int nextDepthSample = currentDepthSample + 1; nextDepthSample < FLAGS_depth_samples;
       ++nextDepthSample) {
    // We don't bother testing a disparity unless the search box is substantially different
    const double nextDisparity = math_util::lerp(
        1 / FLAGS_depth_max, 1 / FLAGS_depth_min, nextDepthSample / (FLAGS_depth_samples - 1.0));
    const cv::Rect2f nextBox = computeBox(camera1, camera0, corner0Coords, 1 / nextDisparity);
    if (!tooMuchOverlap(nextBox, currentBox)) {
      currentDepthSample = nextDepthSample;
      currentDisparity = nextDisparity;
      currentBox = nextBox;
      return true;
    }
  }
  return false;
}

Overlap findMatches(
    const Image& img0,
    const std::vector<Keypoint>& corners0,
    const Camera& camera0,
    const Image& img1,
    const std::vector<Keypoint>& corners1,
    const Camera& camera1) {
  boost::timer::cpu_timer timer;
  boost::timer::cpu_timer znccTimer;
  znccTimer.stop();
  boost::timer::cpu_timer projectCornerTimer;
  projectCornerTimer.stop();

  Image image1; // optimization: avoid reallocation by keeping this outside loop

  // For each corner in corners0, compute its best and second best match in corners1. and vice versa
  std::vector<BestMatch> bestMatches0(corners0.size());
  std::vector<BestMatch> bestMatches1(corners1.size());
  int callsToZncc = 0;
  int callsToProjectCorners = 0;
  for (ssize_t index0 = 0; index0 < ssize(corners0); index0++) {
    LOG_IF(INFO, (FLAGS_threads == 0 || FLAGS_threads == 1) && (index0 % 1000) == 0)
        << "Processing feature " << index0 << " of " << corners0.size() << " from pair "
        << camera0.id << " " << camera1.id;

    const Keypoint& corner0 = corners0[index0];
    int depthSample = -1;
    double disparity = 0;
    cv::Rect2f box1(0, 0, 0, 0);
    bool firstProjection = true;
    while (getNextDepthSample(depthSample, disparity, box1, camera0, corner0.coords, camera1)) {
      // only remap corner for sufficiently large disparities
      if (firstProjection || disparity > 1 / FLAGS_max_depth_for_remap) {
        // compute what the area around corner 0 would look like from camera 1
        projectCornerTimer.resume();
        callsToProjectCorners ++;
        if (!projectCorner(image1, camera1, img0, camera0, corner0, 1 / disparity)) {
          continue;
        }
        projectCornerTimer.stop();

        // don't match if we can't rediscover the corner after it has been reprojected
        if (!hasCornerNearCenter(image1)) {
          continue;
        }
        firstProjection = false;
      }

      Keypoint projection1(image1);

      // look for a corner in c1 that is in the box and looks similar
      znccTimer.resume();
      for (ssize_t index1 = 0; index1 < ssize(corners1); ++index1) {
        cv::Point2f cvCoords(corners1[index1].coords.x(), corners1[index1].coords.y());
        if (!box1.contains(cvCoords)) {
          continue;
        }
        double score = computeZncc(projection1, corners1[index1]);
        bestMatches0[index0].updateCornerScore(score, index1);
        bestMatches1[index1].updateCornerScore(score, index0);
        callsToZncc ++;
      }
      znccTimer.stop();
    }
  }

  // Take match if both ends are strong and each other's best match
  Overlap overlap(camera0.id, camera1.id);
  for (const BestMatch& bestMatch0 : bestMatches0) {
    if (bestMatch0.isWeakCorner()) {
      continue;
    }
    const BestMatch& bestMatch1 = bestMatches1[bestMatch0.bestIdx];
    if (bestMatch1.isWeakCorner()) {
      continue;
    }
    if (&bestMatch0 != &bestMatches0[bestMatch1.bestIdx]) {
      continue;
    }
    overlap.matches.emplace_back(bestMatch0.bestScore, bestMatch1.bestIdx, bestMatch0.bestIdx);
  }

  // Only report timing in single threaded mode
  // In multithreaded mode these will clocks include time from other threads
  // running simultaneously
  if (FLAGS_enable_timing && FLAGS_threads == 1) {
    LOG(INFO) << folly::sformat(
        "{} and {} matching complete. Overlap fraction: {}. Matches: {}. Timing: {} "
        "Calls to ZNCC: {}. ZNCC Time: {} "
        "Calls to ProjectCorners: {}. Project Corner Time: {} ",
        camera0.id,
        camera1.id,
        camera0.overlap(camera1),
        overlap.matches.size(),
        timer.format(),
        callsToZncc,
        znccTimer.format(),
        callsToProjectCorners,
        projectCornerTimer.format());
  } else {
    LOG(INFO) << folly::sformat(
        "{} and {} matching complete. Overlap fraction: {}. Matches: {}",
        camera0.id,
        camera1.id,
        camera0.overlap(camera1),
        overlap.matches.size());
  }
  return overlap;
}

std::vector<Overlap> findAllMatches(
    const Camera::Rig& rig,
    const std::vector<Image>& images,
    const std::map<ImageId, std::vector<Keypoint>>& allCorners) {
  std::vector<Overlap> overlaps;
  boost::timer::cpu_timer matchTimer;

  // Find matches across all pairings
  const int threadCount = ThreadPool::getThreadCountFromFlag(FLAGS_threads);
  std::queue<std::future<Overlap>> overlapFutures;
  for (ssize_t c1 = 0; c1 < ssize(rig); c1++) {
    for (ssize_t c2 = c1 + 1; c2 < ssize(rig); c2++) {
      if (rig[c1].overlap(rig[c2]) < FLAGS_overlap_threshold) {
        continue;
      }
      overlapFutures.push(std::async(
          threadCount == 0 ? std::launch::deferred : std::launch::async,
          &findMatches,
          std::cref(images[c1]),
          std::cref(allCorners.at(rig[c1].id)),
          std::cref(rig[c1]),
          std::cref(images[c2]),
          std::cref(allCorners.at(rig[c2].id)),
          std::cref(rig[c2])));
      if (ssize(overlapFutures) >= threadCount && !overlapFutures.empty()) {
        overlaps.emplace_back(overlapFutures.front().get());
        overlapFutures.pop();
      }
    }
  }

  while (!overlapFutures.empty()) {
    overlaps.emplace_back(overlapFutures.front().get());
    overlapFutures.pop();
  }

  if (FLAGS_enable_timing) {
    LOG(INFO) << folly::sformat("Matching stage time: {}", matchTimer.format());
  }

  return overlaps;
}

} // namespace calibration
} // namespace fb360_dep


#ifndef MESH_HPP
#define MESH_HPP

#include <polyfem/Common.hpp>
#include <polyfem/Navigation.hpp>
#include <polyfem/Types.hpp>

#include <Eigen/Dense>
#include <geogram/mesh/mesh.h>
#include <memory>

namespace polyfem
{
	// NOTE:
	// For the purpose of the tagging, elements (facets in 2D, cells in 3D) adjacent to a polytope
	// are tagged as boundary, and vertices incident to a polytope are also considered as boundary.
	enum class ElementType
	{
		Simplex,					// Triangle/tet element
		RegularInteriorCube,		// Regular quad/hex inside a 3^n patch
		SimpleSingularInteriorCube, // Quad/hex incident to exactly 1 singular vertex (in 2D) or edge (in 3D)
		MultiSingularInteriorCube,  // Quad/Hex incident to more than 1 singular vertices (should not happen in 2D)
		RegularBoundaryCube,		// Boundary quad/hex, where all boundary vertices/edges are incident to at most 2 quads/hexes
		SimpleSingularBoundaryCube, // Quad incident to exactly 1 singular vertex (in 2D); hex incident to exactly 1 singular interior edge, 0 singular boundary edge, 1 boundary face (in 3D)
		MultiSingularBoundaryCube,  // Boundary hex that is not regular nor SimpleSingularBoundaryCube
		InterfaceCube,				// Quad/hex that is at the interface with a polytope (if a cube has both external boundary and and interface with a polytope, it is marked as interface)
		InteriorPolytope,			// Interior polytope
		BoundaryPolytope,			// Boundary polytope
		Undefined					// For invalid configurations
	};

	class Mesh
	{
	protected:
		class EdgeNodes
		{
		public:
			int v1, v2;
			Eigen::MatrixXd nodes;
		};

		class FaceNodes
		{
		public:
			int v1, v2, v3;
			Eigen::MatrixXd nodes;
		};

		class CellNodes
		{
		public:
			int v1, v2, v3, v4;
			Eigen::MatrixXd nodes;
		};

	public:
		static std::unique_ptr<Mesh> create(const std::string &path);
		static std::unique_ptr<Mesh> create(GEO::Mesh &M);
		static std::unique_ptr<Mesh> create(const std::vector<json> &meshes);

		Mesh() = default;
		virtual ~Mesh() = default;
		POLYFEM_DEFAULT_MOVE_COPY(Mesh)

		virtual void refine(const int n_refinement, const double t, std::vector<int> &parent_nodes) = 0;

		//Queries
		virtual bool is_volume() const = 0;
		int dimension() const { return (is_volume() ? 3 : 2); }

		int n_elements() const { return (is_volume() ? n_cells() : n_faces()); }

		virtual int n_cells() const = 0;
		virtual int n_faces() const = 0;
		virtual int n_edges() const = 0;
		virtual int n_vertices() const = 0;

		virtual bool is_boundary_vertex(const int vertex_global_id) const = 0;
		virtual bool is_boundary_edge(const int edge_global_id) const = 0;
		virtual bool is_boundary_face(const int face_global_id) const = 0;
		virtual bool is_boundary_element(const int element_global_id) const = 0;

		//IO
		virtual bool build_from_matrices(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F) = 0;
		virtual bool save(const std::string &path) const = 0;

		virtual void attach_higher_order_nodes(const Eigen::MatrixXd &V, const std::vector<std::vector<int>> &nodes) = 0;
		inline const Eigen::MatrixXi &orders() const { return orders_; }
		inline bool is_rational() const { return is_rational_; }

		virtual void normalize() = 0;

		//Tagging of the elements
		virtual void compute_elements_tag() = 0;
		virtual void update_elements_tag() { assert(false); }

		//d-1 primitive sizes
		virtual double edge_length(const int gid) const
		{
			assert(false);
			return 0;
		}
		virtual double quad_area(const int gid) const
		{
			assert(false);
			return 0;
		}
		virtual double tri_area(const int gid) const
		{
			assert(false);
			return 0;
		}

		//Nodal access
		virtual RowVectorNd point(const int global_index) const = 0;
		virtual int cell_vertex(const int f_id, const int lv_id) const = 0;
		virtual RowVectorNd edge_barycenter(const int e) const = 0;
		virtual RowVectorNd face_barycenter(const int f) const = 0;
		virtual RowVectorNd cell_barycenter(const int c) const = 0;
		void edge_barycenters(Eigen::MatrixXd &barycenters) const;
		void face_barycenters(Eigen::MatrixXd &barycenters) const;
		void cell_barycenters(Eigen::MatrixXd &barycenters) const;

		virtual void elements_boxes(std::vector<std::array<Eigen::Vector3d, 2>> &boxes) const = 0;
		virtual void barycentric_coords(const RowVectorNd &p, const int el_id, Eigen::MatrixXd &coord) const = 0;

		virtual void bounding_box(RowVectorNd &min, RowVectorNd &max) const = 0;

		//Queries on the tags
		bool is_spline_compatible(const int el_id) const;
		bool is_cube(const int el_id) const;
		bool is_polytope(const int el_id) const;
		bool is_simplex(const int el_id) const;

		const std::vector<ElementType> &elements_tag() const { return elements_tag_; }

		//Boundary condition handling
		virtual void load_boundary_ids(const std::string &path);
		virtual void compute_boundary_ids(const double eps) = 0;
		virtual void compute_boundary_ids(const std::function<int(const RowVectorNd &)> &marker) = 0;
		virtual void compute_boundary_ids(const std::function<int(const RowVectorNd &, bool)> &marker) = 0;
		virtual void compute_boundary_ids(const std::function<int(const std::vector<int> &, bool)> &marker) = 0;
		virtual void compute_body_ids(const std::function<int(const RowVectorNd &)> &marker) = 0;
		void set_boundary_ids(const std::vector<int> &boundary_ids) { boundary_ids_ = boundary_ids; }
		void set_body_ids(const std::vector<int> &body_ids) { body_ids_ = body_ids; }
		void set_tag(const int el, const ElementType type) { elements_tag_[el] = type; }

		inline int get_boundary_id(const int primitive) const { return boundary_ids_[primitive]; }
		inline int get_body_id(const int primitive) const
		{
			if (has_body_ids())
				return body_ids_[primitive];
			else
				return 0;
		}

		inline bool has_boundary_ids() const { return !boundary_ids_.empty(); }
		inline bool has_body_ids() const { return !body_ids_.empty(); }

		//Visualization methods
		virtual void compute_element_barycenters(Eigen::MatrixXd &barycenters) const = 0;
		virtual void get_edges(Eigen::MatrixXd &p0, Eigen::MatrixXd &p1) const = 0;
		virtual void get_edges(Eigen::MatrixXd &p0, Eigen::MatrixXd &p1, const std::vector<bool> &valid_elements) const = 0;
		virtual void triangulate_faces(Eigen::MatrixXi &tris, Eigen::MatrixXd &pts, std::vector<int> &ranges) const = 0;

		const std::vector<double> &cell_weights(const int cell_index) const { return cell_weights_[cell_index]; }

		bool has_poly() const
		{
			for (int i = 0; i < n_elements(); ++i)
			{
				if (is_polytope(i))
					return true;
			}

			return false;
		}

	protected:
		virtual bool load(const std::string &path) = 0;
		virtual bool load(const GEO::Mesh &M) = 0;

		std::vector<ElementType> elements_tag_;
		std::vector<int> boundary_ids_;
		std::vector<int> body_ids_;
		Eigen::MatrixXi orders_;
		bool is_rational_ = false;

		std::vector<EdgeNodes> edge_nodes_;
		std::vector<FaceNodes> face_nodes_;
		std::vector<CellNodes> cell_nodes_;
		std::vector<std::vector<double>> cell_weights_;
	};
} // namespace polyfem

#endif //MESH_HPP


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_BITINTEGER_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_BITINTEGER_HPP_INCLUDED

#include <boost/simd/ieee/functions/bitinteger.hpp>
#include <boost/simd/include/functions/scalar/bitwise_cast.hpp>
#include <boost/simd/include/functions/scalar/is_positive.hpp>
#include <boost/simd/include/constants/signmask.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::bitinteger_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return is_positive(a0) ?
            bitwise_cast<result_type, A0>(a0) :
            Signmask<result_type>()-bitwise_cast<result_type, A0>(a0);
    }
  };
} } }

#endif


#include <cctbx/boost_python/flex_fwd.h>

#include <scitbx/boost_python/utils.h>
#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <cctbx/maptbx/bcr/bcr.h>

#include <scitbx/array_family/boost_python/shared_wrapper.h>
#include <scitbx/boost_python/is_polymorphic_workaround.h>

namespace cctbx { namespace maptbx { namespace boost_python {

  boost::python::tuple
  getinitargs_bcr_model(bcr_model<> const& self)
  {
    return boost::python::make_tuple(
      self.scatterer,
      self.B,
      self.C,
      self.R);
  }

namespace {

  void init_module()
  {
    using namespace boost::python;

    {
      typedef return_value_policy<return_by_value> rbv;
      class_<bcr_model<> >("bcr_model")
        .def(init<cctbx::xray::scatterer<> const&,
                  af::shared<double> const&,
                  af::shared<double> const&,
                  af::shared<double> const& >((arg("scatterer"),arg("B"),arg("C"),
                                                 arg("R"))))
        .add_property("scatterer", make_getter(&bcr_model<>::scatterer, rbv()))
        .add_property("B",         make_getter(&bcr_model<>::B,         rbv()))
        .add_property("C",         make_getter(&bcr_model<>::C,         rbv()))
        .add_property("R",         make_getter(&bcr_model<>::R,         rbv()))
        .enable_pickling()
        .def("__getinitargs__", getinitargs_bcr_model)
        //.def("atom_radius",   &bcr_model<>::atom_radius)
        //.def("rho",           &bcr_model<>::rho)
      ;
    }

    {
      class_<image<> >("BCRimage", no_init) // why no_init ?
        .def(init<
          cctbx::uctbx::unit_cell const&,
          boost::python::list const&,
          int const& >(
                    (arg("unit_cell"),
                     arg("bcr_models"),
                     arg("step"))))
        .def("fsc",   &image<>::cc)
        .def("d",     &image<>::d)
        .def("d_inv", &image<>::d_inv)
      ;
    }

    {
      class_<calculator<> >("calculator")
        .def(init<bcr_model<double> const& >((arg("bcr_model") )))
        .def("atom_radius",   &calculator<>::atom_radius)
        .def("rho",           &calculator<>::rho)
      ;
    }

  }

} // namespace <anonymous>

}}} // namespace cctbx::maptbx::boost_python

BOOST_PYTHON_MODULE(cctbx_maptbx_bcr_bcr_ext)
{
  cctbx::maptbx::boost_python::init_module();
}


#include <ciphey/freq.hpp>

#include <boost/math/special_functions/gamma.hpp>

#include <future>
#include <optional>
#include <random>
#include <set>
#include <thread>

namespace ciphey {
  static void merge_last(assoc_table& assoc) {
    // Take the last two elements
    auto to_merge = std::move(assoc.back());
    assoc.pop_back();
    auto target = assoc.back();
    assoc.pop_back();

    // Combine them
    target.expected += to_merge.expected;
    target.observed += to_merge.observed;

    // Insert them into the correct position
    auto new_pos = std::partition_point(assoc.begin(), assoc.end(),
                                        [&](auto& a) { return a.expected > target.expected; });
    assoc.insert(new_pos, std::move(target));

  }

  void prepare_chisq(assoc_table& assoc, freq_t count) {
    // IIRC copying is much, MUCH faster than mallocing, so a list would be slower in this case
    //
    // Benchmarks seem to confirm this

    // If there's nothing we can do, give up
    if (count < 10)
      return;

    std::sort(assoc.begin(), assoc.end(), [](auto& a, auto& b) { return a.expected > b.expected; });

    // We now perform a *really* simple bin packing to make sure that no expected frequency is lower that 1
    auto exp_1_target = 1. / count;

    while (assoc.size() >= 2 && assoc.back().expected < exp_1_target)
      merge_last(assoc);

    auto exp_5_target = 5. / count;
    if (exp_5_target > 0.2)
      return;
    while (assoc.size() >= 2) {
      auto upper_bound = std::partition_point(assoc.begin(), assoc.end(),
                                              [&](auto& a) { return a.expected > exp_5_target; });
      auto n_less_than_fifth = assoc.end() - upper_bound;
      if (n_less_than_fifth <= assoc.size() / 5.)
        break;
      merge_last(assoc);
    }
  }

  float_t run_chisq(const assoc_table& assoc, freq_t count) {
    float_t chisq = 0;

    for (auto const& elem : assoc) {
      if (elem.expected == 0 && elem.observed != 0)
        return std::numeric_limits<float_t>::infinity();

      float_t contrib = elem.expected - elem.observed;
      contrib *= contrib;
      contrib /= elem.expected;

      chisq += contrib;
    }

    // (f_e/n - f_o/n)^2/(f_e/n) = (1/n) (f_e - f_o)/(f_e)
    //
    // We need to normalise this for our stat to be interpretable
    chisq *= count;
    return chisq;
  }

  float_t run_g(const assoc_table& assoc, freq_t count) {
    float_t g = 0;

    for (auto const& elem : assoc) {
      if (elem.observed == 0)
        continue;

      if (elem.expected == 0)
        return std::numeric_limits<float_t>::infinity();

      float_t contrib = ::log(elem.observed / elem.expected); // `count` cancels out here
      contrib *= elem.observed;

      g += contrib;
    }

    // (f_e/n - f_o/n)^2/(f_e/n) = (1/n) (f_e - f_o)/(f_e)
    //
    // We need to normalise this for our stat to be interpretable
    g *= 2 * count;
    return g;
  }

  prob_t chisq_cdf(freq_t dof, float_t up_to) {
    // Handle the asymptopic value first, to save time
    if (up_to == std::numeric_limits<float_t>::infinity())
      return 1;
    else if (up_to == 0)
      return 0;
    else if (dof == 0) // https://en.wikipedia.org/wiki/Zero_degrees_of_freedom
      return 0;
    return boost::math::gamma_p(static_cast<float>(dof) / 2, up_to / 2);
  }

  prob_t gof_test(assoc_table assoc, freq_t count) {
//    prepare_chisq(assoc, count);
    auto stat = run_g(assoc, count);
    // We want the upper tail
    auto p_value = 1 - chisq_cdf(assoc.size() - 1, stat);

    return p_value;
//    return std::max(p_value, orig_p_value);
  }

  assoc_table create_assoc_table(prob_table const& observed, prob_table const& expected) {
    // TODO: optimise
    //
    // First, let's get all the keys
    struct key_elem {
      std::optional<float_t> obs_val, exp_val;
    };

    std::map<char_t, key_elem> keys;
    // We can use index notation for this, as that gets default constructed
    for (auto& i : expected)
      keys[i.first].exp_val = i.second;
    for (auto& i : observed) {
      if (i.second == 0.)
        continue;
      keys[i.first].obs_val = i.second;
    }

    // We can now fill in all the values, with non-existent expected values being defined as zero
    assoc_table ret;
//    ret.reserve(keys.size());

    for (auto& i : keys)
      ret.emplace_back(assoc_table_elem{
        .observed = i.second.obs_val.value_or(0),
        .expected = i.second.exp_val.value_or(0)});

    return ret;
  }

  prob_table freq_conv(freq_table const& freqs, freq_t total_len) {
    prob_table ret;
    for (auto& i : freqs)
      ret[i.first] = static_cast<prob_t>(i.second) / total_len;
    return ret;
  }
  prob_table freq_conv(freq_table const& freqs) {
    freq_t total_len = 0;
    for (auto& i : freqs)
      total_len += i.second;
    return freq_conv(freqs, total_len);
  }
  windowed_prob_table freq_conv(windowed_freq_table& freqs, freq_t total_len) {
    windowed_prob_table ret;
    ret.reserve(total_len);
    for (auto& i : freqs)
      // TODO: work the length out here, rather than do slow counting
      ret.emplace_back(freq_conv(i));
    return ret;
  }
  windowed_prob_table freq_conv(windowed_freq_table& freqs) {
    freq_t total_len = 0;
    for (auto& i : freqs)
      for (auto& entry: i)
        total_len += entry.second;
    return freq_conv(freqs, total_len);
  }

  size_t filter_missing(freq_table& target, prob_table const& lookup) {
    std::vector<char_t> to_remove;
    size_t ret = 0;
    for (auto& i : target) {
      if (auto iter = lookup.find(i.first); iter == lookup.end() || iter->second == 0) {
        to_remove.push_back(i.first);
        ret += i.second;
      }
    }

    for (auto i : to_remove)
      target.erase(i);

    return ret;
  }

  size_t filter_missing(freq_table& target, domain_t const& tab) {
    std::vector<char_t> to_remove;
    size_t ret = 0;
    for (auto& i : target) {
      if (!tab.count(i.first)) {
        to_remove.push_back(i.first);
        ret += i.second;
      }
    }

    for (auto i : to_remove)
      target.erase(i);

    return ret;
  }

//  size_t filter_missing(windowed_freq_table& target, prob_table const& lookup) {
//    size_t acc = 0;
//    for (auto& i : target)
//      acc += filter_missing(i, lookup);
//    return acc;
//  }

  void freq_analysis(freq_table& tab, string_const_ref_t const& str) {
    for (auto& i : str)
      ++tab[i];
  }
  size_t freq_analysis(freq_table& tab, string_const_ref_t const& str, domain_t const& domain) {
    size_t n = 0;
    for (auto& i : str) {
      if (domain.count(i)) {
        ++tab[i];
        ++n;
      }
    }
    return n;
  }
  void freq_analysis(windowed_freq_table& tabs, string_const_ref_t const& str, size_t offset) {
    for (size_t i = 0; i < str.size(); ++i)
      ++tabs[(offset + i) % tabs.size()][str[i]];
  }

  size_t freq_analysis(windowed_freq_table& tabs, string_const_ref_t const& str, domain_t const& domain, size_t offset) {
    for (auto& c : str) {
      if (domain.count(c)) {
        ++tabs[offset % tabs.size()][c];
        ++offset;
      }
    }
    return offset;
  }

  string_t generate_fuzz(prob_table const& tab, size_t len) {
    string_t ret;
    ret.resize(len);
    std::mt19937 rng;

    {
      thread_local std::random_device seed_rng;
      thread_local std::uniform_int_distribution<decltype(rng)::result_type> seed_dist;
      rng.seed(seed_dist(seed_rng));
    }

    thread_local std::uniform_real_distribution<float_t> dist{0, 1};

    for (auto& rand_char : ret) {
    restat_char:
      float_t stat = dist(rng);
      // Iterate through the table, removing the probabilities until we are within a bracket
      for (auto const& i : tab) {
        if ((stat -= i.second) <= 0) {
          rand_char = i.first;
          goto next_char;
        }
      }
      // This should not happen in normal usage!
      //
      // However, floats do weird things with rounding, so we will be leniant
      goto restat_char;
      next_char: {}
    }

    return ret;
  }

  assoc_table closeness_assoc(prob_table const& observed, prob_table const& expected) {
    // What can we possibly do with this?
    if (observed.size() == 0)
      return {{.observed = 0, .expected = 1}};

    assoc_table assoc;

    // Sort the expected values
    std::vector<prob_t> expected_sorted;
    expected_sorted.reserve(expected_sorted.size());
    for (auto& i : expected)
      expected_sorted.emplace_back(i.second);
    std::sort(expected_sorted.rbegin(), expected_sorted.rend());

    // Sort the observed values
    std::vector<prob_t> observed_sorted;
    observed_sorted.reserve(observed.size());
    for (auto& i : observed)
      observed_sorted.emplace_back(i.second);
    std::sort(observed_sorted.rbegin(), observed_sorted.rend());

    // Fill table with observed values, or zeroes where appropriate
    size_t i;
    if (observed.size() > expected.size()) {
      for (i = 0; i < expected.size(); ++i)
        assoc.emplace_back(assoc_table_elem{.observed = observed_sorted[i], .expected = expected_sorted[i]});
      for (; i < observed.size(); ++i)
        assoc.emplace_back(assoc_table_elem{.observed = observed_sorted[i], .expected = 0.});
    }
    else {
      for (i = 0; i < observed.size(); ++i)
        assoc.emplace_back(assoc_table_elem{.observed = observed_sorted[i], .expected = expected_sorted[i]});
      for (; i < expected.size(); ++i)
        assoc.emplace_back(assoc_table_elem{.observed = 0, .expected = expected_sorted[i]});
    }

    return assoc;
  }

  prob_t closeness_test(prob_table const& observed, prob_table const& expected, freq_t count) {
    // Quick bypass to avoid more filling than we have to
    if (observed.size() > expected.size())
      return 0;

    auto assoc = closeness_assoc(observed, expected);
    prepare_chisq(assoc, count);
    auto stat = run_chisq(assoc, count);

    return 1 - chisq_cdf(assoc.size() - 1, stat);
  }

  prob_t closeness_test(windowed_prob_table const& observed, prob_table const& expected, freq_t count) {
    struct imdt_res_t {
      size_t tab_size;
      float_t chi_sq;
    };

    std::vector<std::future<imdt_res_t>> asyncs(observed.size());

    for (size_t i = 0; i < observed.size(); ++i) {
      asyncs[i] = std::async(std::launch::async, [&, i]() -> imdt_res_t {
        auto assoc = closeness_assoc(observed[i], expected);
        prepare_chisq(assoc, count);

        // We can normalise at the end for efficiency
        return {.tab_size = assoc.size(), .chi_sq = run_chisq(assoc)};
      });
    }

    float_t stat = 0;
    size_t total_tab_len = 0;
    for (auto& i : asyncs) {
      auto res = i.get();
      total_tab_len += res.tab_size;
      stat += res.chi_sq;
    }

    // Finally, we can normalise
    stat *= count;

    return 1 - chisq_cdf(total_tab_len - 1, stat);
  }

  float_t calculate_entropy(std::map<uint8_t, freq_t> const& freqs, size_t len) {
    float_t ret = 0;
    for (auto& i : freqs)  {
      float_t observed_prob = static_cast<float_t>(i.second) / len;
      ret -= observed_prob * ::log2(observed_prob);
      // Some nice printing, kept for future debug
//      std::cout << "  " << std::hex << (int)i.first << ": " << i.second << std::endl;
    }

//    std::cout << ret << std::endl;
    return ret;
  }

  float_t information_content(bytes_const_ref_t b) {
    // TODO:
    // I would prefer something more empirical,
    // rather than just brute forcing the token size

    std::map<uint8_t, freq_t> byte_freqs;
    std::map<uint8_t, freq_t> nybble_freqs;
    std::map<uint8_t, freq_t> pair_freqs;
    std::map<uint8_t, freq_t> bit_freqs;
    for (auto i : b) {
      ++byte_freqs[i];
      ++nybble_freqs[i>>4];
      ++nybble_freqs[i&0xf];
      ++pair_freqs[i&0x3];
      ++pair_freqs[(i>>2)&0x3];
      ++pair_freqs[(i>>4)&0x3];
      ++pair_freqs[(i>>6)&0x3];
      for (int bit = 0; bit < 8; ++bit)
        ++bit_freqs[(i >> bit) & 1];
    }

    return std::min({
                      calculate_entropy(byte_freqs,   b.size()    ) / 8,
                      calculate_entropy(nybble_freqs, b.size() * 2) / 4,
                      calculate_entropy(pair_freqs,   b.size() * 4) / 2,
                      calculate_entropy(bit_freqs,    b.size() * 8)
                    }) * b.size();
  }

  freq_t hamming_weight(uint8_t byte) {
    /// Precomputed hamming weight table for each byte
    static std::array<freq_t, 256> weights {
      0, 1, 1, 2, 1, 2, 2, 3, 1, 2, 2, 3, 2, 3, 3, 4,
      1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
      1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
      2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
      1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
      2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
      2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
      3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
      1, 2, 2, 3, 2, 3, 3, 4, 2, 3, 3, 4, 3, 4, 4, 5,
      2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
      2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
      3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
      2, 3, 3, 4, 3, 4, 4, 5, 3, 4, 4, 5, 4, 5, 5, 6,
      3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
      3, 4, 4, 5, 4, 5, 5, 6, 4, 5, 5, 6, 5, 6, 6, 7,
      4, 5, 5, 6, 5, 6, 6, 7, 5, 6, 6, 7, 6, 7, 7, 8,
    };

    return weights[byte];
  }

  freq_t hamming_weight(bytes_const_ref_t b) {
    freq_t ret = 0;
    for (auto i : b)
      ret += hamming_weight(i);
    return ret;
  }

  freq_t hamming_distance(bytes_const_ref_t x, bytes_const_ref_t y) {
    if (x.size() != y.size())
      throw std::invalid_argument("Lengths must be the same for hamming distance");
    freq_t ret = 0;
    for (size_t i = 0; i < x.size(); ++i)
      ret += hamming_weight(x[i] ^ y[i]);
    return ret;
  }
}