#include "libsnark/gadgetlib1/gadgets/basic_gadgets.hpp"
#include "libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp"
#include "libsnark/common/default_types/r1cs_ppzksnark_pp.hpp"
#include "libsnark/common/utils.hpp"
#include <boost/optional.hpp>

using namespace libsnark;
using namespace std;

#include "gadget_neg.hpp"

template<typename ppzksnark_ppT>
r1cs_ppzksnark_keypair<ppzksnark_ppT> generate_keypair_neg(const int jw[2][32])
{
    typedef Fr<ppzksnark_ppT> FieldT;

    //根据预先定义的计算门和约束生成公共参数秘钥对
    //证明生成端若需要采用对应的公共秘钥生成证明数据成功，则必须使两端的数据符合预先定于的计算约束(R1+X=R2+R3)
    //如此，当验证端根据对应的验证秘钥验证证明数据为真时，验证者就能够相信对应的交易中是符合预定义的计算约束的，而不是生成假证明以通过检查
    protoboard<FieldT> pb;
    l_gadget_neg<FieldT> g(pb);
    g.generate_r1cs_constraints();
    const r1cs_constraint_system<FieldT> constraint_system = pb.get_constraint_system();

    cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;

    return r1cs_ppzksnark_generator<ppzksnark_ppT>(constraint_system);
}

template<typename ppzksnark_ppT>
boost::optional<r1cs_ppzksnark_proof<ppzksnark_ppT>> generate_proof_neg(r1cs_ppzksnark_proving_key<ppzksnark_ppT> proving_key,
                                                                   const std::vector<bit_vector> &hash_input_vec,
																	const std::vector<bit_vector> &hash_output_vec,
																	const std::vector<bit_vector> &input_vec,
																	const std::vector<bit_vector> &output_vec,
																	const bit_vector &x,
																   const int jw[2][32]
                                                                   )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    l_gadget_neg<FieldT> g(pb);
    g.generate_r1cs_constraints();
    g.generate_r1cs_witness(hash_input_vec, hash_output_vec, input_vec, output_vec, x);

    if (!pb.is_satisfied()) {
      std::cout << "System not satisfied!" << std::endl;
        return boost::none;
    }

    return r1cs_ppzksnark_prover<ppzksnark_ppT>(proving_key, pb.primary_input(), pb.auxiliary_input());
}

template<typename ppzksnark_ppT>
bool verify_proof(r1cs_ppzksnark_verification_key<ppzksnark_ppT> verification_key,
                  r1cs_ppzksnark_proof<ppzksnark_ppT> proof,
                  const std::vector<bit_vector> &hash_input_vec,
				  const std::vector<bit_vector> &hash_output_vec,
				  const bit_vector &x
                 )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    const r1cs_primary_input<FieldT> input = l_input_map<FieldT>(hash_input_vec, hash_output_vec, x);

    std::cout << "**** After l_input_map *****" << std::endl;

    return r1cs_ppzksnark_verifier_strong_IC<ppzksnark_ppT>(verification_key, input, proof);

}


// Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

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

#ifndef UUID_A61EC088D31511DFA59D2B03E0D72085
#define UUID_A61EC088D31511DFA59D2B03E0D72085

#include <boost/qvm/vec_mat_operations2.hpp>
#include <boost/qvm/vec_mat_operations3.hpp>
#include <boost/qvm/vec_mat_operations4.hpp>

namespace boost {
namespace qvm {
////////////////////////////////////////////////

namespace qvm_detail {
template <int M, int N> struct mul_mv_defined {
  static bool const value = false;
};
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    is_mat<A>::value && is_vec<B>::value &&
        mat_traits<A>::cols == vec_traits<B>::dim &&
        !qvm_detail::mul_mv_defined<mat_traits<A>::rows,
                                    mat_traits<A>::cols>::value,
    deduce_vec2<A, B, mat_traits<A>::rows>>::type
operator*(A const &a, B const &b) {
  typedef typename deduce_vec2<A, B, mat_traits<A>::rows>::type R;
  R r;
  for (int i = 0; i < mat_traits<A>::rows; ++i) {
    typedef typename vec_traits<R>::scalar_type Tr;
    Tr x(scalar_traits<Tr>::value(0));
    for (int j = 0; j < mat_traits<A>::cols; ++j)
      x += mat_traits<A>::read_element_idx(i, j, a) *
           vec_traits<B>::read_element_idx(j, b);
    vec_traits<R>::write_element_idx(i, r) = x;
  }
  return r;
}

namespace qvm_detail {
template <int M, int N> struct mul_vm_defined {
  static bool const value = false;
};
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS typename lazy_enable_if_c<
    is_vec<A>::value && is_mat<B>::value &&
        vec_traits<A>::dim == mat_traits<B>::rows &&
        !qvm_detail::mul_vm_defined<mat_traits<B>::rows,
                                    mat_traits<B>::cols>::value,
    deduce_vec2<A, B, mat_traits<B>::cols>>::type
operator*(A const &a, B const &b) {
  typedef typename deduce_vec2<A, B, mat_traits<B>::cols>::type R;
  R r;
  for (int i = 0; i < mat_traits<B>::cols; ++i) {
    typedef typename vec_traits<R>::scalar_type Tr;
    Tr x(scalar_traits<Tr>::value(0));
    for (int j = 0; j < mat_traits<B>::rows; ++j)
      x += vec_traits<A>::read_element_idx(j, a) *
           mat_traits<B>::read_element_idx(j, i, b);
    vec_traits<R>::write_element_idx(i, r) = x;
  }
  return r;
}

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

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<mat_traits<A>::rows == 4 &&
                                  mat_traits<A>::cols == 4 &&
                                  vec_traits<B>::dim == 3,
                              deduce_vec2<A, B, 3>>::type
    transform_point(A const &a, B const &b) {
  typedef typename mat_traits<A>::scalar_type Ta;
  typedef typename vec_traits<B>::scalar_type Tb;
  Ta const a00 = mat_traits<A>::template read_element<0, 0>(a);
  Ta const a01 = mat_traits<A>::template read_element<0, 1>(a);
  Ta const a02 = mat_traits<A>::template read_element<0, 2>(a);
  Ta const a03 = mat_traits<A>::template read_element<0, 3>(a);
  Ta const a10 = mat_traits<A>::template read_element<1, 0>(a);
  Ta const a11 = mat_traits<A>::template read_element<1, 1>(a);
  Ta const a12 = mat_traits<A>::template read_element<1, 2>(a);
  Ta const a13 = mat_traits<A>::template read_element<1, 3>(a);
  Ta const a20 = mat_traits<A>::template read_element<2, 0>(a);
  Ta const a21 = mat_traits<A>::template read_element<2, 1>(a);
  Ta const a22 = mat_traits<A>::template read_element<2, 2>(a);
  Ta const a23 = mat_traits<A>::template read_element<2, 3>(a);
  Tb const b0 = vec_traits<B>::template read_element<0>(b);
  Tb const b1 = vec_traits<B>::template read_element<1>(b);
  Tb const b2 = vec_traits<B>::template read_element<2>(b);
  typedef typename deduce_vec2<A, B, 3>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 3);
  R r;
  vec_traits<R>::template write_element<0>(r) =
      a00 * b0 + a01 * b1 + a02 * b2 + a03;
  vec_traits<R>::template write_element<1>(r) =
      a10 * b0 + a11 * b1 + a12 * b2 + a13;
  vec_traits<R>::template write_element<2>(r) =
      a20 * b0 + a21 * b1 + a22 * b2 + a23;
  return r;
}

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<mat_traits<A>::rows == 4 &&
                                  mat_traits<A>::cols == 4 &&
                                  vec_traits<B>::dim == 3,
                              deduce_vec2<A, B, 3>>::type
    transform_vector(A const &a, B const &b) {
  typedef typename mat_traits<A>::scalar_type Ta;
  typedef typename vec_traits<B>::scalar_type Tb;
  Ta const a00 = mat_traits<A>::template read_element<0, 0>(a);
  Ta const a01 = mat_traits<A>::template read_element<0, 1>(a);
  Ta const a02 = mat_traits<A>::template read_element<0, 2>(a);
  Ta const a10 = mat_traits<A>::template read_element<1, 0>(a);
  Ta const a11 = mat_traits<A>::template read_element<1, 1>(a);
  Ta const a12 = mat_traits<A>::template read_element<1, 2>(a);
  Ta const a20 = mat_traits<A>::template read_element<2, 0>(a);
  Ta const a21 = mat_traits<A>::template read_element<2, 1>(a);
  Ta const a22 = mat_traits<A>::template read_element<2, 2>(a);
  Tb const b0 = vec_traits<B>::template read_element<0>(b);
  Tb const b1 = vec_traits<B>::template read_element<1>(b);
  Tb const b2 = vec_traits<B>::template read_element<2>(b);
  typedef typename deduce_vec2<A, B, 3>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 3);
  R r;
  vec_traits<R>::template write_element<0>(r) = a00 * b0 + a01 * b1 + a02 * b2;
  vec_traits<R>::template write_element<1>(r) = a10 * b0 + a11 * b1 + a12 * b2;
  vec_traits<R>::template write_element<2>(r) = a20 * b0 + a21 * b1 + a22 * b2;
  return r;
}

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

namespace sfinae {
using ::boost::qvm::operator*;
using ::boost::qvm::transform_point;
using ::boost::qvm::transform_vector;
} // namespace sfinae

////////////////////////////////////////////////
} // namespace qvm
} // namespace boost

#endif


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * 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.
 *
 * 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 HOLDER 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.
 */

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

#if BOOST_VERSION < 105600
#  include <boost/geometry/multi/geometries/multi_polygon.hpp>
#else
#  include <boost/geometry/geometries/multi_polygon.hpp>
#endif

#include "dbglog/dbglog.hpp"

#include "nonconvexclip.hpp"
#include "triangulate.hpp"

namespace bg = boost::geometry;

namespace geometry {

typedef bg::model::d2::point_xy<double> Point;
typedef bg::model::polygon<Point, false, false> Polygon; // ccw, unclosed
typedef bg::model::multi_polygon<Polygon> MultiPolygon;
typedef Polygon::ring_type Ring;

namespace {

template<typename List>
std::vector<Point> bgPoints(const List &list)
{
    std::vector<Point> result;
    result.reserve(list.size() + 1);
    for (const auto &p : list) {
        result.emplace_back(p(0), p(1));
    }
    return result;
}

inline math::Points2d ringPoints(const Ring &ring)
{
    math::Points2d result;
    result.reserve(ring.size());
    for (const auto &p : ring) {
        result.emplace_back(p.x(), p.y());
    }
    return result;
}

inline double checkCcw(const math::Point2 &a, const math::Point2 &b
                       , const math::Point2 &c)
{
    return math::crossProduct(math::Point2(math::normalize(b - a))
                              , math::Point2(math::normalize(c - a)));
}

inline double area(const math::Triangle2d &t) {
    return 0.5 * std::abs(math::crossProduct( math::Point2(t[1] - t[0])
                                            , math::Point2(t[2] - t[0])));
}

} // namespace

math::Triangles3d clipTriangleNonconvex(const math::Triangle3d &tri_,
                                        const math::MultiPolygon &clipRegion)
{
    math::Triangle3d tri(tri_);

    // tri -> tri2
    math::Triangle2d tri2;
    for (int i = 0; i < 3; i++) {
        tri2[i](0) = tri[i](0);
        tri2[i](1) = tri[i](1);
    }

    bool flip = false;
    double ccw(checkCcw(tri2[0], tri2[1], tri2[2]));

    // convert clipRegion to boost MultiPolygon
    MultiPolygon clipMultiPoly;
    for (const auto &pts : clipRegion) {
        Polygon part;
        bg::assign_points(part, bgPoints(pts));
        clipMultiPoly.push_back(part);
    }

    if (std::abs(ccw) < 1e-4)
    {
        // TODO: handle exactly vertical triangles properly
        if (bg::within(Point{tri[0](0), tri[0](1)}, clipMultiPoly) &&
            bg::within(Point{tri[1](0), tri[1](1)}, clipMultiPoly) &&
            bg::within(Point{tri[2](0), tri[2](1)}, clipMultiPoly))
        {
            LOG(debug)
                << "Including near vertical triangle, all vertices lie inside.";
            return {tri};
        } else {
            LOG(debug) << "Excluding near vertical triangle, "
                          "not all vertices lie inside.";
            return {};
        }
    }

    // ensure counter-clockwise orientation for clipping
    if (ccw < 0.0) {
        std::swap(tri[1], tri[2]);
        std::swap(tri2[1], tri2[2]);
        flip = true;
    }

    // convert input to 2D polygons
    Polygon trianglePoly;
    bg::assign_points(trianglePoly, bgPoints(tri));

    // calculate intersection
    std::deque<Polygon> isect;
    bg::intersection(trianglePoly, clipMultiPoly, isect);

    // triangulate
    math::MultiPolygon isect2;
    isect2.reserve(isect.size());
    for (const auto &poly : isect) {
        isect2.push_back(ringPoints(poly.outer()));
        for (const auto &ring : poly.inners()) {
            isect2.push_back(ringPoints(ring));
        }
    }

    math::Triangles2d tris2(generalPolyTriangulate(isect2));

    // work around boost errorneously returning whole polygon as intersection
    // check area of input triangle against area of the result, should be same
    // or less. Definitelly it should not be significantly bigger.
    double interArea(0.0);
    for (const auto &t2 : tris2) {
        interArea += area(t2);
    }
    if (interArea > (area(tri2) * 1.1) ) { // 1.1 for numerical stability
        LOG(warn1) << "Throwing away spurious intersection (ratio of areas: "
                   << interArea / area(tri2) << ").";
        return {};
    }

    // restore Z coords
    math::Triangles3d tris3;
    tris3.reserve(tris2.size());
    for (const auto &t2 : tris2)
    {
        math::Triangle3d t3;
        for (int i = 0; i < 3; i++)
        {
            math::Point3 l(math::barycentricCoords(t2[i], tri2));
            t3[i](0) = t2[i](0);
            t3[i](1) = t2[i](1);
            t3[i](2) = l(0)*tri[0](2) +
                       l(1)*tri[1](2) +
                       l(2)*tri[2](2);
        }
        if (flip) {
            std::swap(t3[1], t3[2]);
        }
        tris3.push_back(t3);
    }

    return tris3;
}

math::Point3 barycentric3D(const math::Point3 &p, const math::Point3 &a,
                           const math::Point3 &b, const math::Point3 &c)
{
    typedef math::Point3 P3;
    double abp = norm_2(math::crossProduct(P3(b - a), P3(p - a)));
    double bcp = norm_2(math::crossProduct(P3(c - b), P3(p - b)));
    double cap = norm_2(math::crossProduct(P3(a - c), P3(p - c)));
    double nor = 1.0 / (abp + bcp + cap);
    return {bcp*nor, cap*nor, abp*nor};
}

std::tuple<math::Triangles3d, math::Triangles2d>
    clipTexturedTriangleNonconvex(const math::Triangle3d &tri,
                                  const math::Triangle2d &uv,
                                  const math::MultiPolygon &clipRegion)
{
    std::tuple<math::Triangles3d, math::Triangles2d> result;
    auto &tris3(std::get<0>(result));
    auto &uvs(std::get<1>(result));

    // clip the geometry first
    tris3 = clipTriangleNonconvex(tri, clipRegion);

    // if one same triangle -> copy texcoorsds
    // this clearly solves degen cases.
    if ((tris3.size() == 1) && (tris3[0] == tri)) {
        uvs.push_back(uv);
        return result;
    }

    // interpolate UV coords
    uvs.reserve(tris3.size());
    for (const auto &t3 : tris3)
    {
        math::Triangle2d t2;
        for (int i = 0; i < 3; i++)
        {
            math::Point3 l(barycentric3D(t3[i], tri[0], tri[1], tri[2]));
            for (int j = 0; j < 2; j++) {
                t2[i](j) = l(0)*uv[0](j) + l(1)*uv[1](j) + l(2)*uv[2](j);
            }
        }
        uvs.push_back(t2);
    }

    return result;
}

} // namespace geometry


#ifndef RECONST3D_CREATE_OPTIMIZER
#define RECONST3D_CREATE_OPTIMIZER

#include <Eigen/Geometry>

#include <g2o/core/block_solver.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/solver.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/robust_kernel_impl.h>

#include <opencv2/core/core.hpp>
#include <opencv2/rgbd/rgbd.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <opencv_candidate_reconst3d/reconst3d.hpp>

typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> >  G2OBlockSolver;
typedef g2o::LinearSolver< G2OBlockSolver::PoseMatrixType> G2OLinearSolver;
typedef g2o::LinearSolverCholmod<G2OBlockSolver::PoseMatrixType> G2OLinearCholmodSolver;
typedef g2o::LinearSolverCSparse<G2OBlockSolver::PoseMatrixType> G2OLinearCSparseSolver;

const std::string DEFAULT_LINEAR_SOLVER_TYPE = "cholmod"; //"csparse";
const std::string DEFAULT_NON_LINEAR_SOLVER_TYPE  = "GN";

// get norms
//
static inline
float tvecNorm(const cv::Mat& Rt)
{
    return cv::norm(Rt(cv::Rect(3,0,1,3)));
}

static inline
float rvecNormDegrees(const cv::Mat& Rt)
{
    cv::Mat rvec;
    cv::Rodrigues(Rt(cv::Rect(0,0,3,3)), rvec);
    return cv::norm(rvec) * 180. / CV_PI;
}

// convertions
//
inline
Eigen::Vector3d cvtPoint_ocv2egn(const cv::Point3f& ocv_p)
{
    Eigen::Vector3d egn_p;
    egn_p[0] = ocv_p.x;
    egn_p[1] = ocv_p.y;
    egn_p[2] = ocv_p.z;

    return egn_p;
}

inline
cv::Mat cvtIsometry_egn2ocv(const Eigen::Isometry3d& egn_o)
{
    Eigen::Matrix3d eigenRotation = egn_o.rotation();
    Eigen::Matrix<double,3,1> eigenTranslation = egn_o.translation();

    cv::Mat R, t;
    eigen2cv(eigenRotation, R);
    eigen2cv(eigenTranslation, t);
    t = t.reshape(1,3);

    cv::Mat ocv_o = cv::Mat::eye(4,4,CV_64FC1);
    R.copyTo(ocv_o(cv::Rect(0,0,3,3)));
    t.copyTo(ocv_o(cv::Rect(3,0,1,3)));

    return ocv_o;
}

// corresps
inline
void set2shorts(int& dst, int short_v1, int short_v2)
{
    unsigned short* ptr = reinterpret_cast<unsigned short*>(&dst);
    ptr[0] = static_cast<unsigned short>(short_v1);
    ptr[1] = static_cast<unsigned short>(short_v2);
}

inline
void get2shorts(int src, int& short_v1, int& short_v2)
{
    typedef union { int vint32; unsigned short vuint16[2]; } s32tou16;
    const unsigned short* ptr = (reinterpret_cast<s32tou16*>(&src))->vuint16;
    short_v1 = ptr[0];
    short_v2 = ptr[1];
}

int computeCorrespsFiltered(const cv::Mat& K, const cv::Mat& K_inv, const cv::Mat& Rt,
                            const cv::Mat& depth0, const cv::Mat& validMask0,
                            const cv::Mat& depth1, const cv::Mat& selectMask1, float maxDepthDiff,
                            cv::Mat& corresps,
                            const cv::Mat& normals0, const cv::Mat& normals1,
                            const cv::Mat& image0, const cv::Mat& image1,
                            float maxColorDiff = FLT_MAX);

void selectPosesSubset(const std::vector<cv::Mat>& poses,
                       const std::vector<int>& indices,
                       std::vector<int>& selectedIndices, size_t count);

// create solver
//
inline
G2OLinearSolver* createLinearSolver(const std::string& type)
{
    G2OLinearSolver* solver = 0;
    if(type == "cholmod")
        solver = new G2OLinearCholmodSolver();
    else if(type == "csparse")
    {
        solver = new G2OLinearCSparseSolver();
    }
    else
    {
        CV_Assert(0);
    }

    return solver;
}

inline
G2OBlockSolver* createBlockSolver(G2OLinearSolver* linearSolver)
{
    return new G2OBlockSolver(linearSolver);
}

inline
g2o::OptimizationAlgorithm* createNonLinearSolver(const std::string& type, G2OBlockSolver* blockSolver)
{
    g2o::OptimizationAlgorithm* solver = 0;
    if(type == "GN")
        solver = new g2o::OptimizationAlgorithmGaussNewton(blockSolver);
    else if(type == "LM")
        solver = new g2o::OptimizationAlgorithmLevenberg(blockSolver);
    else
        CV_Assert(0);

    return solver;
}

inline
g2o::SparseOptimizer* createOptimizer(g2o::OptimizationAlgorithm* solver)
{
    g2o::SparseOptimizer* optimizer = new g2o::SparseOptimizer();
    optimizer->setAlgorithm(solver);
    optimizer->setVerbose(true);
    return optimizer;
}

// graph opt

// Restore refined camera poses from the graph.
void getSE3Poses(g2o::SparseOptimizer* optimizer, const std::vector<int>& frameIndices, std::vector<cv::Mat>& poses);

// Fill the given graph by vertices and edges for the camera pose refinement geometrically.
// Each vertex is a camera pose. Each edge constraint is the odometry between linked vertices.
void fillGraphSE3(g2o::SparseOptimizer* optimizer,
                  const std::vector<cv::Mat>& poses, const std::vector<PosesLink>& posesLinks,
                  std::vector<int>& frameIndices);

// Refine camera poses geometrically
void refineGraphSE3(const std::vector<cv::Mat>& poses, const std::vector<PosesLink>& posesLinks,
                    std::vector<cv::Mat>& refinedPoses, std::vector<int>& frameIndices);

void refineGraphSE3Segment(const std::vector<cv::Mat>& odometryPoses,
                           const std::vector<cv::Mat>& partiallyRefinedPoses,
                           const std::vector<int>& refinedFrameIndices,
                           std::vector<cv::Mat>& refinedAllPoses);

// Graph with 2 types of edges: odometry and Rgbd+ICP for correspondences.
// TODO we need next iteration of the code refactoring
void fillGraphSE3RgbdICP(g2o::SparseOptimizer* optimizer, int pyramidLevel,
                         const std::vector<cv::Ptr<cv::OdometryFrame> >& frames,
                         const std::vector<cv::Mat>& poses, const std::vector<PosesLink>& posesLinks, const cv::Mat& cameraMatrix,
                         std::vector<int>& frameIndices,
                         double maxTranslation, double maxRotation, double maxDepthDiff);

// Refine camera poses by graph with odometry edges and Rgbd+ICP edges
void refineGraphSE3RgbdICP(const std::vector<cv::Ptr<cv::RgbdFrame> >& frames,
                           const std::vector<cv::Mat>& poses, const std::vector<PosesLink>& posesLinks, const cv::Mat& cameraMatrix,
                           float pointsPart, std::vector<cv::Mat>& refinedPoses, std::vector<int>& frameIndices);

void refineGraphSE3RgbdICPModel(std::vector<cv::Ptr<cv::RgbdFrame> >& frames,
                                const std::vector<cv::Mat>& poses, const std::vector<PosesLink>& posesLinks, const cv::Mat& cameraMatrix,
                                std::vector<cv::Mat>& refinedPoses, std::vector<int>& frameIndices);

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   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 NT2_LINALG_FUNCTIONS_TIED_NSEIG_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_TIED_NSEIG_HPP_INCLUDED

#include <nt2/linalg/functions/nseig.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/include/functions/balance.hpp>
#include <nt2/include/functions/from_diag.hpp>
#include <nt2/include/functions/gebak.hpp>
#include <nt2/include/functions/gebal.hpp>
#include <nt2/include/functions/geev_w.hpp>
#include <nt2/include/functions/geev_wvr.hpp>
#include <nt2/include/functions/geev_wvrvl.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/resize.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/linalg/details/utility/lapack_verify.hpp>
#include <nt2/sdk/meta/as_real.hpp>
#include <nt2/linalg/options.hpp>
#include <nt2/core/container/dsl/as_terminal.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( nseig_, tag::cpu_
                            , (A0)
                            , (scalar_<unspecified_<A0> >)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0) const
    {
      return a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( nseig_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&) const
    {
      return a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( nseig_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                              (unspecified_<A2>)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&, const A2&) const
    {
      return a0;
    }
  };

  //============================================================================
  //Eig computations
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( nseig_, tag::cpu_
                            , (A0)(N0)(A1)(N1)
                            , ((node_<A0, nt2::tag::nseig_
                                    , N0, nt2::container::domain
                                      >
                              ))
                              ((node_<A1, nt2::tag::tie_
                                    , N1, nt2::container::domain
                                     >
                              ))
                            )
  {
    typedef void  result_type;
    typedef typename boost::proto::result_of::child_c<A0&,0>::value_type    child0;
    typedef typename child0::value_type                                     type_t;
    typedef typename nt2::meta::as_real<type_t>::type                      rtype_t;
    typedef typename nt2::meta::as_complex<rtype_t>::type                  ctype_t;
    typedef nt2::memory::container<tag::table_, ctype_t, nt2::_2D> desired_semantic;

    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      eval(a0, a1, N0(), N1());
    }

    //==========================================================================
    /// INTERNAL ONLY - W = NSEIG(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<1> const&
              ) const
    {
      eval1_2(a0, a1, nt2::policy<ext::vector_>());
    }

    /// INTERNAL ONLY - W = NSEIG(A, matrix_/vector_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<1> const&
              ) const
    {
      eval1_2(a0, a1, boost::proto::value(boost::proto::child_c<1>(a0)));
    }

    /// INTERNAL ONLY: 1o 2i
    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic, a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, w, boost::proto::child_c<0>(a1));
      w.resize(of_size(height(a), 1));
      NT2_LAPACK_VERIFY(nt2::geev_w( boost::proto::value(a)
                                   , boost::proto::value(w)));
      boost::proto::child_c<0>(a1) = w;
    }

    /// INTERNAL ONLY: 1o 2i
    BOOST_FORCEINLINE
    void eval1_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic, a, boost::proto::child_c<0>(a0), work);
      nt2::container::table <ctype_t, _2D > w(of_size(height(a), 1));
      NT2_LAPACK_VERIFY(nt2::geev_w( boost::proto::value(a)
                                  , boost::proto::value(w)
                                  ));
      boost::proto::child_c<0>(a1) = from_diag(w); //from_diag doesnt support aliasing currently
    }

    //==========================================================================
    /// INTERNAL ONLY - [VR, W]= NSEIG(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_2(a0, a1,nt2::policy<ext::matrix_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [VR, W]= NSEIG(A,  matrix_/vector_/balance_/no_balance_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_2(a0, a1, boost::proto::value(boost::proto::child_c<1>(a0)));
    }

   /// INTERNAL ONLY: 2o 2i
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic,  w, boost::proto::child_c<1>(a1));
      size_t n = height(a);
      w.resize(of_size(n, 1));
      vr.resize(of_size(n, n));
      NT2_LAPACK_VERIFY(nt2::geev_wvr( boost::proto::value(a)
                                     , boost::proto::value(w)
                                     , boost::proto::value(vr)
                                     ));
      boost::proto::child_c<1>(a1) = w;
      boost::proto::child_c<0>(a1) = vr;
    }

    /// INTERNAL ONLY: 2o 2i
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      size_t n = height(a);
      nt2::container::table<ctype_t> w(of_size(n, 1));
      vr.resize(of_size(n, n));
      NT2_LAPACK_VERIFY(nt2::geev_wvr( boost::proto::value(a)
                                     , boost::proto::value(w)
                                     , boost::proto::value(vr)
                                     ));
      boost::proto::child_c<1>(a1) = from_diag(w);
      boost::proto::child_c<0>(a1) = vr;
    }

    /// INTERNAL ONLY: 2o 2i
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::balance_> const &
                 ) const
    {
      eval2_3(a0, a1
             , nt2::policy<ext::matrix_>()
             , nt2::policy<ext::balance_>());
    }

    /// INTERNAL ONLY: 2o 2i
    BOOST_FORCEINLINE
    void eval2_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::no_balance_> const &
                 ) const
    {
      eval2_3(a0, a1
             , nt2::policy<ext::matrix_>()
             , nt2::policy<ext::no_balance_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [VR, W, VL]= NSEIG(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<1> const&
              , boost::mpl::long_<3> const&
              ) const
    {
      eval3_3( a0, a1
             , nt2::policy<ext::matrix_>()
             , nt2::policy<ext::no_balance_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [VR, W, VL]= NSEIG(A, vector_/matrix_/balance_/no_balance_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<3> const&
              ) const
    {
      eval3_2(a0, a1, boost::proto::value(boost::proto::child_c<1>(a0)));
    }

    /// INTERNAL ONLY: 3o 2i
    BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic, vl, boost::proto::child_c<2>(a1));
      size_t n = height(a);
      nt2::container::table<ctype_t> w(of_size(n, 1));
      vr.resize(of_size(n, n));
      vl.resize(of_size(n, n));
      NT2_LAPACK_VERIFY(nt2::geev_wvrvl( boost::proto::value(a)
                                       , boost::proto::value(w)
                                       , boost::proto::value(vr)
                                       , boost::proto::value(vl)
                                       ));
      boost::proto::child_c<1>(a1) = from_diag(w);
      boost::proto::child_c<0>(a1) = vr;
      boost::proto::child_c<2>(a1) = vl;
    }

    /// INTERNAL ONLY: 3o 2i
    BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic,  w, boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic, vl, boost::proto::child_c<2>(a1));
      size_t n = height(a);
      vr.resize(of_size(n, n));
      vl.resize(of_size(n, n));
      w.resize(of_size(n, 1));
      NT2_LAPACK_VERIFY(nt2::geev_wvrvl( boost::proto::value(a)
                                       , boost::proto::value(w)
                                       , boost::proto::value(vr)
                                       , boost::proto::value(vl)
                                       ));
      boost::proto::child_c<1>(a1) = w;
      boost::proto::child_c<0>(a1) = vr;
      boost::proto::child_c<2>(a1) = vl;
    }

    /// INTERNAL ONLY: 3o 2i
    template < class T> BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                 , nt2::policy<ext::no_balance_> const &
                 ) const
    {
      eval3_3(a0, a1
             , nt2::policy<ext::matrix_>()
             , nt2::policy<ext::no_balance_>());
    }

    /// INTERNAL ONLY: 3o 2i
    template < class T> BOOST_FORCEINLINE
    void eval3_2 ( A0& a0, A1& a1
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      eval3_3(a0, a1, nt2::policy<ext::matrix_>(), nt2::policy<ext::balance_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [W]= NSEIG(A, matrix_/vector_, balance_/no_balance_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<3> const&
              , boost::mpl::long_<1> const&
              ) const
    {
      eval1_3(a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0))
             , boost::proto::value(boost::proto::child_c<2>(a0)));
    }

    /// INTERNAL ONLY: 1o 3i
    template < class T >
    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 , T const &
                 ,  nt2::policy<ext::no_balance_> const &
                 ) const
    {
      eval1_2(a0, a1, T());
    }

    /// INTERNAL ONLY: 1o 3i
    template < class T >
    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 ,  nt2::policy<ext::vector_>  const &
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      nt2::container::table<type_t> a = boost::proto::child_c<0>(a0);
      nt2::container::table<type_t> b = balance(a);
      boost::proto::child_c<0>(a1) = nseig(b, nt2::policy<ext::vector_>());
    }
    /// INTERNAL ONLY: 1o 3i
    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 ,  nt2::policy<ext::matrix_>  const &
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      nt2::container::table<type_t> a = boost::proto::child_c<0>(a0);
      nt2::container::table<type_t> b = balance(a);
      boost::proto::child_c<0>(a1) = nseig(b); //as_temporary(b));

    }
    //==========================================================================
    /// INTERNAL ONLY - [V, W]= NSEIG(A, matrix_/vector_, balance_/no_balance_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<3> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_3(a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0))
             , boost::proto::value(boost::proto::child_c<2>(a0)));
    }

    /// INTERNAL ONLY: 2o 3i
    template < class T> BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 ,  T  const &
                 ,  nt2::policy<ext::no_balance_> const &
                 ) const
    {
      eval2_2 (a0, a1, T());
    }

    /// INTERNAL ONLY: 2o 3i
    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 ,  nt2::policy<ext::vector_>  const &
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic,  v, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic,  w, boost::proto::child_c<1>(a1));

      nt2_la_int ilo, ihi;
      nt2_la_int n = height(a);
      nt2::container::table<rtype_t> scale(of_size(n, 1));
      NT2_LAPACK_VERIFY(gebal(boost::proto::value(a)
                             , boost::proto::value(scale)
                             , ilo, ihi, 'B'));
      tie(v, w) = nseig(a, nt2::policy<ext::vector_>());
      NT2_LAPACK_VERIFY(gebak( boost::proto::value(v)
                             , boost::proto::value(scale)
                             , ilo, ihi, 'B', 'R'));
      boost::proto::child_c<1>(a1) = w;
      boost::proto::child_c<0>(a1) = v;
    }

    /// INTERNAL ONLY: 2o 3i
    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 ,  nt2::policy<ext::matrix_>  const &
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic,  v, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic,  w, boost::proto::child_c<1>(a1));
      nt2_la_int ilo, ihi;
      nt2_la_int n = height(a);
      nt2::container::table<rtype_t> scale(of_size(n, 1));
      NT2_LAPACK_VERIFY(gebal( boost::proto::value(a)
                             , boost::proto::value(scale)
                             , ilo, ihi, 'B'));
      tie(v, w) = nseig(a, nt2::policy<ext::matrix_>());
      NT2_LAPACK_VERIFY(gebak( boost::proto::value(v)
                             , boost::proto::value(scale)
                             , ilo, ihi, 'B', 'R'));
      boost::proto::child_c<1>(a1) = w;
      boost::proto::child_c<0>(a1) = v;
    }

    //==========================================================================
    /// INTERNAL ONLY - [VR, W, VL]= NSEIG(A, matrix_/vector_, balance_/no_balance_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<3> const&
              , boost::mpl::long_<3> const&
              ) const
    {
      eval3_3(a0, a1
             , boost::proto::value(boost::proto::child_c<1>(a0))
             , boost::proto::value(boost::proto::child_c<2>(a0)));
    }

     /// INTERNAL ONLY: 3o 3i
    BOOST_FORCEINLINE
    void eval3_3( A0& a0, A1& a1
                 ,  nt2::policy<ext::vector_>  const &
                 ,  nt2::policy<ext::no_balance_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      tie( boost::proto::child_c<0>(a1)
         , boost::proto::child_c<1>(a1)
         , boost::proto::child_c<2>(a1)) = nseig(a, nt2::policy<ext::vector_>());
    }

     /// INTERNAL ONLY: 3o 3i
    BOOST_FORCEINLINE
    void eval3_3( A0& a0, A1& a1
                 ,  nt2::policy<ext::matrix_>  const &
                 ,  nt2::policy<ext::no_balance_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic, vl, boost::proto::child_c<2>(a1));
      nt2_la_int n = height(a);
      nt2::container::table<ctype_t> w(of_size(n, 1));
      tie( vr, w, vl) = nseig(a, nt2::policy<ext::vector_>());
      boost::proto::child_c<1>(a1) =  from_diag(w);
      boost::proto::child_c<0>(a1) = vr;
      boost::proto::child_c<2>(a1) = vl;
    }

    /// INTERNAL ONLY: 3o 3i
    BOOST_FORCEINLINE
    void eval3_3( A0& a0, A1& a1
                 ,  nt2::policy<ext::vector_>  const &
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic, vl, boost::proto::child_c<2>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic,  w, boost::proto::child_c<1>(a1));
      nt2_la_int ilo, ihi;
      nt2_la_int n = height(a);
      nt2::container::table<rtype_t> scale(of_size(n, 1));
      NT2_LAPACK_VERIFY(gebal(boost::proto::value(a)
                             , boost::proto::value(scale)
                             , ilo, ihi, 'B'));
      tie(vr, w, vl) = nseig(a, nt2::policy<ext::vector_>(), nt2::policy<ext::no_balance_>());
      NT2_LAPACK_VERIFY(gebak(boost::proto::value(vr),
                              boost::proto::value(scale)
                             , ilo, ihi, 'B', 'R'));
      NT2_LAPACK_VERIFY(gebak(boost::proto::value(vl)
                             , boost::proto::value(scale)
                             , ilo, ihi, 'B', 'L'));
      boost::proto::child_c<1>(a1) = w;
      boost::proto::child_c<0>(a1) = vr;
      boost::proto::child_c<2>(a1) = vl;
    }

    /// INTERNAL ONLY
    BOOST_FORCEINLINE
    void eval3_3 ( A0& a0, A1& a1
                 ,  nt2::policy<ext::matrix_>  const &
                 ,  nt2::policy<ext::balance_> const &
                 ) const
    {
      nt2::container::table<type_t> work;
      NT2_AS_TERMINAL_INOUT(desired_semantic,  a, boost::proto::child_c<0>(a0), work);
      NT2_AS_TERMINAL_OUT  (desired_semantic, vr, boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic, vl, boost::proto::child_c<2>(a1));
      NT2_AS_TERMINAL_OUT  (desired_semantic,  w, boost::proto::child_c<1>(a1));
      nt2_la_int ilo, ihi;
      nt2_la_int n = height(a);
      nt2::container::table<rtype_t> scale(of_size(n, 1));
      NT2_LAPACK_VERIFY(gebal(boost::proto::value(a), boost::proto::value(scale), ilo, ihi, 'B'));
      tie(vr, w, vl) = nseig(a, nt2::policy<ext::matrix_>(), nt2::policy<ext::no_balance_>());
      NT2_LAPACK_VERIFY(gebak(boost::proto::value(vr),
                              boost::proto::value(scale), ilo, ihi, 'B', 'R'));
      NT2_LAPACK_VERIFY(gebak(boost::proto::value(vl),
                              boost::proto::value(scale), ilo, ihi, 'B', 'L'));
      boost::proto::child_c<1>(a1) = w;
      boost::proto::child_c<0>(a1) = vr;
      boost::proto::child_c<2>(a1) = vl;
    }

  };
} }

#endif


/****************************************************************************
 * Copyright (c) 2017-2021 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include <ArborX.hpp>
#include <ArborX_Ray.hpp>
#include <ArborX_Version.hpp>

#include <Kokkos_Core.hpp>

#include <boost/program_options.hpp>

#include <iostream>
#include <random>

template <typename MemorySpace>
struct SpheresToBoxes
{
  Kokkos::View<ArborX::Sphere *, MemorySpace> _spheres;
};

template <typename MemorySpace>
struct ArborX::AccessTraits<SpheresToBoxes<MemorySpace>, ArborX::PrimitivesTag>
{
  using memory_space = MemorySpace;

  KOKKOS_FUNCTION static std::size_t
  size(const SpheresToBoxes<MemorySpace> &stob)
  {
    return stob._spheres.extent(0);
  }
  KOKKOS_FUNCTION static ArborX::Box
  get(SpheresToBoxes<MemorySpace> const &stobs, std::size_t const i)
  {
    auto const &sphere = stobs._spheres(i);
    auto const &c = sphere.centroid();
    auto const r = sphere.radius();
    return {{c[0] - r, c[1] - r, c[2] - r}, {c[0] + r, c[1] + r, c[2] + r}};
  }
};

template <typename MemorySpace>
struct Rays
{
  Kokkos::View<ArborX::Experimental::Ray *, MemorySpace> _rays;
};

template <typename MemorySpace>
struct ArborX::AccessTraits<Rays<MemorySpace>, ArborX::PredicatesTag>
{
  using memory_space = MemorySpace;

  KOKKOS_FUNCTION static std::size_t size(const Rays<MemorySpace> &rays)
  {
    return rays._rays.extent(0);
  }
  KOKKOS_FUNCTION static auto get(Rays<MemorySpace> const &rays, std::size_t i)
  {
    return attach(intersects(rays._rays(i)), (int)i);
  }
};

template <typename MemorySpace>
struct AccumRaySphereInterDist
{
  Kokkos::View<ArborX::Sphere *, MemorySpace> _spheres;
  Kokkos::View<float *, MemorySpace> _accumulator;

  template <typename Predicate>
  KOKKOS_FUNCTION void operator()(Predicate const &predicate,
                                  int const primitive_index) const
  {
    auto const &ray = ArborX::getGeometry(predicate);
    auto const &sphere = _spheres(primitive_index);

    float const length = overlapDistance(ray, sphere);
    int const i = getData(predicate);

    Kokkos::atomic_add(&_accumulator(i), length);
  }
};

int main(int argc, char *argv[])
{
  using ExecutionSpace = Kokkos::DefaultExecutionSpace;
  using MemorySpace = ExecutionSpace::memory_space;

  Kokkos::ScopeGuard guard(argc, argv);

  namespace bpo = boost::program_options;

  int num_spheres;
  int num_rays;
  float L;

  bpo::options_description desc("Allowed options");
  // clang-format off
  desc.add_options()
    ( "help", "help message" )
    ("spheres", bpo::value<int>(&num_spheres)->default_value(100), "number of spheres")
    ("rays", bpo::value<int>(&num_rays)->default_value(10000), "number of rays")
    ("L", bpo::value<float>(&L)->default_value(100.0), "size of the domain")
    ;
  // clang-format on
  bpo::variables_map vm;
  bpo::store(bpo::command_line_parser(argc, argv).options(desc).run(), vm);
  bpo::notify(vm);

  if (vm.count("help") > 0)
  {
    std::cout << desc << '\n';
    return 1;
  }

  std::cout << "ArborX version: " << ArborX::version() << std::endl;
  std::cout << "ArborX hash   : " << ArborX::gitCommitHash() << std::endl;
  std::cout << "Kokkos version: " << KokkosExt::version() << std::endl;

  std::uniform_real_distribution<float> uniform{0.0, 1.0};
  std::default_random_engine gen;
  auto rand_uniform = [&]() { return uniform(gen); };

  // Random parameters for Gaussian distribution of radii
  const float mu_R = 1.0;
  const float sigma_R = mu_R / 3.0;

  std::normal_distribution<> normal{mu_R, sigma_R};
  auto rand_normal = [&]() { return std::max(normal(gen), 0.0); };

  // Construct spheres
  //
  // The centers of spheres are uniformly sampling the domain. The radii of
  // spheres have Gaussian (mu_R, sigma_R) sampling.
  Kokkos::View<ArborX::Sphere *, MemorySpace> spheres(
      Kokkos::view_alloc(Kokkos::WithoutInitializing, "spheres"), num_spheres);
  auto spheres_host = Kokkos::create_mirror_view(spheres);
  for (int i = 0; i < num_spheres; ++i)
  {
    spheres_host(i) = {
        {rand_uniform() * L, rand_uniform() * L, rand_uniform() * L},
        rand_normal()};
  }
  Kokkos::deep_copy(spheres, spheres_host);

  // Construct rays
  //
  // The origins of rays are uniformly sampling the bottom surface of the
  // domain. The direction vectors are uniformly sampling of a cosine-weighted
  // hemisphere, It requires expressing the direction vector in the spherical
  // coordinates as:
  //    {sinpolar * cosazimuth, sinpolar * sinazimuth, cospolar}
  // A detailed description can be found in the slides here (slide 47):
  // https://cg.informatik.uni-freiburg.de/course_notes/graphics2_08_renderingEquation.pdf
  Kokkos::View<ArborX::Experimental::Ray *, MemorySpace> rays(
      Kokkos::view_alloc(Kokkos::WithoutInitializing, "rays"), num_rays);
  auto rays_host = Kokkos::create_mirror_view(rays);

  for (int i = 0; i < num_rays; ++i)
  {
    float xi_1 = rand_uniform();
    float xi_2 = rand_uniform();

    rays_host(i) = {ArborX::Point{rand_uniform() * L, rand_uniform() * L, 0.f},
                    ArborX::Experimental::Vector{
                        float(std::cos(2 * M_PI * xi_2) * std::sqrt(xi_1)),
                        float(std::sin(2 * M_PI * xi_2) * std::sqrt(xi_1)),
                        std::sqrt(1.f - xi_1)}};
  }
  Kokkos::deep_copy(rays, rays_host);

  Kokkos::Timer timer;

  ExecutionSpace exec_space{};

  exec_space.fence();
  timer.reset();
  ArborX::BVH<MemorySpace> bvh{exec_space,
                               SpheresToBoxes<MemorySpace>{spheres}};

  Kokkos::View<float *, MemorySpace> accumulator("accumulator", num_rays);
  bvh.query(exec_space, Rays<MemorySpace>{rays},
            AccumRaySphereInterDist<MemorySpace>{spheres, accumulator});
  exec_space.fence();
  auto time = timer.seconds();

  auto accumulator_avg =
      ArborX::accumulate(exec_space, accumulator, 0.f) / num_rays;

  printf("time          : %.3f   [%.3fM ray/sec]\n", time,
         num_rays / (1000000 * time));
  printf("ray avg       : %.3f\n", accumulator_avg);

  return EXIT_SUCCESS;
}