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

/** \author Jia Pan */

#include <hpp/fcl/ccd/taylor_model.h>
#include <cassert>
#include <iostream>
#include <cmath>
#include <boost/math/constants/constants.hpp>

namespace fcl
{

TaylorModel::TaylorModel()
{
  coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
}

TaylorModel::TaylorModel(const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
{
  coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
}

TaylorModel::TaylorModel(FCL_REAL coeff, const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
{
  coeffs_[0] = coeff;
  coeffs_[1] = coeffs_[2] = coeffs_[3] = r_[0] = r_[1] = 0;
}

TaylorModel::TaylorModel(FCL_REAL coeffs[3], const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
{
  coeffs_[0] = coeffs[0];
  coeffs_[1] = coeffs[1];
  coeffs_[2] = coeffs[2];
  coeffs_[3] = coeffs[3];

  r_ = r;
}

TaylorModel::TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval) : time_interval_(time_interval)
{
  coeffs_[0] = c0;
  coeffs_[1] = c1;
  coeffs_[2] = c2;
  coeffs_[3] = c3;

  r_ = r;
}

TaylorModel TaylorModel::operator + (FCL_REAL d) const
{
  return TaylorModel(coeffs_[0] + d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_);
}

TaylorModel& TaylorModel::operator += (FCL_REAL d)
{
  coeffs_[0] += d;
  return *this;
}

TaylorModel TaylorModel::operator - (FCL_REAL d) const
{
  return TaylorModel(coeffs_[0] - d, coeffs_[1], coeffs_[2], coeffs_[3], r_, time_interval_);
}

TaylorModel& TaylorModel::operator -= (FCL_REAL d)
{
  coeffs_[0] -= d;
  return *this;
}

TaylorModel TaylorModel::operator + (const TaylorModel& other) const
{
  assert(other.time_interval_ == time_interval_);
  return TaylorModel(coeffs_[0] + other.coeffs_[0], coeffs_[1] + other.coeffs_[1], coeffs_[2] + other.coeffs_[2], coeffs_[3] + other.coeffs_[3], r_ + other.r_, time_interval_);
}

TaylorModel TaylorModel::operator - (const TaylorModel& other) const
{
  assert(other.time_interval_ == time_interval_);
  return TaylorModel(coeffs_[0] - other.coeffs_[0], coeffs_[1] - other.coeffs_[1], coeffs_[2] - other.coeffs_[2], coeffs_[3] - other.coeffs_[3], r_ - other.r_, time_interval_);
}
TaylorModel& TaylorModel::operator += (const TaylorModel& other)
{
  assert(other.time_interval_ == time_interval_);
  coeffs_[0] += other.coeffs_[0];
  coeffs_[1] += other.coeffs_[1];
  coeffs_[2] += other.coeffs_[2];
  coeffs_[3] += other.coeffs_[3];
  r_ += other.r_;
  return *this;
}

TaylorModel& TaylorModel::operator -= (const TaylorModel& other)
{
  assert(other.time_interval_ == time_interval_);
  coeffs_[0] -= other.coeffs_[0];
  coeffs_[1] -= other.coeffs_[1];
  coeffs_[2] -= other.coeffs_[2];
  coeffs_[3] -= other.coeffs_[3];
  r_ -= other.r_;
  return *this;
}

/// @brief Taylor model multiplication:
/// f(t) = c0+c1*t+c2*t^2+c3*t^3+[a,b]
/// g(t) = c0'+c1'*t+c2'*t^2+c3'*t^2+[c,d]
/// f(t)g(t)= c0c0'+
///           (c0c1'+c1c0')t+
///           (c0c2'+c1c1'+c2c0')t^2+
///           (c0c3'+c1c2'+c2c1'+c3c0')t^3+
///           [a,b][c,d]+
///           (c1c3'+c2c2'+c3c1')t^4+
///           (c2c3'+c3c2')t^5+
///           (c3c3')t^6+
///           (c0+c1*t+c2*t^2+c3*t^3)[c,d]+
///           (c0'+c1'*t+c2'*t^2+c3'*c^3)[a,b]
TaylorModel TaylorModel::operator * (const TaylorModel& other) const
{
  TaylorModel res(*this);
  res *= other;
  return res;
}

TaylorModel TaylorModel::operator * (FCL_REAL d) const
{
  return TaylorModel(coeffs_[0] * d, coeffs_[1] * d, coeffs_[2] * d, coeffs_[3] * d,  r_ * d, time_interval_);
}

TaylorModel& TaylorModel::operator *= (const TaylorModel& other)
{
  assert(other.time_interval_ == time_interval_);
  register FCL_REAL c0, c1, c2, c3;
  register FCL_REAL c0b = other.coeffs_[0], c1b = other.coeffs_[1], c2b = other.coeffs_[2], c3b = other.coeffs_[3];

  const Interval& rb = other.r_;

  c0 = coeffs_[0] * c0b;
  c1 = coeffs_[0] * c1b + coeffs_[1] * c0b;
  c2 = coeffs_[0] * c2b + coeffs_[1] * c1b + coeffs_[2] * c0b;
  c3 = coeffs_[0] * c3b + coeffs_[1] * c2b + coeffs_[2] * c1b + coeffs_[3] * c0b;

  Interval remainder(r_ * rb);
  register FCL_REAL tempVal = coeffs_[1] * c3b + coeffs_[2] * c2b + coeffs_[3] * c1b;
  remainder += time_interval_->t4_ * tempVal;

  tempVal = coeffs_[2] * c3b + coeffs_[3] * c2b;
  remainder += time_interval_->t5_ * tempVal;

  tempVal = coeffs_[3] * c3b;
  remainder += time_interval_->t6_ * tempVal;

  remainder += ((Interval(coeffs_[0]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3]) * rb +
                (Interval(c0b) + time_interval_->t_ * c1b + time_interval_->t2_ * c2b + time_interval_->t3_ * c3b) * r_);

  coeffs_[0] = c0;
  coeffs_[1] = c1;
  coeffs_[2] = c2;
  coeffs_[3] = c3;

  r_ = remainder;

  return *this;
}

TaylorModel& TaylorModel::operator *= (FCL_REAL d)
{
  coeffs_[0] *= d;
  coeffs_[1] *= d;
  coeffs_[2] *= d;
  coeffs_[3] *= d;
  r_ *= d;
  return *this;
}

TaylorModel TaylorModel::operator - () const
{
  return TaylorModel(-coeffs_[0], -coeffs_[1], -coeffs_[2], -coeffs_[3], -r_, time_interval_);
}

void TaylorModel::print() const
{
  std::cout << coeffs_[0] << "+" << coeffs_[1] << "*t+" << coeffs_[2] << "*t^2+" << coeffs_[3] << "*t^3+[" << r_[0] << "," << r_[1] << "]" << std::endl;
}

Interval TaylorModel::getBound(FCL_REAL t) const
{
  return Interval(coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]))) + r_;
}

Interval TaylorModel::getBound(FCL_REAL t0, FCL_REAL t1) const
{
  Interval t(t0, t1);
  Interval t2(t0 * t0, t1 * t1);
  Interval t3(t0 * t2[0], t1 * t2[1]);

  return Interval(coeffs_[0]) + t * coeffs_[1] + t2 * coeffs_[2] + t3 * coeffs_[3] + r_;
}

Interval TaylorModel::getBound() const
{
  return Interval(coeffs_[0] + r_[0], coeffs_[1] + r_[1]) + time_interval_->t_ * coeffs_[1] + time_interval_->t2_ * coeffs_[2] + time_interval_->t3_ * coeffs_[3];
}

Interval TaylorModel::getTightBound(FCL_REAL t0, FCL_REAL t1) const
{
  if(t0 < time_interval_->t_[0]) t0 = time_interval_->t_[0];
  if(t1 > time_interval_->t_[1]) t1 = time_interval_->t_[1];

  if(coeffs_[3] == 0)
  {
    register FCL_REAL a = -coeffs_[1] / (2 * coeffs_[2]);
    Interval polybounds;
    if(a <= t1 && a >= t0)
    {
      FCL_REAL AQ = coeffs_[0] + a * (coeffs_[1] + a * coeffs_[2]);
      register FCL_REAL t = t0;
      FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
      t = t1;
      FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);

      FCL_REAL minQ = LQ, maxQ = RQ;
      if(LQ > RQ)
      {
        minQ = RQ;
        maxQ = LQ;
      }

      if(minQ > AQ) minQ = AQ;
      if(maxQ < AQ) maxQ = AQ;

      polybounds.setValue(minQ, maxQ);
    }
    else
    {
      register FCL_REAL t = t0;
      FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);
      t = t1;
      FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * coeffs_[2]);

      if(LQ > RQ) polybounds.setValue(RQ, LQ);
      else polybounds.setValue(LQ, RQ);
    }

    return polybounds + r_;
  }
  else
  {
    register FCL_REAL t = t0;
    FCL_REAL LQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));
    t = t1;
    FCL_REAL RQ = coeffs_[0] + t * (coeffs_[1] + t * (coeffs_[2] + t * coeffs_[3]));

    if(LQ > RQ)
    {
      FCL_REAL tmp = LQ;
      LQ = RQ;
      RQ = tmp;
    }

    // derivative: c1+2*c2*t+3*c3*t^2

    FCL_REAL delta = coeffs_[2] * coeffs_[2] - 3 * coeffs_[1] * coeffs_[3];
    if(delta < 0)
      return Interval(LQ, RQ) + r_;

    FCL_REAL r1 = (-coeffs_[2]-sqrt(delta))/(3*coeffs_[3]);
    FCL_REAL r2 = (-coeffs_[2]+sqrt(delta))/(3*coeffs_[3]);

    if(r1 <= t1 && r1 >= t0)
    {
      FCL_REAL Q = coeffs_[0] + r1 * (coeffs_[1] + r1 * (coeffs_[2] + r1 * coeffs_[3]));
      if(Q < LQ) LQ = Q;
      else if(Q > RQ) RQ = Q;
    }

    if(r2 <= t1 && r2 >= t0)
    {
      FCL_REAL Q = coeffs_[0] + r2 * (coeffs_[1] + r2 * (coeffs_[2] + r2 * coeffs_[3]));
      if(Q < LQ) LQ = Q;
      else if(Q > RQ) RQ = Q;
    }

    return Interval(LQ, RQ) + r_;
  }
}

Interval TaylorModel::getTightBound() const
{
  return getTightBound(time_interval_->t_[0], time_interval_->t_[1]);
}

void TaylorModel::setZero()
{
  coeffs_[0] = coeffs_[1] = coeffs_[2] = coeffs_[3] = 0;
  r_.setValue(0);
}

TaylorModel operator * (FCL_REAL d, const TaylorModel& a)
{
  TaylorModel res(a);
  res.coeff(0) *= d;
  res.coeff(1) *= d;
  res.coeff(2) *= d;
  res.coeff(3) *= d;
  res.remainder() *= d;
  return res;
}

TaylorModel operator + (FCL_REAL d, const TaylorModel& a)
{
  return a + d;
}

TaylorModel operator - (FCL_REAL d, const TaylorModel& a)
{
  return -a + d;
}

void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
{
  FCL_REAL a = tm.getTimeInterval()->t_.center();
  FCL_REAL t = w * a + q0;
  FCL_REAL w2 = w * w;
  FCL_REAL fa = cos(t);
  FCL_REAL fda = -w*sin(t);
  FCL_REAL fdda = -w2*fa;
  FCL_REAL fddda = -w2*fda;

  tm.coeff(0) = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda));
  tm.coeff(1) = fda-a*fdda+0.5*a*a*fddda;
  tm.coeff(2) = 0.5*(fdda-a*fddda);
  tm.coeff(3) = 1.0/6.0*fddda;

  // compute bounds for w^3 cos(wt+q0)/16, t \in [t0, t1]
  Interval fddddBounds;
  if(w == 0) fddddBounds.setValue(0);
  else
  {
    FCL_REAL cosQL = cos(tm.getTimeInterval()->t_[0] * w + q0);
    FCL_REAL cosQR = cos(tm.getTimeInterval()->t_[1] * w + q0);

    if(cosQL < cosQR) fddddBounds.setValue(cosQL, cosQR);
    else fddddBounds.setValue(cosQR, cosQL);

    // enlarge to handle round-off errors
    fddddBounds[0] -= 1e-15;
    fddddBounds[1] += 1e-15;

    // cos reaches maximum if there exists an integer k in [(w*t0+q0)/2pi, (w*t1+q0)/2pi];
    // cos reaches minimum if there exists an integer k in [(w*t0+q0-pi)/2pi, (w*t1+q0-pi)/2pi]

    FCL_REAL k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>());
    FCL_REAL k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>());

    if(w > 0)
    {
      if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1;
      k1 -= 0.5;
      k2 -= 0.5;
      if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1;
    }
    else
    {
      if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1;
      k1 -= 0.5;
      k2 -= 0.5;
      if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1;
    }
  }

  FCL_REAL w4 = w2 * w2;
  fddddBounds *= w4;

  FCL_REAL midSize = 0.5 * (tm.getTimeInterval()->t_[1] - tm.getTimeInterval()->t_[0]);
  FCL_REAL midSize2 = midSize * midSize;
  FCL_REAL midSize4 = midSize2 * midSize2;

  // [0, midSize4] * fdddBounds
  if(fddddBounds[0] > 0)
    tm.remainder().setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24));
  else if(fddddBounds[0] < 0)
    tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0);
  else
    tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24));
}

void generateTaylorModelForSinFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0)
{
  FCL_REAL a = tm.getTimeInterval()->t_.center();
  FCL_REAL t = w * a + q0;
  FCL_REAL w2 = w * w;
  FCL_REAL fa = sin(t);
  FCL_REAL fda = w*cos(t);
  FCL_REAL fdda = -w2*fa;
  FCL_REAL fddda = -w2*fda;

  tm.coeff(0) = fa-a*(fda-0.5*a*(fdda-1.0/3.0*a*fddda));
  tm.coeff(1) = fda-a*fdda+0.5*a*a*fddda;
  tm.coeff(2) = 0.5*(fdda-a*fddda);
  tm.coeff(3) = 1.0/6.0*fddda;

  // compute bounds for w^3 sin(wt+q0)/16, t \in [t0, t1]

  Interval fddddBounds;

  if(w == 0) fddddBounds.setValue(0);
  else
  {
    FCL_REAL sinQL = sin(w * tm.getTimeInterval()->t_[0] + q0);
    FCL_REAL sinQR = sin(w * tm.getTimeInterval()->t_[1] + q0);

    if(sinQL < sinQR) fddddBounds.setValue(sinQL, sinQR);
    else fddddBounds.setValue(sinQR, sinQL);

    // enlarge to handle round-off errors
    fddddBounds[0] -= 1e-15;
    fddddBounds[1] += 1e-15;

    // sin reaches maximum if there exists an integer k in [(w*t0+q0-pi/2)/2pi, (w*t1+q0-pi/2)/2pi];
    // sin reaches minimum if there exists an integer k in [(w*t0+q0-pi-pi/2)/2pi, (w*t1+q0-pi-pi/2)/2pi]

    FCL_REAL k1 = (tm.getTimeInterval()->t_[0] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>()) - 0.25;
    FCL_REAL k2 = (tm.getTimeInterval()->t_[1] * w + q0) / (2 * boost::math::constants::pi<FCL_REAL>()) - 0.25;

    if(w > 0)
    {
      if(ceil(k2) - floor(k1) > 1) fddddBounds[1] = 1;
      k1 -= 0.5;
      k2 -= 0.5;
      if(ceil(k2) - floor(k1) > 1) fddddBounds[0] = -1;
    }
    else
    {
      if(ceil(k1) - floor(k2) > 1) fddddBounds[1] = 1;
      k1 -= 0.5;
      k2 -= 0.5;
      if(ceil(k1) - floor(k2) > 1) fddddBounds[0] = -1;
    }

    FCL_REAL w4 = w2 * w2;
    fddddBounds *= w4;

    FCL_REAL midSize = 0.5 * (tm.getTimeInterval()->t_[1] - tm.getTimeInterval()->t_[0]);
    FCL_REAL midSize2 = midSize * midSize;
    FCL_REAL midSize4 = midSize2 * midSize2;

    // [0, midSize4] * fdddBounds
    if(fddddBounds[0] > 0)
      tm.remainder().setValue(0, fddddBounds[1] * midSize4 * (1.0 / 24));
    else if(fddddBounds[0] < 0)
      tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), 0);
    else
      tm.remainder().setValue(fddddBounds[0] * midSize4 * (1.0 / 24), fddddBounds[1] * midSize4 * (1.0 / 24));
  }
}

void generateTaylorModelForLinearFunc(TaylorModel& tm, FCL_REAL p, FCL_REAL v)
{
  tm.coeff(0) = p;
  tm.coeff(1) = v;
  tm.coeff(2) = 0;
  tm.coeff(3) = 0;
  tm.remainder()[0] = 0;
  tm.remainder()[1] = 0;
}

}


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

#include "libmv/simple_pipeline/keyframe_selection.h"

#include "libmv/numeric/numeric.h"
#include "ceres/ceres.h"
#include "libmv/logging/logging.h"
#include "libmv/multiview/homography.h"
#include "libmv/multiview/fundamental.h"
#include "libmv/simple_pipeline/intersect.h"
#include "libmv/simple_pipeline/bundle.h"

#include <Eigen/Eigenvalues>

namespace libmv {
namespace {

Vec2 NorrmalizedToPixelSpace(const Vec2 &vec,
                             const CameraIntrinsics &intrinsics) {
  Vec2 result;

  double focal_length_x = intrinsics.focal_length_x();
  double focal_length_y = intrinsics.focal_length_y();

  double principal_point_x = intrinsics.principal_point_x();
  double principal_point_y = intrinsics.principal_point_y();

  result(0) = vec(0) * focal_length_x + principal_point_x;
  result(1) = vec(1) * focal_length_y + principal_point_y;

  return result;
}

Mat3 IntrinsicsNormalizationMatrix(const CameraIntrinsics &intrinsics) {
  Mat3 T = Mat3::Identity(), S = Mat3::Identity();

  T(0, 2) = -intrinsics.principal_point_x();
  T(1, 2) = -intrinsics.principal_point_y();

  S(0, 0) /= intrinsics.focal_length_x();
  S(1, 1) /= intrinsics.focal_length_y();

  return S * T;
}

class HomographySymmetricGeometricCostFunctor {
 public:
  HomographySymmetricGeometricCostFunctor(const Vec2 &x,
                                          const Vec2 &y)
      : x_(x), y_(y) { }

  template<typename T>
  bool operator()(const T *homography_parameters, T *residuals) const {
    typedef Eigen::Matrix<T, 3, 3> Mat3;
    typedef Eigen::Matrix<T, 3, 1> Vec3;

    Mat3 H(homography_parameters);

    Vec3 x(T(x_(0)), T(x_(1)), T(1.0));
    Vec3 y(T(y_(0)), T(y_(1)), T(1.0));

    Vec3 H_x = H * x;
    Vec3 Hinv_y = H.inverse() * y;

    H_x /= H_x(2);
    Hinv_y /= Hinv_y(2);

    residuals[0] = H_x(0) - T(y_(0));
    residuals[1] = H_x(1) - T(y_(1));

    residuals[2] = Hinv_y(0) - T(x_(0));
    residuals[3] = Hinv_y(1) - T(x_(1));

    return true;
  }

  const Vec2 x_;
  const Vec2 y_;
};

void ComputeHomographyFromCorrespondences(const Mat &x1, const Mat &x2,
                                          CameraIntrinsics &intrinsics,
                                          Mat3 *H) {
  // Algebraic homography estimation, happens with normalized coordinates
  Homography2DFromCorrespondencesLinear(x1, x2, H, 1e-12);

  // Refine matrix using Ceres minimizer

  // TODO(sergey): look into refinement in pixel space.
  ceres::Problem problem;

  for (int i = 0; i < x1.cols(); i++) {
    HomographySymmetricGeometricCostFunctor
        *homography_symmetric_geometric_cost_function =
            new HomographySymmetricGeometricCostFunctor(x1.col(i),
                                                        x2.col(i));

    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<
            HomographySymmetricGeometricCostFunctor,
            4, /* num_residuals */
            9>(homography_symmetric_geometric_cost_function),
        NULL,
        H->data());
  }

  // Configure the solve.
  ceres::Solver::Options solver_options;
  solver_options.linear_solver_type = ceres::DENSE_QR;
  solver_options.max_num_iterations = 50;
  solver_options.update_state_every_iteration = true;
  solver_options.parameter_tolerance = 1e-16;
  solver_options.function_tolerance = 1e-16;

  // Run the solve.
  ceres::Solver::Summary summary;
  ceres::Solve(solver_options, &problem, &summary);

  VLOG(1) << "Summary:\n" << summary.FullReport();

  // Convert homography to original pixel space
  Mat3 N = IntrinsicsNormalizationMatrix(intrinsics);
  *H = N.inverse() * (*H) * N;
}

class FundamentalSymmetricEpipolarCostFunctor {
 public:
  FundamentalSymmetricEpipolarCostFunctor(const Vec2 &x,
                                          const Vec2 &y)
    : x_(x), y_(y) {}

  template<typename T>
  bool operator()(const T *fundamental_parameters, T *residuals) const {
    typedef Eigen::Matrix<T, 3, 3> Mat3;
    typedef Eigen::Matrix<T, 3, 1> Vec3;

    Mat3 F(fundamental_parameters);

    Vec3 x(T(x_(0)), T(x_(1)), T(1.0));
    Vec3 y(T(y_(0)), T(y_(1)), T(1.0));

    Vec3 F_x = F * x;
    Vec3 Ft_y = F.transpose() * y;
    T y_F_x = y.dot(F_x);

    residuals[0] = y_F_x * T(1) / F_x.head(2).norm();
    residuals[1] = y_F_x * T(1) / Ft_y.head(2).norm();

    return true;
  }

  const Mat x_;
  const Mat y_;
};

void ComputeFundamentalFromCorrespondences(const Mat &x1, const Mat &x2,
                                           CameraIntrinsics &intrinsics,
                                           Mat3 *F) {
  // Algebraic fundamental estimation, happens with normalized coordinates
  NormalizedEightPointSolver(x1, x2, F);

  // Refine matrix using Ceres minimizer

  // TODO(sergey): look into refinement in pixel space.
  ceres::Problem problem;

  for (int i = 0; i < x1.cols(); i++) {
    FundamentalSymmetricEpipolarCostFunctor
        *fundamental_symmetric_epipolar_cost_function =
            new FundamentalSymmetricEpipolarCostFunctor(x1.col(i),
                                                        x2.col(i));

    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<
            FundamentalSymmetricEpipolarCostFunctor,
            2, /* num_residuals */
            9>(fundamental_symmetric_epipolar_cost_function),
        NULL,
        F->data());
  }

  // Configure the solve.
  ceres::Solver::Options solver_options;
  solver_options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
  solver_options.max_num_iterations = 50;
  solver_options.update_state_every_iteration = true;
  solver_options.parameter_tolerance = 1e-16;
  solver_options.function_tolerance = 1e-16;

  // Run the solve.
  ceres::Solver::Summary summary;
  ceres::Solve(solver_options, &problem, &summary);

  VLOG(1) << "Summary:\n" << summary.FullReport();

  // Convert fundamental to original pixel space
  Mat3 N = IntrinsicsNormalizationMatrix(intrinsics);
  *F = N.inverse() * (*F) * N;
}

// P.H.S. Torr
// Geometric Motion Segmentation and Model Selection
//
// http://reference.kfupm.edu.sa/content/g/e/geometric_motion_segmentation_and_model__126445.pdf
//
// d is the number of dimensions modeled
//     (d = 3 for a fundamental matrix or 2 for a homography)
// k is the number of degrees of freedom in the model
//     (k = 7 for a fundamental matrix or 8 for a homography)
// r is the dimension of the data
//     (r = 4 for 2D correspondences between two frames)
double GRIC(const Vec &e, int d, int k, int r) {
  int n = e.rows();
  double lambda1 = log(static_cast<double>(r));
  double lambda2 = log(static_cast<double>(r * n));

  // lambda3 limits the residual error, and this paper
  // http://elvera.nue.tu-berlin.de/files/0990Knorr2006.pdf
  // suggests using lambda3 of 2
  // same value is used in Torr's Problem of degeneracy in structure
  // and motion recovery from uncalibrated image sequences
  // http://www.robots.ox.ac.uk/~vgg/publications/papers/torr99.ps.gz
  double lambda3 = 2.0;

  // measurement error of tracker
  double sigma2 = 0.01;

  // Actual GRIC computation
  double gric_result = 0.0;

  for (int i = 0; i < n; i++) {
    double rho = std::min(e(i) * e(i) / sigma2, lambda3 * (r - d));
    gric_result += rho;
  }

  gric_result += lambda1 * d * n;
  gric_result += lambda2 * k;

  return gric_result;
}

// Compute a generalized inverse using eigen value decomposition.
// It'll actually also zero 7 last eigen values to deal with
// gauges, since this function is used to compute variance of
// reconstructed 3D points.
//
// TODO(sergey): Could be generalized by making it so number
//               of values to be zeroed is passed by an argument
//               and moved to numeric module.
Mat pseudoInverse(const Mat &matrix) {
  Eigen::EigenSolver<Mat> eigenSolver(matrix);
  Mat D = eigenSolver.pseudoEigenvalueMatrix();
  Mat V = eigenSolver.pseudoEigenvectors();

  double epsilon = std::numeric_limits<double>::epsilon();

  for (int i = 0; i < D.cols(); ++i) {
    if (D(i, i) > epsilon)
      D(i, i) = 1.0 / D(i, i);
    else
      D(i, i) = 0.0;
  }

  // Zero last 7 (which corresponds to smallest eigen values).
  // 7 equals to the number of gauge freedoms.
  for (int i = D.cols() - 7; i < D.cols(); ++i)
    D(i, i) = 0.0;

  return V * D * V.inverse();
}
}  // namespace

void SelectkeyframesBasedOnGRICAndVariance(const Tracks &tracks,
                                           CameraIntrinsics &intrinsics,
                                           vector<int> &keyframes) {
  // Mirza Tahir Ahmed, Matthew N. Dailey
  // Robust key frame extraction for 3D reconstruction from video streams
  //
  // http://www.cs.ait.ac.th/~mdailey/papers/Tahir-KeyFrame.pdf

  int max_image = tracks.MaxImage();
  int next_keyframe = 1;
  int number_keyframes = 0;

  // Limit correspondence ratio from both sides.
  // On the one hand if number of correspondent features is too low,
  // triangulation will suffer.
  // On the other hand high correspondence likely means short baseline.
  // which also will affect om accuracy
  const double Tmin = 0.8;
  const double Tmax = 1.0;

  Mat3 N = IntrinsicsNormalizationMatrix(intrinsics);
  Mat3 N_inverse = N.inverse();

  double Sc_best = std::numeric_limits<double>::max();
  double success_intersects_factor_best = 0.0f;

  while (next_keyframe != -1) {
    int current_keyframe = next_keyframe;
    double Sc_best_candidate = std::numeric_limits<double>::max();

    LG << "Found keyframe " << next_keyframe;

    number_keyframes++;
    next_keyframe = -1;

    for (int candidate_image = current_keyframe + 1;
         candidate_image <= max_image;
         candidate_image++) {
      // Conjunction of all markers from both keyframes
      vector<Marker> all_markers =
        tracks.MarkersInBothImages(current_keyframe, candidate_image);

      // Match keypoints between frames current_keyframe and candidate_image
      vector<Marker> tracked_markers =
        tracks.MarkersForTracksInBothImages(current_keyframe, candidate_image);

      // Correspondences in normalized space
      Mat x1, x2;
      CoordinatesForMarkersInImage(tracked_markers, current_keyframe, &x1);
      CoordinatesForMarkersInImage(tracked_markers, candidate_image, &x2);

      LG << "Found " << x1.cols()
         << " correspondences between " << current_keyframe
         << " and " << candidate_image;

      // Not enough points to construct fundamental matrix
      if (x1.cols() < 8 || x2.cols() < 8)
        continue;

      // STEP 1: Correspondence ratio constraint
      int Tc = tracked_markers.size();
      int Tf = all_markers.size();
      double Rc = static_cast<double>(Tc) / Tf;

      LG << "Correspondence between " << current_keyframe
         << " and " << candidate_image
         << ": " << Rc;

      if (Rc < Tmin || Rc > Tmax)
        continue;

      Mat3 H, F;
      ComputeHomographyFromCorrespondences(x1, x2, intrinsics, &H);
      ComputeFundamentalFromCorrespondences(x1, x2, intrinsics, &F);

      // TODO(sergey): STEP 2: Discard outlier matches

      // STEP 3: Geometric Robust Information Criteria

      // Compute error values for homography and fundamental matrices
      Vec H_e, F_e;
      H_e.resize(x1.cols());
      F_e.resize(x1.cols());
      for (int i = 0; i < x1.cols(); i++) {
        Vec2 current_x1 =
          NorrmalizedToPixelSpace(Vec2(x1(0, i), x1(1, i)), intrinsics);
        Vec2 current_x2 =
          NorrmalizedToPixelSpace(Vec2(x2(0, i), x2(1, i)), intrinsics);

        H_e(i) = SymmetricGeometricDistance(H, current_x1, current_x2);
        F_e(i) = SymmetricEpipolarDistance(F, current_x1, current_x2);
      }

      LG << "H_e: " << H_e.transpose();
      LG << "F_e: " << F_e.transpose();

      // Degeneracy constraint
      double GRIC_H = GRIC(H_e, 2, 8, 4);
      double GRIC_F = GRIC(F_e, 3, 7, 4);

      LG << "GRIC values for frames " << current_keyframe
         << " and " << candidate_image
         << ", H-GRIC: " << GRIC_H
         << ", F-GRIC: " << GRIC_F;

      if (GRIC_H <= GRIC_F)
        continue;

      // TODO(sergey): STEP 4: PELC criterion

      // STEP 5: Estimation of reconstruction error
      //
      // Uses paper Keyframe Selection for Camera Motion and Structure
      // Estimation from Multiple Views
      // Uses ftp://ftp.tnt.uni-hannover.de/pub/papers/2004/ECCV2004-TTHBAW.pdf
      // Basically, equation (15)
      //
      // TODO(sergey): separate all the constraints into functions,
      //               this one is getting to much cluttered already

      // Definitions in equation (15):
      // - I is the number of 3D feature points
      // - A is the number of essential parameters of one camera

      EuclideanReconstruction reconstruction;

      // The F matrix should be an E matrix, but squash it just to be sure

      // Reconstruction should happen using normalized fundamental matrix
      Mat3 F_normal = N * F * N_inverse;

      Mat3 E;
      FundamentalToEssential(F_normal, &E);

      // Recover motion between the two images. Since this function assumes a
      // calibrated camera, use the identity for K
      Mat3 R;
      Vec3 t;
      Mat3 K = Mat3::Identity();

      if (!MotionFromEssentialAndCorrespondence(E,
                                                K, x1.col(0),
                                                K, x2.col(0),
                                                &R, &t)) {
        LG << "Failed to compute R and t from E and K";
        continue;
      }

      LG << "Camera transform between frames " << current_keyframe
         << " and " << candidate_image
         << ":\nR:\n" << R
         << "\nt:" << t.transpose();

      // First camera is identity, second one is relative to it
      reconstruction.InsertCamera(current_keyframe,
                                  Mat3::Identity(),
                                  Vec3::Zero());
      reconstruction.InsertCamera(candidate_image, R, t);

      // Reconstruct 3D points
      int intersects_total = 0, intersects_success = 0;
      for (int i = 0; i < tracked_markers.size(); i++) {
        if (!reconstruction.PointForTrack(tracked_markers[i].track)) {
          vector<Marker> reconstructed_markers;

          int track = tracked_markers[i].track;

          reconstructed_markers.push_back(tracked_markers[i]);

          // We know there're always only two markers for a track
          // Also, we're using brute-force search because we don't
          // actually know about markers layout in a list, but
          // at this moment this cycle will run just once, which
          // is not so big deal

          for (int j = i + 1; j < tracked_markers.size(); j++) {
            if (tracked_markers[j].track == track) {
              reconstructed_markers.push_back(tracked_markers[j]);
              break;
            }
          }

          intersects_total++;

          if (EuclideanIntersect(reconstructed_markers, &reconstruction)) {
            LG << "Ran Intersect() for track " << track;
            intersects_success++;
          } else {
            LG << "Filed to intersect track " << track;
          }
        }
      }

      double success_intersects_factor =
          (double) intersects_success / intersects_total;

      if (success_intersects_factor < success_intersects_factor_best) {
        LG << "Skip keyframe candidate because of "
              "lower successful intersections ratio";

        continue;
      }

      success_intersects_factor_best = success_intersects_factor;

      Tracks two_frames_tracks(tracked_markers);
      CameraIntrinsics empty_intrinsics;
      BundleEvaluation evaluation;
      evaluation.evaluate_jacobian = true;

      EuclideanBundleCommonIntrinsics(two_frames_tracks,
                                      BUNDLE_NO_INTRINSICS,
                                      BUNDLE_NO_CONSTRAINTS,
                                      &reconstruction,
                                      &empty_intrinsics,
                                      &evaluation);

      Mat &jacobian = evaluation.jacobian;

      Mat JT_J = jacobian.transpose() * jacobian;
      Mat JT_J_inv = pseudoInverse(JT_J);

      Mat temp_derived = JT_J * JT_J_inv * JT_J;
      bool is_inversed = (temp_derived - JT_J).cwiseAbs2().sum() <
          1e-4 * std::min(temp_derived.cwiseAbs2().sum(),
                          JT_J.cwiseAbs2().sum());

      LG << "Check on inversed: " << (is_inversed ? "true" : "false" )
         << ", det(JT_J): " << JT_J.determinant();

      if (!is_inversed) {
        LG << "Ignoring candidature due to poor jacobian stability";
        continue;
      }

      Mat Sigma_P;
      Sigma_P = JT_J_inv.bottomRightCorner(evaluation.num_points * 3,
                                           evaluation.num_points * 3);

      int I = evaluation.num_points;
      int A = 12;

      double Sc = static_cast<double>(I + A) / Square(3 * I) * Sigma_P.trace();

      LG << "Expected estimation error between "
         << current_keyframe << " and "
         << candidate_image << ": " << Sc;

      // Pairing with a lower Sc indicates a better choice
      if (Sc > Sc_best_candidate)
        continue;

      Sc_best_candidate = Sc;

      next_keyframe = candidate_image;
    }

    // This is a bit arbitrary and main reason of having this is to deal
    // better with situations when there's no keyframes were found for
    // current keyframe this could happen when there's no so much parallax
    // in the beginning of image sequence and then most of features are
    // getting occluded. In this case there could be good keyframe pair in
    // the middle of the sequence
    //
    // However, it's just quick hack and smarter way to do this would be nice
    if (next_keyframe == -1) {
      next_keyframe = current_keyframe + 10;
      number_keyframes = 0;

      if (next_keyframe >= max_image)
        break;

      LG << "Starting searching for keyframes starting from " << next_keyframe;
    } else {
      // New pair's expected reconstruction error is lower
      // than existing pair's one.
      //
      // For now let's store just one candidate, easy to
      // store more candidates but needs some thoughts
      // how to choose best one automatically from them
      // (or allow user to choose pair manually).
      if (Sc_best > Sc_best_candidate) {
        keyframes.clear();
        keyframes.push_back(current_keyframe);
        keyframes.push_back(next_keyframe);
        Sc_best = Sc_best_candidate;
      }
    }
  }
}

}  // namespace libmv


// Boost.Geometry

// Copyright (c) 2018 Yaghyavardhan Singh Khangarot, Hyderabad, India.
// Contributed and/or modified by Yaghyavardhan Singh Khangarot,
//   as part of Google Summer of Code 2018 program.

// This file was modified by Oracle on 2018.
// Modifications copyright (c) 2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_TEST_HAUSDORFF_DISTANCE_HPP
#define BOOST_GEOMETRY_TEST_HAUSDORFF_DISTANCE_HPP

#include <geometry_test_common.hpp>
#include <boost/geometry/algorithms/discrete_hausdorff_distance.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/variant/variant.hpp>

template <typename Geometry1, typename Geometry2, typename Expected>
void test_hausdorff_distance(Geometry1 const& geometry1, Geometry2 const& geometry2,
                             Expected const& expected_hausdorff_distance)
{
    typedef typename bg::distance_result
        <
            typename bg::point_type<Geometry1>::type,
            typename bg::point_type<Geometry2>::type
        >::type result_type;

    result_type h_distance = bg::discrete_hausdorff_distance(geometry1, geometry2);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::ostringstream out;
    out << typeid(typename bg::coordinate_type<Geometry1>::type).name()
        << std::endl
        << typeid(typename bg::coordinate_type<Geometry2>::type).name()
        << std::endl
        << typeid(h_distance).name()
        << std::endl
        << "hausdorff_distance : " << h_distance
        << std::endl;
    std::cout << out.str();
#endif

    BOOST_CHECK_CLOSE(h_distance, result_type(expected_hausdorff_distance), 0.01);
}

template <typename Geometry1, typename Geometry2, typename Expected>
void test_geometry(std::string const& wkt1, std::string const& wkt2,
                   Expected const& expected_hausdorff_distance)
{
    Geometry1 geometry1;
    bg::read_wkt(wkt1, geometry1);
    Geometry2 geometry2;
    bg::read_wkt(wkt2, geometry2);

    test_hausdorff_distance(geometry1, geometry2, expected_hausdorff_distance);

#if defined(BOOST_GEOMETRY_TEST_DEBUG)
    test_hausdorff_distance(boost::variant<Geometry1>(geometry1),
                            boost::variant<Geometry2>(geometry2),
                            expected_hausdorff_distance);
#endif
}

template <typename Geometry1, typename Geometry2, typename Strategy, typename Expected>
void test_hausdorff_distance(Geometry1 const& geometry1, Geometry2 const& geometry2,
                             Strategy strategy, Expected const& expected_hausdorff_distance)
{
    typedef typename bg::distance_result
        <
            typename bg::point_type<Geometry1>::type,
            typename bg::point_type<Geometry2>::type,
            Strategy
        >::type result_type;

    result_type h_distance = bg::discrete_hausdorff_distance(geometry1, geometry2, strategy);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::ostringstream out;
    out << typeid(typename bg::coordinate_type<Geometry1>::type).name()
        << std::endl
        << typeid(typename bg::coordinate_type<Geometry2>::type).name()
        << std::endl
        << typeid(h_distance).name()
        << std::endl
        << "hausdorff_distance : " << h_distance
        << std::endl;
    std::cout << out.str();
#endif

    BOOST_CHECK_CLOSE(h_distance, result_type(expected_hausdorff_distance), 0.01);
}

template <typename Geometry1, typename Geometry2, typename Strategy, typename Expected>
void test_geometry(std::string const& wkt1, std::string const& wkt2,
                   Strategy strategy, Expected const& expected_hausdorff_distance)
{
    Geometry1 geometry1;
    bg::read_wkt(wkt1, geometry1);
    Geometry2 geometry2;
    bg::read_wkt(wkt2, geometry2);

    test_hausdorff_distance(geometry1, geometry2, strategy, expected_hausdorff_distance);

#if defined(BOOST_GEOMETRY_TEST_DEBUG)
    test_hausdorff_distance(boost::variant<Geometry1>(geometry1),
                            boost::variant<Geometry2>(geometry2),
                            strategy, expected_hausdorff_distance);
#endif
}

template <typename Geometry1,typename Geometry2>
void test_empty_input(Geometry1 const& geometry1, Geometry2 const& geometry2)
{
    try
    {
        bg::discrete_hausdorff_distance(geometry1, geometry2);
    }
    catch(bg::empty_input_exception const& )
    {
        return;
    }
    BOOST_CHECK_MESSAGE(false, "A empty_input_exception should have been thrown");
}

#endif


#ifdef _OPENMP
#include <omp.h>
#endif
#include <Python.h>
#include <numpy/ndarrayobject.h>
#include <iostream>
#include <boost/assert.hpp>
#include <boost/python.hpp>
#include <kgraph.h>
#include <kgraph-data.h>

#ifdef USE_BLAS
extern "C" {
    enum CBLAS_ORDER {CblasRowMajor=101, CblasColMajor=102};
    enum CBLAS_TRANSPOSE {CblasNoTrans=111, CblasTrans=112, CblasConjTrans=113};
    void cblas_sgemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA,
                 const enum CBLAS_TRANSPOSE TransB, const int M, const int N,
                 const int K, const float alpha, const float *A,
                 const int lda, const float *B, const int ldb,
                 const float beta, float *C, const int ldc);
    void cblas_dgemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA,
                 const enum CBLAS_TRANSPOSE TransB, const int M, const int N,
                 const int K, const double alpha, const double *A,
                 const int lda, const double *B, const int ldb,
                 const double beta, double *C, const int ldc);
}
#endif

using namespace std;
namespace python = boost::python;

/// Oracle for matrix data.
/** DATA_TYPE can be Matrix or MatrixProxy,
* DIST_TYPE should be one class within the namespace kgraph.metric.
*/
namespace kgraph {

#ifdef USE_BLAS
    void blas_prod (MatrixProxy<float> const &p1, float const *p2, int n2, int l2, Matrix<float> *r) {
        r->zero();
        cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, p1.size(), n2, p1.dim(),
                    -2.0, p1[0], p1[1]-p1[0], p2, l2, 0, (*r)[0], (*r)[1]-(*r)[0]);
    }

    void blas_prod (MatrixProxy<double> const &p1, double const *p2, int n2, int l2, Matrix<double> *r) {
        r->zero();
        cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, p1.size(), n2, p1.dim(),
                    -2.0, p1[0], p1[1]-p1[0], p2, l2, 0, (*r)[0], (*r)[1]-(*r)[0]);
    }
#endif

template <typename DATA_TYPE>
class MatrixOracleL2SQR: public kgraph::IndexOracle {
    MatrixProxy<DATA_TYPE> proxy;
    vector<DATA_TYPE> n2;
public:
    class SearchOracle: public kgraph::SearchOracle {
        MatrixProxy<DATA_TYPE> proxy;
        DATA_TYPE const *proxy_n2;
        DATA_TYPE const *query;
        DATA_TYPE n2;
    public:
        SearchOracle (MatrixProxy<DATA_TYPE> const &p, DATA_TYPE const *p_n2, DATA_TYPE const *q): proxy(p), proxy_n2(p_n2), query(q) {
            n2 = kgraph::metric::l2sqr::norm2(q, proxy.dim());
        }
        virtual unsigned size () const {
            return proxy.size();
        }
        virtual float operator () (unsigned i) const {
            return proxy_n2[i] + n2 - 2 * kgraph::metric::l2sqr::dot(proxy[i], query, proxy.dim());
        }
    };
    template <typename MATRIX_TYPE>
    MatrixOracleL2SQR (MATRIX_TYPE const &m): proxy(m), n2(proxy.size()) {
        for (unsigned i = 0; i < proxy.size(); ++i) {
            n2[i] = kgraph::metric::l2sqr::norm2(proxy[i], proxy.dim());
        }
    }
    virtual unsigned size () const {
        return proxy.size();
    }
    virtual float operator () (unsigned i, unsigned j) const {
        return n2[i] + n2[j] - 2 * kgraph::metric::l2sqr::dot(proxy[i], proxy[j], proxy.dim());
    }
    SearchOracle query (DATA_TYPE const *query) const {
        return SearchOracle(proxy, &n2[0], query);
    }

#ifdef USE_BLAS
    template <typename MATRIX_TYPE>
    void blasQuery (MATRIX_TYPE const &q, unsigned K, float epsilon,
            kgraph::MatrixProxy<unsigned, 1> *ids, kgraph::MatrixProxy<float, 1> *dists) {
        BOOST_VERIFY(dists);

        static unsigned constexpr BLOCK_SIZE = 1024;
        MatrixProxy<DATA_TYPE> q_proxy(q);
        unsigned block = BLOCK_SIZE;
        if (block < K) {
            block = K;
        }
        if (block >= proxy.size()) {
            block = proxy.size();
        }
        BOOST_VERIFY(block >= K);
        Matrix<DATA_TYPE> dot(q_proxy.size(), block);

        vector<float> qn2s(q_proxy.size());
        for (unsigned i = 0; i < q_proxy.size(); ++i) {
            qn2s[i] = kgraph::metric::l2sqr::norm2(q_proxy[i], q_proxy.dim());
        }

        unsigned begin = 0; // divide all data into blocks
        while (begin < proxy.size()) {
            unsigned end = begin + block;
            if (end > proxy.size()) {
                end = proxy.size();
            }
            blas_prod(q_proxy, proxy[begin], end-begin, proxy[1]-proxy[0], &dot);
            // do one block
            if (begin == 0) {
                // first block
#pragma omp parallel for
                for (unsigned i = 0; i < q_proxy.size(); ++i) {
                    DATA_TYPE *row = dot[i];
                    DATA_TYPE qn2 = qn2s[i];
                    vector<pair<DATA_TYPE, unsigned>> rank(end-begin);
                    for (unsigned j = 0; j < rank.size(); ++j) {
                        rank[j] = std::make_pair(qn2 + n2[j] + row[j], j);
                    }
                    std::sort(rank.begin(), rank.end());
                    unsigned *pid = (*ids)[i];
                    float *pdist = (*dists)[i];
                    for (unsigned j = 0; j < K; ++j) {
                        pid[j] = rank[j].second;
                        pdist[j] = rank[j].first;
                    }
                }
            }
            else { // subsequent blocks, using inserting instead of sort
#pragma omp parallel for
                for (unsigned i = 0; i < q_proxy.size(); ++i) {
                    DATA_TYPE *row = dot[i];
                    DATA_TYPE qn2 = qn2s[i];
                    unsigned *pid = (*ids)[i];
                    float *pdist = (*dists)[i];
                    for (unsigned j = 0; j < end-begin; ++j) {
                        // insert
                        unsigned id = begin + j;
                        float d = qn2 + n2[id] + row[j];
                        unsigned c = K-1;
                        if (d >= pdist[c]) continue;
                        while ((c > 0) && (d < pdist[c-1])) {
                            pid[c] = pid[c-1];
                            pdist[c] = pdist[c-1];
                            --c;
                        }
                        pid[c] = id;
                        pdist[c] = d;
                    }
                }
            }
            begin = end;
        }
    }
#endif
};

}

class KGraph {
    kgraph::KGraph *index;
    bool hasIndex;

    template <typename TYPE>
    void checkArray (python::object const &data) {
        PyArrayObject *array = reinterpret_cast<PyArrayObject *>(data.ptr());
        BOOST_VERIFY(array->nd == 2);
        //cerr << "dims: " << array->dimensions[0] << ' ' << array->dimensions[1] << endl;
        //cerr << "stride: " << array->strides[0] << ' ' << array->strides[1] << endl;
        PyArray_Descr *descr = array->descr;
        /*
        BOOST_VERIFY(descr->type_num == NPY_FLOAT);
        */
        BOOST_VERIFY(descr->elsize == sizeof(TYPE));
        //cerr << "type: " << descr->type_num << endl;
        //cerr << "size: " << descr->elsize << endl;
        //cerr << "alignment: " << descr->alignment << endl;
    }

    template <typename TYPE>
    void buildImpl (python::object const &data,
                               kgraph::KGraph::IndexParams params) {
        checkArray<TYPE>(data);
        kgraph::MatrixProxy<TYPE> dmatrix(reinterpret_cast<PyArrayObject *>(data.ptr()));
        kgraph::MatrixOracleL2SQR<TYPE> oracle(dmatrix);
        if (dmatrix.dim() % 4) {
            cerr << "Dimension has to be multiples of 4." << endl;
            BOOST_VERIFY(0);
        }
        index->build(oracle, params, NULL);
        hasIndex = true;
    }

    template <typename TYPE>
    python::object searchImpl (python::object const &data,
                               python::object const &query,
                               kgraph::KGraph::SearchParams params,
                               unsigned threads,
                               bool withDistance,
                               bool blas) {
        checkArray<TYPE>(data);
        checkArray<TYPE>(query);
        kgraph::MatrixProxy<TYPE> dmatrix(reinterpret_cast<PyArrayObject *>(data.ptr()));
        if (dmatrix.dim() % 4) {
            cerr << "Dimension has to be multiples of 4." << endl;
            BOOST_VERIFY(0);
        }
        kgraph::MatrixProxy<TYPE> qmatrix(reinterpret_cast<PyArrayObject *>(query.ptr()));
        kgraph::MatrixOracleL2SQR<TYPE> oracle(dmatrix);
        npy_intp dims[] = {qmatrix.size(), params.K};
        PyObject *result =  PyArray_SimpleNew(2, dims, NPY_UINT32);
        PyObject *distance =  PyArray_SimpleNew(2, dims, NPY_FLOAT);
        kgraph::MatrixProxy<unsigned, 1> rmatrix(reinterpret_cast<PyArrayObject *>(result));
        kgraph::MatrixProxy<float, 1> distmatrix(reinterpret_cast<PyArrayObject *>(distance));
#ifdef _OPENMP
        if (threads) ::omp_set_num_threads(threads);
#endif
#ifdef USE_BLAS
        if (blas) {
            oracle.blasQuery(qmatrix, params.K, params.epsilon, &rmatrix, &distmatrix);
        }
        else
#endif
            if (hasIndex) {
#pragma omp parallel for
            for (unsigned i = 0; i < qmatrix.size(); ++i) {
                if (withDistance) {
                    index->search(oracle.query(qmatrix[i]), params, const_cast<unsigned *>(rmatrix[i]),
                                  const_cast<float *>(distmatrix[i]),NULL);
                }
                else {
                    index->search(oracle.query(qmatrix[i]), params, const_cast<unsigned *>(rmatrix[i]), NULL);
                }
            }
        }
        else {
#pragma omp parallel for
            for (unsigned i = 0; i < qmatrix.size(); ++i) {
                if (withDistance) {
                    oracle.query(qmatrix[i]).search(params.K, params.epsilon, const_cast<unsigned *>(rmatrix[i]),
                                 const_cast<float *>(distmatrix[i]));
                }
                else {
                    oracle.query(qmatrix[i]).search(params.K, params.epsilon, const_cast<unsigned *>(rmatrix[i]), NULL);
                }
            }
        }

        if (withDistance) {
            PyObject* tup = PyTuple_New(2);
            PyTuple_SetItem(tup,0,result);
            PyTuple_SetItem(tup,1,distance);
            return python::object(python::handle<>(tup));
        }
        else {
            Py_DECREF(distance);
            return python::object(python::handle<>(result));
        }
    }

public:
    KGraph (): index(kgraph::KGraph::create()), hasIndex(false) {
        if (!index) throw runtime_error("error creating kgraph instance");
    }
    ~KGraph () {
        if (index) delete index;
    }
    void load (char const *path) {
        index->load(path);
        hasIndex = true;
    }
    void save (char const *path) const {
        index->save(path);
    }
    void build (python::object const &data,
               unsigned iterations,
               unsigned L,
               unsigned K,
               unsigned S,
               unsigned controls,
               float delta,
               float recall,
               unsigned prune) {
        kgraph::KGraph::IndexParams params;
        params.iterations = iterations;
        params.L = L;
        params.K = K;
        params.S = S;
        params.controls = controls;
        params.delta = delta;
        params.recall = recall;
        params.prune = prune;
        PyArrayObject *pd = reinterpret_cast<PyArrayObject *>(data.ptr());
        switch (pd->descr->type_num) {
            case NPY_FLOAT: buildImpl<float>(data, params); return;
            case NPY_DOUBLE: buildImpl<double>(data, params); return;
        }
        throw runtime_error("data type not supported.");
    }
    python::object search (python::object const &data,
                           python::object const &query,
                unsigned K,
                unsigned P,
                unsigned M,
                unsigned T,
                unsigned threads,
                bool withDistance,
                bool blas) {
        kgraph::KGraph::SearchParams params;
        params.K = K;
        params.P = P;
        params.M = M;
        params.T = T;
        PyArrayObject *pd = reinterpret_cast<PyArrayObject *>(data.ptr());
        PyArrayObject *pq = reinterpret_cast<PyArrayObject *>(query.ptr());
        if (pd->descr->type_num != pq->descr->type_num) throw runtime_error("data and query have different types");
        switch (pd->descr->type_num) {
            case NPY_FLOAT: return searchImpl<float>(data, query, params, threads, withDistance, blas);
            case NPY_DOUBLE: return searchImpl<double>(data, query, params, threads, withDistance, blas);
        }
        throw runtime_error("data type not supported.");
        return python::object();
    }
};

BOOST_PYTHON_MODULE(pykgraph)
{
    import_array();
    python::numeric::array::set_module_and_type("numpy", "ndarray");
    python::class_<KGraph>("KGraph")
        .def("load", &KGraph::load)
        .def("save", &KGraph::save)
        .def("build", &KGraph::build,
             (python::arg("data"),
              python::arg("iterations") = kgraph::default_iterations,
              python::arg("L") = kgraph::default_L,
              python::arg("K") = kgraph::default_K,
              python::arg("S") = kgraph::default_S,
              python::arg("controls") = kgraph::default_controls,
              python::arg("delta") = kgraph::default_delta,
              python::arg("recall") = kgraph::default_recall,
              python::arg("prune") = kgraph::default_prune))
        .def("search", &KGraph::search,
            (python::arg("data"),
             python::arg("query"),
             python::arg("K") = kgraph::default_K,
             python::arg("P") = kgraph::default_P,
             python::arg("M") = kgraph::default_M,
             python::arg("T") = kgraph::default_T,
             python::arg("threads") = 0,
             python::arg("withDistance") = false,
             python::arg("blas") = false))
        ;
}


//---------------------------------------------------------------------------//
//!
//! \file   Utility_WattDistribution.hpp
//! \author Aaron Tumulak
//! \brief  Watt distribution class declaration. Modified by Alex Robinson
//!         to accommodate units.
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_WATT_DISTRIBUTION_HPP
#define UTILITY_WATT_DISTRIBUTION_HPP

// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/units/physical_dimensions/energy.hpp>

// Trilinos Includes
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "Utility_OneDDistribution.hpp"
#include "Utility_ParameterListCompatibleObject.hpp"
#include "Utility_QuantityTraits.hpp"

namespace Utility{

/*! Watt distribution class
 * \ingroup one_d_distributions
 */
template<typename IndependentUnit, typename DependentUnit = void>
class UnitAwareWattDistribution : public UnitAwareOneDDistribution<IndependentUnit,DependentUnit>,
				  public ParameterListCompatibleObject<UnitAwareWattDistribution<IndependentUnit,DependentUnit> >
{

  // Only allow construction when the independent unit corresponds to energy
  RESTRICT_UNIT_TO_BOOST_DIMENSION( IndependentUnit, energy_dimension );

private:

  // The distribution multiplier quantity type
  typedef typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::DepQuantity DistMultiplierQuantity;

  // The distribution normalization quantity type
  typedef typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::DistNormQuantity DistNormQuantity;

  // Typedef for QuantityTraits<double>
  typedef QuantityTraits<double> QT;

  // Typedef for QuantityTraits<IndepQuantity>
  typedef QuantityTraits<typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::IndepQuantity> IQT;

  // Typedef for QuantityTraits<InverseIndepQuantity>
  typedef QuantityTraits<typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::InverseIndepQuantity> IIQT;

  // Typedef for QuantityTraits<DepQuantity>
  typedef QuantityTraits<typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::DepQuantity> DQT;

  // Typedef for QuantityTraits<DistMultiplierQuantity>
  typedef QuantityTraits<DistMultiplierQuantity> DMQT;

public:

  //! The distribution type
  typedef UnitAwareWattDistribution<IndependentUnit,DependentUnit> ThisType;

  //! The independent quantity type
  typedef typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::IndepQuantity IndepQuantity;

  //! The inverse independent quantity type
  typedef typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::InverseIndepQuantity InverseIndepQuantity;

  //! The dependent quantity type
  typedef typename UnitAwareOneDDistribution<IndependentUnit,DependentUnit>::DepQuantity DepQuantity;

  //! Default Constructor
  UnitAwareWattDistribution(
			const IndepQuantity incident_energy = IQT::one(),
			const IndepQuantity a_parameter = IQT::one(),
			const InverseIndepQuantity b_parameter = IIQT::one(),
			const IndepQuantity restriction_energy = IQT::zero(),
			const double constant_multiplier = 1.0 );

  //! Constructor
  template<typename InputIndepQuantityA,
	   typename InputIndepQuantityB,
	   typename InputInverseIndepQuantity,
	   typename InputIndepQuantityC>
  UnitAwareWattDistribution(
			    const InputIndepQuantityA incident_energy,
			    const InputIndepQuantityB a_parameter,
			    const InputInverseIndepQuantity b_parameter,
			    const InputIndepQuantityC restriction_energy,
			    const double constant_multiplier = 1.0 );

  //! Copy constructor
  template<typename InputIndepUnit, typename InputDepUnit>
  UnitAwareWattDistribution( const UnitAwareWattDistribution<InputIndepUnit,InputDepUnit>& dist_instance );

  //! Construct distribution from a unitless dist. (potentially dangerous)
  static UnitAwareWattDistribution fromUnitlessDistribution( const UnitAwareWattDistribution<void,void>& unitless_distribution );

  //! Assignment operator
  UnitAwareWattDistribution& operator=( const UnitAwareWattDistribution& dist_instance );

  //! Destructor
  ~UnitAwareWattDistribution()
  { /* ... */ }

  //! Evaluate the distribution
  DepQuantity evaluate( const IndepQuantity indep_var_value ) const;

  //! Evaluate the PDF
  InverseIndepQuantity evaluatePDF( const IndepQuantity indep_var_value ) const;

  //! Return a sample from the distribution
  IndepQuantity sample() const;

  //! Return a sample from the distribution
  static IndepQuantity sample( const IndepQuantity incident_energy,
			       const IndepQuantity a_parameter,
			       const InverseIndepQuantity b_parameter,
			       const IndepQuantity restriction_energy );

  //! Return a random sample from the distribution and record the number of trials
  IndepQuantity sampleAndRecordTrials( unsigned& trials ) const;

  //! Return a random sample from the corresponding CDF and record the number of trials
  static IndepQuantity sampleAndRecordTrials(
    const IndepQuantity incident_energy,
    const IndepQuantity a_parameter,
    const InverseIndepQuantity b_parameter,
    const IndepQuantity restriction_energy,
    unsigned& trials );

  //! Test if the distribution is continuous
  bool isContinuous() const;

  //! Return the upper bound of the distribution independent variable
  IndepQuantity getUpperBoundOfIndepVar() const;

  //! Return the lower bound of the distribution independent variable
  IndepQuantity getLowerBoundOfIndepVar() const;

  //! Return the distribution type
  OneDDistributionType getDistributionType() const;

  //! Method for placing the object in an output stream
  void toStream( std::ostream& os ) const;

  //! Method for initializing the object from an input stream
  void fromStream( std::istream& is );

  //! Method for testing if two objects are equivalent
  bool isEqual( const UnitAwareWattDistribution& other ) const;

protected:

  //! Copy constructor (copying from unitless distribution only)
  UnitAwareWattDistribution( const UnitAwareWattDistribution<void,void>& unitless_dist_instance, int );

private:

  // Calculate the normalization constant of the distribution
  void calculateNormalizationConstant();

  // All possible instantiations are friends
  template<typename FriendIndepUnit, typename FriendDepUnit>
  friend class UnitAwareWattDistribution;

  // The distribution type
  static const OneDDistributionType distribution_type = WATT_DISTRIBUTION;

  // The incident neutron energy of the distribution
  IndepQuantity d_incident_energy;

  // The a_parameter of the distribution
  IndepQuantity d_a_parameter;

  // The b_parameter of the distribution
  InverseIndepQuantity d_b_parameter;

  // The restriction energy of the distribution
  IndepQuantity d_restriction_energy;

  // The distribution multiplier
  DistMultiplierQuantity d_multiplier;

  // The distribution normalization constant
  DistNormQuantity d_norm_constant;
};

/*! The Watt distribution (unit-agnostic)
 * \ingroup one_d_distributions
 */
typedef UnitAwareWattDistribution<void,void> WattDistribution;

} // end Utility namespace

namespace Teuchos{

/*! Type name traits specialization for the Utility::WattDistribution
 *
 * \details The name function will set the type name that must be used in
 * xml files.
 */
template<>
class TypeNameTraits<Utility::WattDistribution>
{
public:
  static std::string name()
  {
    return "Watt Distribution";
  }
  static std::string concreteName(
				const Utility::WattDistribution& instance )
  {
    return name();
  }
};

/*! \brief Type name traits partial specialization for the
 * Utility::UnitAwareWattDistribution
 *
 * \details The name function will set the type name that must be used in
 * xml files.
 */
template<typename U,typename V>
class TypeNameTraits<Utility::UnitAwareWattDistribution<U,V> >
{
public:
  static std::string name()
  {
    return "Unit-Aware Watt Distribution (" +
      Utility::UnitTraits<U>::symbol() + "," +
      Utility::UnitTraits<V>::symbol() + ")";
  }
  static std::string concreteName(
		      const Utility::UnitAwareWattDistribution<U,V>& instance )
  {
    return name();
  }
};

} // end Teuchos namespace

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "Utility_WattDistribution_def.hpp"

//---------------------------------------------------------------------------//

#endif // end UTILITY_WATT_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end Utility_WattDistribution.hpp
//---------------------------------------------------------------------------//