#include <iostream>
#include <string>
#include <algorithm>
#include <vector>
#include <chrono>
#include <sys/stat.h>
#include <sys/types.h>
#include <dirent.h>
#include <memory>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>
#include <yaml-cpp/yaml.h>
#include <Open3D/Open3D.h>
#include <glog/logging.h>

#include "teche_calibration.h"
#include "FileSystemTools.h"
#include "YamlFileIO.h"
#ifdef VISUALIZE_TRAJECTORY
#include "viewer.h"
#endif

/**
  * @brief save monocular camera pose in w2c form.
  * @param folder_name is name of file folder.
  * @param cam_id is the id of each camera.
  * @param cam_pose is the camera pose in w2c.
  * @return .
  */
bool saveCamPoseTxt(const std::string &filepath, const Eigen::Matrix4d &cam_pose)
{
    std::ofstream output_file(filepath);
    if (!output_file.is_open())
    {
        LOG(FATAL) << " Fail to open " << filepath << "\n";
        return false;
    }
    LOG(INFO) << " Write " << filepath << "\n";
    for (size_t i = 0; i < 4; ++i)
    {
        for (size_t j = 0; j < 4; ++j)

        {
            if (j != 3)
                output_file << cam_pose(i, j) << ", ";
            else
                output_file << cam_pose(i, j) << "\n";
        }
    }
    output_file.close();

    return true;
}

std::shared_ptr<open3d::geometry::PointCloud> convertPointClouds(const std::vector<Eigen::Vector3d> &v_pointclouds)
{
    assert(v_pointclouds.size());

    int pts_num = v_pointclouds.size();
    std::shared_ptr<open3d::geometry::PointCloud> pcl_ptr(new open3d::geometry::PointCloud);
    for(int i = 0; i < pts_num; ++i)
    {
        pcl_ptr->points_.push_back(v_pointclouds[i]);
    }

    return pcl_ptr;
}

int main(int argc, char **argv)
{
    if (argc < 8)
    {
        std::cerr << "Usage : test_techecalibrator [global_map.yaml] [cctag_result.yaml]\n \
        [cam0_intrin_file] [cam1_intrin_file] [cam2_intrin_file] [cam3_intrin_file]\n \
        [output_folder] \n";
        return -1;
    }
    // start timer
    // const auto tp_1 = std::chrono::steady_clock::now();

    // save w2c camera pose
    bool b_save_camera_pose_w_c = true;
    // save c2r camera pose
    bool b_save_camera_pose_c_r = true;

    std::string gt_map_file_name(argv[1]);
    std::string cctag_file_name(argv[2]);
    std::string cam0_calibration_file_name(argv[3]);
    std::string cam1_calibration_file_name(argv[4]);
    std::string cam2_calibration_file_name(argv[5]);
    std::string cam3_calibration_file_name(argv[6]);
    std::string output_folder(argv[7]);

    if (!common::fileExists(gt_map_file_name))
    {
        LOG(ERROR) << " FIle " << gt_map_file_name << " doesnot exist!";
        return -1;
    }
    if (!common::pathExists(output_folder))
    {
        if (!common::createPath(output_folder))
        {
            LOG(ERROR) << " Fail to create " << output_folder;
            return -1;
        }
    }

    std::vector<std::string> v_intrinsic_fns = {cam0_calibration_file_name, cam1_calibration_file_name,
                                                cam2_calibration_file_name, cam3_calibration_file_name};
    std::vector<std::string> v_calib_img_paths;
    std::vector<std::vector<cv::Point2d>> v_cctag_centers;
    bool sts = common::loadCCTagResultFile(cctag_file_name, v_calib_img_paths, v_cctag_centers);
    if (!sts)
    {
        LOG(ERROR) << " Failed to parse " << cctag_file_name;
        return -1;
    }

    TecheCalibrator teche_calibrator(camera::model_type_t::Fisheye, target::target_type_t::CCTAG, v_intrinsic_fns, gt_map_file_name, 4);
    sts = teche_calibrator.addCctagData(v_calib_img_paths, v_cctag_centers);
    if (!sts)
    {
        LOG(ERROR) << " Feail to add cctag data!";
        return -1;
    }

    sts = teche_calibrator.calibrate();
    if (!sts)
    {
        LOG(ERROR) << " Fail to calibrate teche 360 anywhere camera";
        return -1;
    }
    // const auto tp_2 = std::chrono::steady_clock::now();
    // const auto calib_time = std::chrono::duration_cast<std::chrono::duration<double>>(tp_2 - tp_1).count();
    // TIMER_STREAM("[test_techecalibrator] caliration time is " << calib_time << " s");

    std::vector<Eigen::Matrix4d> v_cam_poses_w2c = teche_calibrator.validCameraPoses();
    // rotation axis 2 camera
    std::vector<Eigen::Matrix4d> v_cam_poses_r2c = teche_calibrator.validCameraPosesInRotationAxisFrame();

    // save camera pose and relative pose
    for (size_t i = 0; i < v_cam_poses_w2c.size(); ++i)
    {
        if (b_save_camera_pose_w_c)
        {
            std::string txt_filepath = output_folder + "/cam" + std::to_string(i) + "_w2c.txt";
            saveCamPoseTxt(txt_filepath, v_cam_poses_w2c[i]);
        }
        if (i == 0)
            continue;

        Eigen::Matrix4d T_cam0_cami = v_cam_poses_w2c[0].inverse() * v_cam_poses_w2c[i];
        std::string yml_filepath = output_folder + "/camera0"+"_to_camera"+ std::to_string(i) +".yml";
        common::saveExtFileOpencv(yml_filepath, T_cam0_cami);
    }

    // for (size_t i = 0; i < v_cam_poses_r2c.size(); ++i)
    // {
    //     if (b_save_camera_pose_c_r)
    //     {
    //         sts = writeCameraPosesInYml(output_folder, i, v_cam_poses_r2c[i]);
    //         if (!sts)
    //         {
    //             ERROR_STREAM("[test_techecalibrator] Fail to write frame " << i);
    //             return -1;
    //         }
    //     }
    // }

#ifdef VISUALIZE_TRAJECTORY
    Viewer viewer(v_visual_frames);
    std::vector<Eigen::Vector3d> v_map_points = teche_calibrator.objectPoints();
    // std::shared_ptr<open3d::geometry::PointCloud> targtboard_pcl_ptr = convertPointClouds(v_map_points);
    // open3d::io::WritePointCloudToPLY(output_folder + "/calibration_room.ply", *targtboard_pcl_ptr, true);
    viewer.setCurrentMapPoints(v_map_points);
    viewer.run();
#endif

    return 0;
}


#ifndef TRAJECTORY_TRAJLIB_HPP
#define TRAJECTORY_TRAJLIB_HPP

/*
 * Trajectory class representing a trajectory in state space
 *
 * Author: Andrew Barry, <abarry@csail.mit.edu> 2013-2015
 *
 */

#include <iostream>
#include <string>
#include <fstream>
#include <vector>
#include <sstream>
#include <cmath>

#include <bot_core/rotations.h>
#include <bot_frames/bot_frames.h>
#include <GL/gl.h>
#include <bot_lcmgl_client/lcmgl.h>
#include <bot_vis/gl_util.h>
#include "gtest/gtest.h"
#include "../../utils/utils/RealtimeUtils.hpp"

#include "../../externals/csvparser/csvparser.h"
#include "../../estimators/StereoOctomap/StereoOctomap.hpp"

#include <Eigen/Core>

class Trajectory
{

    public:
        Trajectory();
        Trajectory(std::string filename_prefix, bool quiet = false); // loads a trajectory from a .csv file

        void LoadTrajectory(std::string filename_prefix, bool quiet = false);

        int GetDimension() const { return dimension_; }
        int GetUDimension() const { return udimension_; }
        int GetTrajectoryNumber() const { return trajectory_number_; }
        double GetDT() const { return dt_; }

        void GetXyzYawTransformedPoint(double t, const BotTrans &transform, double *xyz) const;
        void Draw(bot_lcmgl_t *lcmgl, const BotTrans *transform = nullptr, double final_time = -1) const;

        int GetIndexAtTime(double t) const;
        double GetTimeAtIndex(int index) const { return xpoints_(index, 0); }

        double GetMaxTime() const { return xpoints_(xpoints_.rows() - 1, 0); }

        bool IsTimeInvariant() const { return upoints_.rows() == 1; }

        int GetNumberOfPoints() const { return int(xpoints_.rows()); }

        Eigen::VectorXd GetState(double t) const;
        Eigen::VectorXd GetUCommand(double t) const;
        Eigen::MatrixXd GetGainMatrix(double t) const;

        Eigen::MatrixXd GetXpoints() const { return xpoints_; }

        double ClosestObstacleInRemainderOfTrajectory(const StereoOctomap &octomap, const BotTrans &body_to_local, double current_t, double min_altitude_allowed) const;

        void Print() const;

        double GetMinimumAltitude() const { return min_altitude_; }

    private:

        Eigen::MatrixXd xpoints_;
        Eigen::MatrixXd upoints_;

        Eigen::MatrixXd kpoints_;
        Eigen::MatrixXd affine_points_;

        double dt_;
        double min_altitude_;

        int dimension_; // state space dimension
        int udimension_; // control input dimension

        int trajectory_number_;
        std::string filename_prefix_;

        void LoadMatrixFromCSV(const std::string& filename, Eigen::MatrixXd &matrix, bool quiet = false);

        int GetNumberOfLines(std::string filename) const;

};

#endif


#ifndef STAN_MATH_PRIM_SCAL_PROB_EXPONENTIAL_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_EXPONENTIAL_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <boost/random/exponential_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/**
 * Return a exponential random variate with inverse scale beta
 * using the specified random number generator.
 *
 * beta can be a scalar or a one-dimensional container.
 *
 * @tparam T_inv Type of inverse scale parameter
 * @tparam RNG class of random number generator
 * @param beta (Sequence of) positive inverse scale parameter(s)
 * @param rng random number generator
 * @return (Sequence of) exponential random variate(s)
 * @throw std::domain_error if beta is nonpositive
 */
template <typename T_inv, class RNG>
inline typename VectorBuilder<true, double, T_inv>::type exponential_rng(
    const T_inv& beta, RNG& rng) {
  using boost::exponential_distribution;
  using boost::variate_generator;

  static const char* function = "exponential_rng";

  check_positive_finite(function, "Inverse scale parameter", beta);

  scalar_seq_view<T_inv> beta_vec(beta);
  size_t N = length(beta);
  VectorBuilder<true, double, T_inv> output(N);

  for (size_t n = 0; n < N; ++n) {
    variate_generator<RNG&, exponential_distribution<> > exp_rng(
        rng, exponential_distribution<>(beta_vec[n]));
    output[n] = exp_rng();
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


#define BOOST_TEST_MODULE bvh
#include <boost/test/included/unit_test.hpp>

#include <utility>
#include <vector>
#include "../src/bvh.hpp"

BOOST_AUTO_TEST_CASE(test_correct_calc_bbox_area) {
    Vec corner0(-1.0, 0.0, -1.0), corner1(1.0, 2.0, 1.0);
    BBox bbox(corner0, corner1);

    double result = calc_bbox_area(bbox);
    BOOST_CHECK_EQUAL(result, 24.0);
}

BOOST_AUTO_TEST_CASE(test_correct_merge_box) {
    Vec corner0(-1.0, 0.0, -1.0), corner1(1.0, 2.0, 1.0);
    BBox bbox1(corner0, corner1);
    BBox bbox2(corner0 + 2.0, corner1 + 2.0);
    BBox result;

    merge_bbox(bbox1, bbox2, result);
    BOOST_CHECK(result.corner0 == Vec(-1.0, 0.0, -1.0));
    BOOST_CHECK(result.corner1 == Vec(3.0, 4.0, 3.0));
    BOOST_CHECK(result.center == Vec(1.0, 2.0, 1.0));
}

struct Fixture {};

BOOST_FIXTURE_TEST_SUITE(test_bvh_sute, Fixture)
BOOST_AUTO_TEST_CASE(test_correct_bvh_enclosing_bbox) {
    BBox bbox1(Vec(-1.0, 0.0, -1.0), Vec(0.0, 1.0, 0.0));
    BBox bbox2(Vec(0.0, 1.0, 0.0), Vec(1.0, 2.0, 1.0));
    BBox bbox3(Vec(1.0, 2.0, 1.0), Vec(2.0, 3.0, 2.0));
    std::vector<std::pair<int, BBox *>> bboxes;
    bboxes.push_back(std::make_pair(0, &bbox1));
    bboxes.push_back(std::make_pair(1, &bbox2));
    bboxes.push_back(std::make_pair(2, &bbox3));

    BVH bvh;
    BBox result;
    result.empty();
    bvh.enclosing_bbox(bboxes, result);

    BOOST_CHECK(result.corner0 == Vec(-1.0, 0.0, -1.0));
    BOOST_CHECK(result.corner1 == Vec(2.0, 3.0, 2.0));
    BOOST_CHECK(result.center == Vec(0.5, 1.5, 0.5));
}

BOOST_AUTO_TEST_CASE(test_correct_bvh_make_leaf) {
    BBox bbox1(Vec(-1.0, 0.0, -1.0), Vec(0.0, 1.0, 0.0));
    BBox bbox2(Vec(0.0, 1.0, 0.0), Vec(1.0, 2.0, 1.0));
    BBox bbox3(Vec(1.0, 2.0, 1.0), Vec(2.0, 3.0, 2.0));
    std::vector<std::pair<int, BBox *>> bboxes;
    bboxes.push_back(std::make_pair(1000, &bbox1));
    bboxes.push_back(std::make_pair(20, &bbox2));
    bboxes.push_back(std::make_pair(500, &bbox3));

    BVH bvh;
    BvhNode result;
    bvh.make_leaf(bboxes, result);

    BOOST_CHECK_EQUAL(result.child_node_idx.first, -1);
    BOOST_CHECK_EQUAL(result.child_node_idx.second, -1);
    BOOST_CHECK_EQUAL(result.target_indices[0], 1000);
    BOOST_CHECK_EQUAL(result.target_indices[1], 20);
    BOOST_CHECK_EQUAL(result.target_indices[2], 500);
}

BOOST_AUTO_TEST_CASE(test_correct_bvh_axis_decending_sort) {
    BBox bbox1(Vec(-1.0, 0.0, -1.0), Vec(0.0, 1.0, 0.0));
    BBox bbox2(Vec(1.0, 2.0, 1.0), Vec(2.0, 3.0, 2.0));
    BBox bbox3(Vec(0.0, 1.0, 0.0), Vec(1.0, 2.0, 1.0));

    std::vector<std::pair<int, BBox *>> bboxes;
    bboxes.push_back(std::make_pair(0, &bbox1));
    bboxes.push_back(std::make_pair(1, &bbox2));
    bboxes.push_back(std::make_pair(2, &bbox3));

    BVH bvh;
    bvh.axis_decending_sort(bboxes, 0);
    BOOST_CHECK_EQUAL(bboxes[0].first, 1);
    BOOST_CHECK_EQUAL(bboxes[1].first, 2);
    BOOST_CHECK_EQUAL(bboxes[2].first, 0);

    bvh.axis_decending_sort(bboxes, 1);
    BOOST_CHECK_EQUAL(bboxes[0].first, 1);
    BOOST_CHECK_EQUAL(bboxes[1].first, 2);
    BOOST_CHECK_EQUAL(bboxes[2].first, 0);

    bvh.axis_decending_sort(bboxes, 2);
    BOOST_CHECK_EQUAL(bboxes[0].first, 1);
    BOOST_CHECK_EQUAL(bboxes[1].first, 2);
    BOOST_CHECK_EQUAL(bboxes[2].first, 0);

}

BOOST_AUTO_TEST_SUITE_END()


//
// Created by yche on 12/25/17.
//
#include <cstdlib>

#include <chrono>
#include <iostream>

#include <boost/program_options.hpp>

#include "../../yche_refactor/bflpmc_yche.h"
#include "../../yche_refactor/simrank.h"
#include "../../util/random_pair_generator.h"
#include "../../playground/pretty_print.h"

void test_BFLPMC(string data_name, double c, double epsilon, double delta, int pair_num, int round, double r_max) {
    // init graph
    string path = get_edge_list_path(data_name);
    GraphYche g(path);
    size_t n = static_cast<size_t>(g.n);
    cout << n << endl;

    // init bflpmc object
    auto bflpmc = BFLPMC(data_name, g, c, epsilon, delta, r_max);

#ifdef GROUND_TRUTH
    TruthSim ts(data_name, g, c, epsilon);
    auto max_err = 0.0;
#endif

#ifdef GROUND_TRUTH_STATISTICS
    auto above_eps_count = 0;
    auto above_point1_count = 0;
    auto above_point2_count = 0;
    auto above_point3_count = 0;
    auto above_point4_count = 0;
    auto above_point5_count = 0;
    auto above_point6_count = 0;
    for (auto i = 0u; i < n; i++) {
        for (auto j = i; j < n; j++) {
            if (ts.sim(i, j) > 0.01) { above_eps_count++; }
            if (ts.sim(i, j) > 0.1) { above_point1_count++; }
            if (ts.sim(i, j) > 0.2) { above_point2_count++; }
            if (ts.sim(i, j) > 0.3) { above_point3_count++; }
            if (ts.sim(i, j) > 0.4) { above_point4_count++; }
            if (ts.sim(i, j) > 0.5) { above_point5_count++; }
            if (ts.sim(i, j) > 0.6) { above_point6_count++; }
        }
    }
    cout << "above eps:" << above_eps_count << endl;
    cout << "above 0.1 :" << above_point1_count << endl;
    cout << "above 0.2 :" << above_point2_count << endl;
    cout << "above 0.3 :" << above_point3_count << endl;
    cout << "above 0.4 :" << above_point4_count << endl;
    cout << "above 0.5 :" << above_point5_count << endl;
    cout << "above 0.6 :" << above_point6_count << endl;
#endif

    auto sample_pairs = read_sample_pairs(data_name, pair_num, round);
    auto clock_start = clock();
    auto start = std::chrono::high_resolution_clock::now();
#pragma omp parallel
    {
        auto local_bflpmc = bflpmc;
//        auto local_bflpmc = BFLPMC(data_name, g, c, epsilon, delta);

#ifdef GROUND_TRUTH
#pragma omp for reduction(max:max_err) schedule(dynamic, 100)
#else
#pragma omp for schedule(dynamic, 100)
#endif
        for (auto pair_i = 0; pair_i < pair_num; pair_i++) {
            auto q = pair<uint32_t, uint32_t>(sample_pairs[pair_i].first, sample_pairs[pair_i].second);
#ifdef GROUND_TRUTH
            auto res = local_bflpmc.query_one2one(q);
            max_err = max(max_err, abs(ts.sim(q.first, q.second) - res));
            if (abs(ts.sim(q.first, q.second) - res) > 0.01) {
#pragma omp critical
                cout << sample_pairs[pair_i].first << "," << sample_pairs[pair_i].second << ","
                << ts.sim(q.first, q.second) << "," << res << endl;
            }
#else
            local_bflpmc.query_one2one(q);
#endif
        }
    };

    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = end - start;

    auto clock_end = clock();
    cout << "total query cpu time:" << static_cast<double>(clock_end - clock_start) / CLOCKS_PER_SEC << "s" << endl;
#ifdef GROUND_TRUTH
    cout << "max err:" << max_err << endl;
#endif
    cout << format("total query cost: %s s") % elapsed.count() << endl; // record the pre-processing time
}

int main(int args, char *argv[]) {
    string data_name(argv[1]);
    double c = 0.6;
    double epsilon = 0.01;
    double delta = 0.01;

    int pair_num = atoi(argv[2]);
    int round_i = atoi(argv[3]);
    double r_max = atof(argv[4]);
    test_BFLPMC(data_name, c, epsilon, delta, pair_num, round_i, r_max);
}


//==============================================================================
//         Copyright 2003 - 2013   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
//==============================================================================
#include <nt2/include/functions/rec.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/mzero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/cnan.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/mhalf.hpp>

NT2_TEST_CASE_TPL ( rec_real,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using nt2::rec;
  using nt2::tag::rec_;
  typedef typename std::complex<T> cT;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<cT,ext_t>                vcT;
  typedef typename nt2::dry<T>             dT;
  typedef native<dT,ext_t>                vdT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
   NT2_TEST_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::Inf<T>(),  nt2::Inf<T>() ))), nt2::splat<vcT>(cT(nt2::Zero<cT>())));
   NT2_TEST_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::Minf<T>(), nt2::Minf<T>()))), nt2::splat<vcT>(cT(nt2::Zero<cT>())));
   NT2_TEST_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::Inf<T>(),  nt2::Zero<T>()))), nt2::splat<vcT>(cT(nt2::Zero<cT>())));
   NT2_TEST_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::Zero<T>(), nt2::Inf<T>( )))), nt2::splat<vcT>(cT(nt2::Zero<cT>())));
   NT2_TEST_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::Minf<T>(), nt2::Zero<T>()))), nt2::splat<vcT>(cT(nt2::Zero<cT>())));
   NT2_TEST_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::Zero<T>(), nt2::Minf<T>()))), nt2::splat<vcT>(cT(nt2::Zero<cT>())));
   NT2_TEST_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::Zero<T>(), nt2::Zero<T>()))), nt2::splat<vcT>(cT(nt2::Inf<cT>() )));
   NT2_TEST_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::Mzero<T>(),nt2::Zero<T>()))), nt2::splat<vcT>(cT(nt2::Minf<cT>())));
   NT2_TEST_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::Nan<T>(),  nt2::Nan<T>() ))), nt2::Cnan<vcT>());
   NT2_TEST_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::Nan<T>(),  nt2::One<T>() ))), nt2::Cnan<vcT>());
   NT2_TEST_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::One<T>(),  nt2::Nan<T>() ))), nt2::Cnan<vcT>());
   NT2_TEST_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::Nan<T>(),  nt2::Zero<T>()))), nt2::Nan<vcT>());
   NT2_TEST_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::Zero<T>(), nt2::Nan<T>() ))), nt2::splat<vcT>(cT(nt2::Zero<T>(), nt2::Nan<T>())));
#endif
   NT2_TEST_EQUAL(nt2::rec(nt2::One<vcT>()), nt2::One<vcT>());
   NT2_TEST_EQUAL(nt2::rec(nt2::One<vdT>()), nt2::One<vdT>());

   NT2_TEST_ULP_EQUAL(nt2::rec(nt2::splat<vcT>(cT(nt2::One<T>(), nt2::One<T>() ))),
                      nt2::splat<vcT>(cT(nt2::Half<T>(),nt2::Mhalf<T>())), 0.5);

}


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

#include <commander/algorithm.hpp>

#include <boost/test/unit_test.hpp>

#include "arithmetic_helpers.hpp"

#include <tuple>
#include <type_traits>

BOOST_AUTO_TEST_CASE( arithmetic_compare )
{
    namespace com  = commander;
    namespace comd = commander::detail;

    using schar8  = std::make_signed_t<char>;     using uchar8  = std::make_unsigned_t<char>;
    using schar16 = std::make_signed_t<char16_t>; using uchar16 = std::make_unsigned_t<char16_t>;
    using schar32 = std::make_signed_t<char32_t>; using uchar32 = std::make_unsigned_t<char32_t>;
    using swchar  = std::make_signed_t<wchar_t>;  using uwchar  = std::make_unsigned_t<wchar_t>;

    const auto non_arithmetic_tuple = std::make_tuple(non_arithmetic_true(), non_arithmetic_false());
    const auto int_tuple = std::make_tuple(int(-5), int(-1), int(0), int(1), int(5));
    const auto float_tuple = std::make_tuple(float(-5), float(-1), float(0), float(1), float(5));
    const auto char_tuple = std::make_tuple(
        sc<schar8>(sc<char>('a')),      sc<uchar8>(sc<char>('a')),
        sc<schar16>(sc<char16_t>('a')), sc<uchar16>(sc<char16_t>('a')),
        sc<schar32>(sc<char32_t>('a')), sc<uchar32>(sc<char32_t>('a')),
        sc<swchar>(sc<wchar_t>('a')),   sc<uwchar>(sc<wchar_t>('a'))
    );

    const auto bool_limits_tuple = std::make_tuple(true, false);
    const auto signed_integral_limits_tuple = std::make_tuple(
        nl<int8_t>::max(),  nl<int8_t>::lowest(),
        nl<int16_t>::max(), nl<int16_t>::lowest(),
        nl<int32_t>::max(), nl<int32_t>::lowest(),
        nl<int64_t>::max(), nl<int64_t>::lowest()
    );
    const auto unsigned_integral_limits_tuple = std::make_tuple(
        nl<uint8_t>::max(),  nl<uint8_t>::lowest(),
        nl<uint16_t>::max(), nl<uint16_t>::lowest(),
        nl<uint32_t>::max(), nl<uint32_t>::lowest(),
        nl<uint64_t>::max(), nl<uint64_t>::lowest()
    );
    const auto floating_point_limits_tuple = std::make_tuple(
        nl<float>::max(),       nl<float>::lowest(),
        nl<double>::max(),      nl<double>::lowest(),
        nl<long double>::max(), nl<long double>::lowest()
    );

    {
        com::tuple_visit(
            [&](const auto& non_arithmetic) {
                size_t count = 0;
                com::tuple_visit(
                    [&](const auto& v) { count += comd::arithmetic_compare(non_arithmetic, v) ? 1 : 0; },
                    non_arithmetic_tuple, int_tuple, float_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
                );
                BOOST_CHECK(count == 0);
            },
            non_arithmetic_tuple
        );
    }

    {
        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 0>(int_tuple), std::get< 0>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 0>(int_tuple), std::get< 0>(float_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 0>(int_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 2);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(int_tuple), std::get< 1>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(int_tuple), std::get< 1>(float_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 1>(int_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 2);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(int_tuple), std::get< 2>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(int_tuple), std::get< 2>(float_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(int_tuple), std::get< 1>(bool_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(int_tuple), std::get< 1>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(int_tuple), std::get< 3>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(int_tuple), std::get< 5>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(int_tuple), std::get< 7>(unsigned_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 2>(int_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 7);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(int_tuple), std::get< 3>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(int_tuple), std::get< 3>(float_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(int_tuple), std::get< 0>(bool_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 3>(int_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 3);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 4>(int_tuple), std::get< 4>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 4>(int_tuple), std::get< 4>(float_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 4>(int_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 2);
        }
    }

    {
        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 0>(float_tuple), std::get< 0>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 0>(float_tuple), std::get< 0>(float_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 0>(float_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 2);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(float_tuple), std::get< 1>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(float_tuple), std::get< 1>(float_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 1>(float_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 2);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(float_tuple), std::get< 2>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(float_tuple), std::get< 2>(float_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(float_tuple), std::get< 1>(bool_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(float_tuple), std::get< 1>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(float_tuple), std::get< 3>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(float_tuple), std::get< 5>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(float_tuple), std::get< 7>(unsigned_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 2>(float_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 7);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(float_tuple), std::get< 3>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(float_tuple), std::get< 3>(float_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(float_tuple), std::get< 0>(bool_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 3>(float_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 3);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 4>(float_tuple), std::get< 4>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 4>(float_tuple), std::get< 4>(float_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 4>(float_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 2);
        }
    }

    {
        com::tuple_visit(
            [&](const auto& character) {
                BOOST_CHECK(com::tuple_visit_conjunction(
                    [&](const auto& v) -> bool { return comd::arithmetic_compare(character, v); },
                    char_tuple
                ) == true);

                size_t count = 0;
                com::tuple_visit(
                    [&](const auto& v) { count += comd::arithmetic_compare(character, v) ? 1 : 0; },
                    non_arithmetic_tuple, int_tuple, float_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
                );
                BOOST_CHECK(count == 0);
            },
            char_tuple
        );
    }

    {
        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 0>(bool_limits_tuple), std::get< 3>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 0>(bool_limits_tuple), std::get< 3>(float_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 0>(bool_limits_tuple), std::get< 0>(bool_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 0>(bool_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 3);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(bool_limits_tuple), std::get< 2>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(bool_limits_tuple), std::get< 2>(float_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(bool_limits_tuple), std::get< 1>(bool_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(bool_limits_tuple), std::get< 1>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(bool_limits_tuple), std::get< 3>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(bool_limits_tuple), std::get< 5>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(bool_limits_tuple), std::get< 7>(unsigned_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 1>(bool_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 7);
        }
    }

    {
        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 0>(signed_integral_limits_tuple), std::get< 0>(signed_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 0>(signed_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(signed_integral_limits_tuple), std::get< 1>(signed_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 1>(signed_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(signed_integral_limits_tuple), std::get< 2>(signed_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 2>(signed_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(signed_integral_limits_tuple), std::get< 3>(signed_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 3>(signed_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 4>(signed_integral_limits_tuple), std::get< 4>(signed_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 4>(signed_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 5>(signed_integral_limits_tuple), std::get< 5>(signed_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 5>(signed_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 6>(signed_integral_limits_tuple), std::get< 6>(signed_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 6>(signed_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 7>(signed_integral_limits_tuple), std::get< 7>(signed_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 7>(signed_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }
    }

    {
        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 0>(unsigned_integral_limits_tuple), std::get< 0>(unsigned_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 0>(unsigned_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(unsigned_integral_limits_tuple), std::get< 2>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(unsigned_integral_limits_tuple), std::get< 2>(float_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(unsigned_integral_limits_tuple), std::get< 1>(bool_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(unsigned_integral_limits_tuple), std::get< 1>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(unsigned_integral_limits_tuple), std::get< 3>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(unsigned_integral_limits_tuple), std::get< 5>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(unsigned_integral_limits_tuple), std::get< 7>(unsigned_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 1>(unsigned_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 7);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(unsigned_integral_limits_tuple), std::get< 2>(unsigned_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 2>(unsigned_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(unsigned_integral_limits_tuple), std::get< 2>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(unsigned_integral_limits_tuple), std::get< 2>(float_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(unsigned_integral_limits_tuple), std::get< 1>(bool_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(unsigned_integral_limits_tuple), std::get< 1>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(unsigned_integral_limits_tuple), std::get< 3>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(unsigned_integral_limits_tuple), std::get< 5>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(unsigned_integral_limits_tuple), std::get< 7>(unsigned_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 3>(unsigned_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 7);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 4>(unsigned_integral_limits_tuple), std::get< 4>(unsigned_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 4>(unsigned_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 5>(unsigned_integral_limits_tuple), std::get< 2>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 5>(unsigned_integral_limits_tuple), std::get< 2>(float_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 5>(unsigned_integral_limits_tuple), std::get< 1>(bool_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 5>(unsigned_integral_limits_tuple), std::get< 1>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 5>(unsigned_integral_limits_tuple), std::get< 3>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 5>(unsigned_integral_limits_tuple), std::get< 5>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 5>(unsigned_integral_limits_tuple), std::get< 7>(unsigned_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 5>(unsigned_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 7);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 6>(unsigned_integral_limits_tuple), std::get< 6>(unsigned_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 6>(unsigned_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 7>(unsigned_integral_limits_tuple), std::get< 2>(int_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 7>(unsigned_integral_limits_tuple), std::get< 2>(float_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 7>(unsigned_integral_limits_tuple), std::get< 1>(bool_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 7>(unsigned_integral_limits_tuple), std::get< 1>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 7>(unsigned_integral_limits_tuple), std::get< 3>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 7>(unsigned_integral_limits_tuple), std::get< 5>(unsigned_integral_limits_tuple)) == true);
            BOOST_CHECK(comd::arithmetic_compare(std::get< 7>(unsigned_integral_limits_tuple), std::get< 7>(unsigned_integral_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 7>(unsigned_integral_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 7);
        }
    }

    {
        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 0>(floating_point_limits_tuple), std::get< 0>(floating_point_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 0>(floating_point_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 1>(floating_point_limits_tuple), std::get< 1>(floating_point_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 1>(floating_point_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 2>(floating_point_limits_tuple), std::get< 2>(floating_point_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 2>(floating_point_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 3>(floating_point_limits_tuple), std::get< 3>(floating_point_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 3>(floating_point_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 4>(floating_point_limits_tuple), std::get< 4>(floating_point_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 4>(floating_point_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }

        {
            BOOST_CHECK(comd::arithmetic_compare(std::get< 5>(floating_point_limits_tuple), std::get< 5>(floating_point_limits_tuple)) == true);

            size_t count = 0;
            com::tuple_visit(
                [&](const auto& v) { count += comd::arithmetic_compare(std::get< 5>(floating_point_limits_tuple), v) ? 1 : 0; },
                non_arithmetic_tuple, int_tuple, float_tuple, char_tuple, bool_limits_tuple, signed_integral_limits_tuple, unsigned_integral_limits_tuple, floating_point_limits_tuple
            );
            BOOST_CHECK(count == 1);
        }
    }
}


/*
 * Copyright (c) 2019 Opticks Team. All Rights Reserved.
 *
 * This file is part of Opticks
 * (see https://bitbucket.org/simoncblyth/opticks).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <bitset>
#include <boost/math/special_functions/sign.hpp>

#include "NField3.hpp"
#include "NGrid3.hpp"

#include "OPTICKS_LOG.hh"

#include "NGLM.hpp"
#include "NBox.hpp"
#include "NBBox.hpp"

typedef NField<glm::vec3,glm::ivec3,3> F3 ;
typedef NGrid<glm::vec3,glm::ivec3,3> G3 ;

template struct NGrid<glm::vec3, glm::ivec3, 3> ;

void test_cross_isosurface_0(const F3& field, const G3& grid)
{
    glm::ivec3 ijk[2] = {{0,0,0},{0,0,0}} ;
    glm::vec3  fpos[2];
    glm::vec3  fpos2[2];
    glm::vec3  pos[2];
    float sdf[2];

    int ncross = 0 ;
    for(int loc=0 ; loc < grid.nloc - 1 ; loc++)
    {
        ijk[0] = loc > 0 ? ijk[1] : grid.ijk(loc) ;
        ijk[1] = grid.ijk(loc+1) ;

        fpos[0] = loc > 0 ? fpos[1] : grid.fpos(ijk[0]);
        fpos[1] = grid.fpos(ijk[1]);

        fpos2[0] = loc > 0 ? fpos2[1] : grid.fpos(loc);
        fpos2[1] = grid.fpos(loc+1);

        assert( fpos2[0] == fpos[0] );
        assert( fpos2[1] == fpos[1] );

        sdf[0] = loc > 0 ? sdf[1] : field(fpos[0]);
        sdf[1] = field(fpos[1]);

        pos[0] = loc > 0 ? pos[1] : field.position(fpos[0]);
        pos[1] = field.position(fpos[1]);

        bool cross_isosurface = boost::math::sign(sdf[0]) != boost::math::sign(sdf[1]) ;

        if(cross_isosurface)
        {
            ncross++ ;
            std::cout << " loc " << std::setw(5) << loc
                      << " ijk[0] " << glm::to_string(ijk[0])
                      << " ijk[1] " << glm::to_string(ijk[0])
                      << " pos[0] " << glm::to_string(pos[0])
                      << " pos[1] " << glm::to_string(pos[1])
                      << " sdf[0] " << sdf[0]
                      << " sdf[1] " << sdf[1]
                      << std::endl
                      ;
            }
    }

    LOG(info) << "(test_cross_isosurface_0) ncross " << ncross ;
}

void test_cross_isosurface_1(const F3& field, const G3& grid)
{
    int ncross = 0 ;
    float sdf_prior = 0 ;
    for(int loc=0 ; loc < grid.nloc ; loc++)
    {
        glm::vec3 fpos = grid.fpos(loc);
        float sdf = field(fpos);

        bool cross_isosurface = loc > 0 && boost::math::sign(sdf) != boost::math::sign(sdf_prior) ;
        if(cross_isosurface) ncross++ ;

        sdf_prior = sdf ;
    }
    LOG(info) << "(test_cross_isosurface_1) ncross " << ncross ;
}

void test_zcorners( const F3& field, const G3& grid)
{
    glm::vec3 fpos[3] = {{0.f, 0.f, 0.f}, {0.5f, 0.5f, 0.5f}, {1.f, 1.f, 1.f}} ;
    for(int i=0 ; i < 3 ; i++) field.zcorners( fpos[i] , grid.elem );
}

void test_cross_isosurface_2(const F3& field, const G3& grid)
{
    int ncross = 0 ;
    float sdf_prior = 0 ;
    for(int loc=0 ; loc < grid.nloc ; loc++)
    {
        glm::vec3 fpos = grid.fpos(loc);
        float sdf = field(fpos);

        bool cross_isosurface = loc > 0 && boost::math::sign(sdf) != boost::math::sign(sdf_prior) ;
        if(cross_isosurface)
        {
            ncross++ ;
            int zc = field.zcorners( fpos, grid.elem );
            LOG(info)
                << " zcorners " << std::setw(3) << zc
                << " 0x" << std::setfill('0') << std::setw(2) << std::hex << zc
                << " 0b" << std::bitset<8>(zc)
                 << std::dec
            ;
        }
        sdf_prior = sdf ;
    }
    LOG(info) << "(test_cross_isosurface_1) ncross " << ncross ;
}

int main(int argc, char** argv)
{
    OPTICKS_LOG(argc, argv);

    nbox* world = make_box(0,0,0,11) ;
    nbbox wbb = world->bbox() ;

    nbox* obj = make_box(0,0,0,7) ;

    std::function<float(float,float,float)> fn = obj->sdf();

    glm::vec3 wbb_min(wbb.min.x, wbb.min.y, wbb.min.z );
    glm::vec3 wbb_max(wbb.max.x, wbb.max.y, wbb.max.z );

    F3 field( &fn , wbb_min, wbb_max );
    LOG(info) << field.desc() ;

    G3 grid(3);
    LOG(info) << grid.desc() ;

    test_cross_isosurface_2(field, grid);

    return 0 ;
}


// sdf: Triangle mesh to signed-distance function (SDF) library
// Copyright Alex Yu 2020
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. 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.

#pragma once
#ifndef SDF_SDF_F15B6437_01FD_4DBE_AB0D_BC1EE8ACC4C4
#define SDF_SDF_F15B6437_01FD_4DBE_AB0D_BC1EE8ACC4C4

#include <Eigen/Core>
#include <cstdint>

#include <memory>
#include <vector>
#ifdef __GNUC__
#include <experimental/propagate_const>
#endif
#include <Eigen/Geometry>

namespace sdf {

using Index = uint32_t;
using Points = Eigen::Matrix<float, Eigen::Dynamic, 3, Eigen::RowMajor>;
using Points2D = Eigen::Matrix<float, Eigen::Dynamic, 2, Eigen::RowMajor>;
using Triangles = Eigen::Matrix<Index, Eigen::Dynamic, 3, Eigen::RowMajor>;
using Matrix =
    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using Vector = Eigen::Matrix<float, Eigen::Dynamic, 1>;

namespace util {

template <class T>
// 3D point to line shortest distance SQUARED
T dist_point2line(
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& p,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& a,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& b) {
    Eigen::Matrix<T, 1, 3> ap = p - a, ab = b - a;
    return (ap - (ap.dot(ab) / ab.squaredNorm()) * ab).squaredNorm();
}

// 3D point to line segment shortest distance SQUARED
template <class T>
T dist_point2lineseg(
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& p,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& a,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& b) {
    Eigen::Matrix<T, 1, 3> ap = p - a, ab = b - a;
    T t = ap.dot(ab) / ab.squaredNorm();
    t = std::max(T(0.0), std::min(T(1.0), t));
    return (ap - t * ab).squaredNorm();
}

template <class T>
const Eigen::Matrix<T, 1, 3, Eigen::RowMajor> normal(
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& a,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& b,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& c) {
    return (b - a).cross(c - a);
}

template <class T>
// Find barycentric coords of p in triangle (a,b,c) in 3D
// (p does NOT have to be projected to plane beforehand)
// normal, area_abc to be computed using util::normal,
// where normal is normalized vector, area is magnitude
Eigen::Matrix<T, 1, 3, Eigen::RowMajor> bary(
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& p,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& a,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& b,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& c,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& normal,
    float area_abc) {
    float area_pbc = normal.dot((b - p).cross(c - p));
    float area_pca = normal.dot((c - p).cross(a - p));

    Eigen::Matrix<T, 1, 3> uvw;
    uvw.x() = area_pbc / area_abc;
    uvw.y() = area_pca / area_abc;
    uvw.z() = T(1.0) - uvw.x() - uvw.y();

    return uvw;
}

template <class T>
// 3D point to triangle shortest distance SQUARED
// normal, area_abc to be computed using util::normal,
// where normal is normalized vector, area is magnitude
T dist_point2tri(
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& p,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& a,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& b,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& c,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 3, Eigen::RowMajor>>& normal,
    float area) {
    const Eigen::Matrix<T, 1, 3> uvw = bary<T>(p, a, b, c, normal, area);
    if (uvw[0] < 0) {
        return dist_point2lineseg<T>(p, b, c);
    } else if (uvw[1] < 0) {
        return dist_point2lineseg<T>(p, a, c);
    } else if (uvw[2] < 0) {
        return dist_point2lineseg<T>(p, a, b);
    } else {
        return (uvw[0] * a + uvw[1] * b + uvw[2] * c - p).squaredNorm();
    }
}

template <class T>
Eigen::Matrix<T, 1, 3, Eigen::RowMajor> bary2d(
    const Eigen::Ref<const Eigen::Matrix<T, 1, 2, Eigen::RowMajor>>& p,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 2, Eigen::RowMajor>>& a,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 2, Eigen::RowMajor>>& b,
    const Eigen::Ref<const Eigen::Matrix<T, 1, 2, Eigen::RowMajor>>& c) {
    Eigen::Matrix<T, 1, 2, Eigen::RowMajor> v0 = b - a, v1 = c - a, v2 = p - a;
    Eigen::Matrix<T, 1, 3> result;
    const float invden = 1.f / (v0.x() * v1.y() - v1.x() * v0.y());
    result[1] = (v2.x() * v1.y() - v1.x() * v2.y()) * invden;
    result[2] = (v0.x() * v2.y() - v2.x() * v0.y()) * invden;
    result[0] = 1.0f - result.template tail<2>().sum();
    return result;
}
}  // namespace util

// Signed distance function utility for watertight meshes.
//
// Basic usage: SDF sdf(verts, faces); Vector sdf_vals = sdf(query_points);
// Get nearest neighbor (verts) indices: sdf.nn(query_points);
// Check containment (returns bool): sdf.contains(query_points);
struct SDF {
    // Construct SDF instance from triangle mesh with given vertices and faces
    // Note: Mesh is assumed to be watertight and all vertex positions are
    // expected to be free of nans/infs
    //
    // Basic usage: SDF sdf(verts, faces); Vector sdf_vals = sdf(query_points);
    // Get nearest neighbor (verts) indices: sdf.nn(query_points);
    // Check containment (returns bool): sdf.contains(query_points);
    //
    // @param verts mesh vertices. If the contents of this matrix are modified,
    // please call SDF::update() to update the internal representation.
    // Else the results will be incorrect.
    // @param faces mesh faces. The contents of this matrix should not be
    // modified for the lifetime of this instance.
    // @param robust whether to use robust mode. In robust mode,
    // @param copy whether to make a copy of the data instead of referencing it
    // SDF/containment computation is robust to mesh self-intersections and
    // facewinding but is slower.
    SDF(Eigen::Ref<const Points> verts, Eigen::Ref<const Triangles> faces,
        bool robust = true, bool copy = false);
    ~SDF();

    /*** PRIMARY INTERFACE ***/
    // Fast approximate signed-distance function.
    // Points inside the mesh have positive sign and outside have negative sign.
    //
    // Method: computes minimum distance to a triangular face incident to
    // the nearest vertex for each point.
    //
    // @param points input points
    // @param trunc_aabb if true, returns -FLT_MAX for all points outside mesh's
    // bounding box
    // @return approx SDF values at input points
    //
    // WARNING: if robust=false (from constructor), this WILL FAIL if the mesh
    // has self-intersections. In particular, the signs of points inside the
    // mesh may be flipped.
    Vector operator()(Eigen::Ref<const Points> points,
                      bool trunc_aabb = false) const;

    // Return exact nearest neighbor vertex index for each point (index as in
    // input verts)
    Eigen::VectorXi nn(Eigen::Ref<const Points> points) const;

    // Return 1 for each point inside/on surface of the mesh and 0 for outside.
    //
    // @param points input points
    // @return indicator of whether each point is in OR on surface of mesh
    //
    // WARNING: if robust=false (from constructor), this WILL FAIL if the mesh
    // has self-intersections.
    Eigen::Matrix<bool, Eigen::Dynamic, 1> contains(
        Eigen::Ref<const Points> points) const;

    // Call if vertex positions have been updated to rebuild the KD tree
    // and update face normals+areas
    void update();

    /*** MISC UTILITIES ***/
    // Sample 'num_points' points uniformly on surface, output (num_points, 3).
    // Note: this takes O(num_points * log(num_faces)) time.
    Points sample_surface(int num_points) const;

    /*** DATA ACCESSORS ***/
    // Get adjacent faces of point at verts[pointid]
    const std::vector<int>& adj_faces(int pointid) const;

    // Get total surface area of mesh
    const float surface_area() const;

    // Get vector of face areas, shape (num_faces)
    const Vector& face_areas() const;

    // Get matrix of face normals, shape (num_faces, 3).
    // normal of face i (from faces passed to constructor) is in row i
    const Points& face_normals() const;

    // Get AABB of entire mesh, shape (6).
    // (minx, miny, minz, maxx, maxy, maxz)
    Eigen::Ref<const Eigen::Matrix<float, 6, 1>> aabb() const;

    // Get faces
    Eigen::Ref<const Triangles> faces() const;
    Eigen::Ref<Triangles> faces_mutable();

    // Get verts
    Eigen::Ref<const Points> verts() const;
    Eigen::Ref<Points> verts_mutable();

    // Whether SDF is in robust mode
    const bool robust;

    // Whether we own data
    const bool own_data;

   private:
    // Optional owned data
    Points owned_verts;
    Triangles owned_faces;

    struct Impl;
#ifdef __GNUC__
    std::experimental::propagate_const<std::unique_ptr<Impl>> p_impl;
#else
    std::unique_ptr<Impl> p_impl;
#endif
};

// Image-space raycast renderer utility for watertight meshes.
//
// Renders depth maps (render_depth), object mask (render_mask), and
// vertex ids (render_nn)
// using raycasting in image space. Also supports querying these for
// arbitrary continuous points (x, y) in image space
// (operator(), contains, nn).
//
// By image space we mean the space of (x,y,z)
// where a pinhole camera perspective projection was applied to x,y.
// This class is somehow similar to sdf::SDF but in
// image space and only available on the image plane.
//
// NOTE: We assume no objects are present where z <= 0. This allows
// us to use 2D data structures and skip a check.
//
// This is not a very efficient method.
// If object has relatively few points compared to image size,
// painter's algorithm (implemented in sxyu/avatar) probably performs better.
//
// Assumes camera is at origin facing +z, where up is -y and right is +x.
// Note the coordinate system is right-handed.
struct Renderer {
    // Construct software renderer
    // @param verts mesh vertices. If the contents of this matrix are modified,
    // please call Renderer::update() to update the internal representation.
    // Else the results will be incorrect.
    // @param faces mesh faces. The contents of this matrix should not be
    // modified for the lifetime of this instance.
    // @param width image width
    // @param height image height
    // @param fx focal length x
    // @param fy focal length y
    // @param cx principal point x
    // @param cy principal point y
    // @param copy whether to make a copy of the data instead of referencing it
    // SDF/containment computation is robust to mesh self-intersections and
    // facewinding but is slower.
    Renderer(Eigen::Ref<const Points> verts, Eigen::Ref<const Triangles> faces,
             int width = 1080, int height = 1080, float fx = 2600.f,
             float fy = 2600.f, float cx = 540.f, float cy = 540.f,
             bool copy = false);

    // Destructor, pImpl pattern needs this
    ~Renderer();

    // *** PRIMARY INTERFACE ***
    // Render (height, width) depth map of the mesh.
    // @return Each pixel will be distance from z=0 plane and 0 if no object is
    // present.
    Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    render_depth() const;

    // Render (height, width) mask.
    // @return Each pixel is 1 where object is present, 0 else.
    Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    render_mask() const;

    // Render (height, width) vertex map, i.e. vertex id nearest to raycast hit
    // at each pixel. Each pixel is -1 if empty space, index of vertex in verts
    // else
    // @param fill_outside if true, instead of returning -1 for empty space,
    // finds nearest-neighbor vertex in 2d and uses its index
    Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    render_nn(bool fill_outside = false) const;

    // Compute depth at 2D points
    // (render_depth for continuous points)
    Vector operator()(Eigen::Ref<const Points2D> points) const;

    // Compute mask at 2D points (1 means inside)
    // (render_mask for continuous points)
    Eigen::Matrix<bool, Eigen::Dynamic, 1> contains(
        Eigen::Ref<const Points2D> points) const;

    // Compute vertex id hit by raycast at 2D points
    // (render_vertex for continuous points)
    // @param fill_outside if true, instead of returning -1 for empty space,
    // finds nearest-neighbor vertex in 2d and uses its index
    Eigen::VectorXi nn(Eigen::Ref<const Points2D> points,
                       bool fill_outside = false) const;

    // Call if vertex positions have been updated to rebuild the KD tree
    // and update face normals+areas
    void update();

    /*** DATA ACCESSORS ***/
    // Get faces
    Eigen::Ref<const Triangles> faces() const;
    Eigen::Ref<Triangles> faces_mutable();

    // Get verts
    Eigen::Ref<const Points> verts() const;
    Eigen::Ref<Points> verts_mutable();

    // Whether we own data
    const bool own_data;

   private:
    // Optional owned data
    Points owned_verts;
    Triangles owned_faces;

    struct Impl;
#ifdef __GNUC__
    std::experimental::propagate_const<std::unique_ptr<Impl>> p_impl;
#else
    std::unique_ptr<Impl> p_impl;
#endif
};

}  // namespace sdf

#endif  // ifndef SDF_SDF_F15B6437_01FD_4DBE_AB0D_BC1EE8ACC4C4


/*
 *  Copyright (c) 2009, Rene Wagner
 *  All rights reserved.
 *
 *  Author: Rene Wagner <rw@nelianur.org>
 *
 *  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 Rene Wagner nor the names of any
 *     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.
 */

#ifndef __UKFOM_LAPACK_INVERT_HPP__
#define __UKFOM_LAPACK_INVERT_HPP__

#include "lapack.h"

#include <Eigen/Core>

namespace ukf {
namespace lapack {

// import most common Eigen types
USING_PART_OF_NAMESPACE_EIGEN

template<size_t P>
Matrix<double, P, P> invert(const Matrix<double, P, P> &m)
{
	Matrix<double, P, P> tmp = m;

	/* first call DGETRF */
	int M = tmp.rows();
	int N = tmp.cols();
	int LDA = tmp.stride();
	int IPIV[M];

	int INFO;

	dgetrf_(&M, &N, tmp.data(), &LDA, IPIV, &INFO);

	if(INFO != 0)
		throw "dgetrf failed";

	/* now call DGETRI */
	int LWORK = 64 * N; // FIXME: retrieve block size from ILAENV
	double WORK[LWORK];

	dgetri_(&N, tmp.data(), &LDA, IPIV, WORK, &LWORK, &INFO);

	if(INFO != 0)
		throw "dgetri failed";

	return tmp;
}

} // namespace lapack
} // namespace ukf

#endif // __UKFOM_LAPACK_INVERT_HPP__


#pragma once
#include <limits>
#include <vector>
#include <future>
#include <Eigen/Dense>
#include "Scene.hpp"
#include "camera/Camera.hpp"

class Render
{
    // TODO: Support custom type instead uint8_t
    using Type = uint8_t;
    using Color = Eigen::Matrix<Type, 3, 1>;
    using ColorBuffer = std::vector<Color>;

private:
    int width;
    int height;
    int size;

    ColorBuffer color_buffer;

public:
    Render(int width, int height) : width(width), height(height), size(width * height)
    {
        color_buffer.resize(size);
    };

    void draw(const Scene &scene, const Camera &camera)
    {
        auto calcPixel = [&](auto offsetX, auto offsetY)
        {
            auto x = (static_cast<float>(offsetX) / width) * 2 - 1;
            auto y = (static_cast<float>(offsetY) / height) * 2 - 1;

            auto ray = camera.generate_primary_ray(x, y);
            auto color = scene.cast_ray(ray);

            constexpr auto max = std::numeric_limits<Type>::max();
            constexpr auto cast = [](float color) { return static_cast<Type>(max * color); };

            return Color{cast(color[0]), cast(color[1]), cast(color[2])};
        };

        std::vector<std::future<Color>> futures;
        futures.reserve(size);
        for (auto i = 0; i < height; i++)
            for (auto j = 0; j < width; j++)
                futures.emplace_back(std::async(calcPixel, j, i));

        for (int m = 0; m < size; m++)
        {
            futures[m].wait();
            // TODO: Progress
        }

        for (int m = 0; m < size; m++)
            color_buffer[m] = futures[m].get();
    };

    ColorBuffer &get_frame_buffer() noexcept { return color_buffer; };
};


#include <scitbx/array_family/boost_python/flex_wrapper.h>
#include <scitbx/array_family/boost_python/flex_pickle_single_buffered.h>
#include <scitbx/array_family/boost_python/flex_wrapper_complex.h>
#include <scitbx/array_family/boost_python/numpy_bridge.hpp>
#include <scitbx/array_family/versa_matrix.h>
#include <scitbx/matrix/move.h>
#include <boost/python/make_constructor.hpp>

namespace scitbx { namespace af { namespace boost_python {

namespace {

  versa<std::complex<double>, flex_grid<> >*
  from_pair_of_flex_double(
    versa<double, flex_grid<> > const& reals,
    versa<double, flex_grid<> > const& imags)
  {
    SCITBX_ASSERT(reals.size() == imags.size());
    typedef versa<std::complex<double>, flex_grid<> > result_type;
    result_type result(
      reals.accessor(), init_functor_null<std::complex<double> >());
    std::complex<double>*c = result.begin();
    std::complex<double>*c_end = result.end();
    const double* r = reals.begin();
    const double* i = imags.begin();
    while (c != c_end) {
      *c++ = std::complex<double>(*r++, *i++);
    }
    return new result_type(result);
  }

  boost::python::tuple
  part_names()
  {
    return boost::python::make_tuple("real", "imag");
  }

  boost::python::tuple
  parts(
    versa<std::complex<double>, flex_grid<> > const& O)
  {
    return boost::python::make_tuple(real(O), imag(O));
  }

  versa<std::complex<double>, flex_grid<> >
  mul_ac_ar(
    versa<std::complex<double>, flex_grid<> > const& a1,
    versa<             double , flex_grid<> > const& a2)
  {
    return a1 * a2;
  }

  versa<std::complex<double>, flex_grid<> >
  imul_ac_ar(
            versa<std::complex<double>, flex_grid<> > & a1,
            versa<             double , flex_grid<> > const& a2)
  {
    return a1 *= a2;
  }

  versa<std::complex<double>, c_grid<2> >
  matrix_multiply_complex_matrix_complex_matrix(
    const_ref<std::complex<double>, c_grid<2> > const& a,
    const_ref<std::complex<double>, c_grid<2> > const& b)
  {
    return matrix_multiply(a, b);
  }

  versa<std::complex<double>, c_grid<2> >
  matrix_multiply_complex_matrix_real_matrix(
    const_ref<std::complex<double>, c_grid<2> > const& a,
    const_ref<double, c_grid<2> > const& b)
  {
    return matrix_multiply(a, b);
  }

  bool
  all_approx_equal_a_a(const_ref<std::complex<double> > const& self,
                       const_ref<std::complex<double> > const& other,
                       double relative_error=1e-6)
  {
    return self.all_approx_equal(other, relative_error);
  }

  bool
  all_approx_equal_a_s(const_ref<std::complex<double> > const& self,
                       std::complex<double>  other,
                       double relative_error=1e-6)
  {
    return self.all_approx_equal(other, relative_error);
  }

  bool
  all_approx_equal_relatively_a_a(const_ref<std::complex<double> > const& self,
                                  const_ref<std::complex<double> > const& other,
                                  double relative_error=1e-6)
  {
    return self.all_approx_equal_relatively(other, relative_error);
  }

  bool
  all_approx_equal_relatively_a_s(const_ref<std::complex<double> > const& self,
                                  std::complex<double>  other,
                                  double relative_error=1e-6)
  {
    return self.all_approx_equal_relatively(other, relative_error);
  }

} // namespace <anonymous>

  void wrap_flex_complex_double()
  {
    using namespace boost::python;
    using boost::python::arg;
    typedef flex_wrapper<std::complex<double> > f_w;
    scope local_scope;
    f_w::numeric_common("complex_double", local_scope)
      .def_pickle(flex_pickle_single_buffered<std::complex<double> >())
      .def("__init__", make_constructor(
        from_pair_of_flex_double, default_call_policies(), (
          arg("reals"), arg("imags"))))
      .def("__init__", make_constructor(
        flex_complex_double_from_numpy_array, default_call_policies()))
      .def("part_names", part_names)
      .staticmethod("part_names")
      .def("parts", parts)
      .def("all_approx_equal",
        all_approx_equal_a_a, (
          arg("other"),
          arg("tolerance")=1e-6))
      .def("all_approx_equal",
        all_approx_equal_a_s, (
          arg("other"),
          arg("tolerance")=1e-6))
      .def("all_approx_equal_relatively",
        all_approx_equal_relatively_a_a, (
          arg("other"),
          arg("relative_error")=1e-6))
      .def("all_approx_equal_relatively",
        all_approx_equal_relatively_a_s, (
          arg("other"),
          arg("relative_error")=1e-6))
      .def("__mul__", mul_ac_ar)
      .def("__imul__", imul_ac_ar)
      .def("__rmul__", mul_ac_ar)
      .def("matrix_multiply", matrix_multiply_complex_matrix_complex_matrix)
      .def("matrix_multiply", matrix_multiply_complex_matrix_real_matrix)
      .def("matrix_transpose",
        (versa<std::complex<double>, c_grid<2> >(*)(
           const_ref<std::complex<double>, c_grid<2> > const&))
             matrix_transpose)
      .def("matrix_packed_u_as_symmetric",
        (versa<std::complex<double>, c_grid<2> >(*)(
          const_ref<std::complex<double> > const&))
            matrix::packed_u_as_symmetric)
      .def("matrix_packed_u_diagonal",
        (shared<std::complex<double> >(*)(
          const_ref<std::complex<double> > const&))
            matrix::packed_u_diagonal)
      .def("matrix_copy_block",
        (versa<std::complex<double>, c_grid<2> >(*)(
          const_ref<std::complex<double>, c_grid<2> > const&,
          unsigned, unsigned, unsigned, unsigned))
            matrix::copy_block, (
              arg("i_row"),
              arg("i_column"),
              arg("n_rows"),
              arg("n_columns")))
      .def("matrix_paste_block_in_place",
        (void(*)(
          ref<std::complex<double>, c_grid<2> > const&,
          const_ref<std::complex<double>, c_grid<2> > const&,
          unsigned, unsigned))
            matrix::paste_block_in_place, (
              arg("block"),
              arg("i_row"),
              arg("i_column")))
      .def("as_numpy_array", flex_complex_double_as_numpy_array, (
        arg("optional")=false))
    ;
    def("mean", f_w::mean_a);
    def("mean_sq", f_w::mean_sq_a);
    flex_wrapper_complex_functions<double>::wrap(local_scope);
  }

}}} // namespace scitbx::af::boost_python