/// Personal calibration of kappa angle for use with RGB-D gaze estimation

////////////////////
/// DEPENDENCIES ///
////////////////////

// ROS 2
#include <rclcpp/rclcpp.hpp>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_ros/transform_listener.h>

// ROS 2 interfaces
#include <geometry_msgs/msg/point_stamped.hpp>
#include <gaze_msgs/msg/gaze_binocular_stamped.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

// Eigen
#include <Eigen/Geometry>

// PCL
#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>

/////////////////
/// CONSTANTS ///
/////////////////

/// The name of this node
const std::string NODE_NAME = "rgbd_gaze_calibration_kappa";
/// Size of the queue size used by the synchronizer in its policy
const uint8_t SYNCHRONIZER_QUEUE_SIZE = 50;

/// Index of the left eye
const uint8_t EYE_LEFT = 0;
/// Index of the right eye
const uint8_t EYE_RIGHT = 1;

const float VISUAL_OPTICAL_AXIS_LENGTH = 5.0;
const float VISUAL_OPTICAL_AXIS_WIDTH = 0.001;
const float VISUAL_OPTICAL_AXIS_COLOR[] = {1.0, 0, 0, 1};

const float VISUAL_REAL_GAZE_WIDTH = 0.0015;
const float VISUAL_REAL_GAZE_COLOR[] = {0, 1.0, 0, 1};

/////////////
/// TYPES ///
/////////////

/// Policy of the synchronizer
typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::msg::PoseStamped,
                                                        gaze_msgs::msg::GazeBinocularStamped,
                                                        geometry_msgs::msg::PointStamped>
    synchronizer_policy;

////////////////////////
/// HELPER FUNCTIONS ///
////////////////////////

namespace Eigen
{
  void fromMsg(const geometry_msgs::msg::Vector3 &input, Eigen::Vector3d &output)
  {
    output.x() = input.x;
    output.y() = input.y;
    output.z() = input.z;
  }
} // namespace Eigen

namespace tf2
{
  Eigen::Vector3d transform(Eigen::Vector3d &input, const Eigen::Isometry3d &transformation)
  {
    return transformation * input;
  }

  Eigen::ParametrizedLine<double, 3> transform(Eigen::ParametrizedLine<double, 3> &parametrized_line, const Eigen::Isometry3d &transformation)
  {
    return Eigen::ParametrizedLine<double, 3>(transformation * parametrized_line.origin(), transformation.rotation() * parametrized_line.direction());
  }
} // namespace tf2

double compute_angle(double x1, double y1, double x2, double y2)
{
  double norm1 = std::sqrt(x1 * x1 + y1 * y1);
  double norm2 = std::sqrt(x2 * x2 + y2 * y2);
  x1 /= norm1;
  y1 /= norm1;
  x2 /= norm2;
  y2 /= norm2;

  return std::atan2(x1 * y2 - y1 * x2, x1 * x2 + y1 * y2);
}

//////////////////
/// NODE CLASS ///
//////////////////

/// Class representation of this node
class RgbdGazeCalibrationKappa : public rclcpp::Node
{
public:
  /// Constructor
  RgbdGazeCalibrationKappa();

private:
  /// Subscriber to the head pose
  message_filters::Subscriber<geometry_msgs::msg::PoseStamped> sub_head_pose_;
  /// Subscriber to the optical axis
  message_filters::Subscriber<gaze_msgs::msg::GazeBinocularStamped> sub_optical_axes_;
  /// Subscriber to the head pose
  message_filters::Subscriber<geometry_msgs::msg::PointStamped> sub_scene_point_;
  /// Synchronizer of the subscribers
  message_filters::Synchronizer<synchronizer_policy> synchronizer_;

  /// Publisher of visualisation markers
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;

  /// Buffer for tf2 transforms
  tf2_ros::Buffer tf2_buffer_;
  /// Listener of tf2 transforms
  tf2_ros::TransformListener tf2_listener_;

  // /// Vector of observed kappa angle pairs <alpha, beta>
  // std::vector<std::pair<double, double>> kappa_observations_[2];

  /// Kappa angle observations represented as point cloud (used for outlier removal)
  pcl::PointCloud<pcl::PointXYZ>::Ptr pc_kappa_observations_[2];

  uint64_t epoch_;

  /// Callback called each time a message is received on all topics
  void synchronized_callback(
      const geometry_msgs::msg::PoseStamped::SharedPtr msg_head_pose,
      const gaze_msgs::msg::GazeBinocularStamped::SharedPtr msg_optical_axes,
      const geometry_msgs::msg::PointStamped::SharedPtr msg_scene_point);
};

RgbdGazeCalibrationKappa::RgbdGazeCalibrationKappa() : Node(NODE_NAME),
                                                       sub_head_pose_(this, "head_pose"),
                                                       sub_optical_axes_(this, "optical_axes"),
                                                       sub_scene_point_(this, "scene_point"),
                                                       synchronizer_(synchronizer_policy(SYNCHRONIZER_QUEUE_SIZE), sub_head_pose_, sub_optical_axes_, sub_scene_point_),
                                                       tf2_buffer_(this->get_clock()),
                                                       tf2_listener_(tf2_buffer_),
                                                       epoch_(0)
{
  // Synchronize the subscriptions under a single callback
  synchronizer_.registerCallback(&RgbdGazeCalibrationKappa::synchronized_callback, this);

  // Parameters of the element
  this->declare_parameter<bool>("publish_markers", true);

  // Register publisher of the visualisation markers
  if (this->get_parameter("publish_markers").get_value<bool>())
  {
    rclcpp::QoS qos_markers = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
    pub_markers_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("visualisation_markers", qos_markers);
  }

  // Initialise point cloud
  for (uint8_t eye = 0; eye < 2; eye++)
  {
    pc_kappa_observations_[eye].reset(new pcl::PointCloud<pcl::PointXYZ>);
    pc_kappa_observations_[eye]->is_dense = true;
    pc_kappa_observations_[eye]->height = 1;
    pc_kappa_observations_[eye]->width = 0;
  }

  RCLCPP_INFO(this->get_logger(), "Node initialised");
}

void RgbdGazeCalibrationKappa::synchronized_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg_head_pose,
                                                     const gaze_msgs::msg::GazeBinocularStamped::SharedPtr msg_optical_axes,
                                                     const geometry_msgs::msg::PointStamped::SharedPtr msg_scene_point)
{
  RCLCPP_DEBUG(this->get_logger(), "Received synchronized messages for processing");
  epoch_++;

  // Convert head pose to Eigen
  Eigen::Isometry3d head_pose;
  tf2::fromMsg(msg_head_pose->pose, head_pose);

  // Convert scene point to Eigen
  Eigen::Vector3d scene_point;
  Eigen::fromMsg(msg_scene_point->point, scene_point);

  // Create a parametrized lines from optical axes
  Eigen::ParametrizedLine<double, 3> optical_axes[2];
  for (uint8_t eye = 0; eye < 2; eye++)
  {
    Eigen::Vector3d origin, direction;
    Eigen::fromMsg(msg_optical_axes->gaze[eye].origin, origin);
    Eigen::fromMsg(msg_optical_axes->gaze[eye].direction, direction);
    optical_axes[eye] = Eigen::ParametrizedLine<double, 3>(origin, direction);
  }

  // Convert scene point into coordinate system of the head pose
  if (msg_scene_point->header.frame_id != msg_head_pose->header.frame_id)
  {
    // Attemp to find transformation between the coordinate frames
    geometry_msgs::msg::TransformStamped transform_stamped;
    try
    {
      transform_stamped = tf2_buffer_.lookupTransform(msg_scene_point->header.frame_id, msg_head_pose->header.frame_id, rclcpp::Time(0));
    }
    catch (tf2::TransformException &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
      return;
    }

    // Transform scene point into coordinate system of optical axes
    scene_point = tf2::transform(scene_point, tf2::transformToEigen(transform_stamped.transform));
  }

  // Convert optical axes into coordinate system of the head pose, if needed
  if (msg_optical_axes->header.frame_id != msg_head_pose->header.frame_id)
  {
    // Attemp to find transformation between the coordinate frames
    geometry_msgs::msg::TransformStamped transform_stamped;
    try
    {
      transform_stamped = tf2_buffer_.lookupTransform(msg_optical_axes->header.frame_id, msg_head_pose->header.frame_id, rclcpp::Time(0));
    }
    catch (tf2::TransformException &ex)
    {
      RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
      return;
    }

    // Transform optical axes into coordinate system of optical axes
    for (uint8_t eye = 0; eye < 2; eye++)
    {
      optical_axes[eye] = tf2::transform(optical_axes[eye], tf2::transformToEigen(transform_stamped.transform));
    }
  }

  // Compute kappa
  Eigen::ParametrizedLine<double, 3> real_gaze[2];
  for (uint8_t eye = 0; eye < 2; eye++)
  {
    // Construct the real gaze based on the scene point
    real_gaze[eye] = Eigen::ParametrizedLine<double, 3>(optical_axes[eye].origin(), (scene_point - optical_axes[eye].origin()).normalized());

    // Transform optical axes and real gaze of both eyes to head coordinate frame (kappa is fixed with this frame)
    Eigen::Vector3d optical_axes_direction_wrt_head = head_pose.inverse().rotation() * optical_axes[eye].direction();
    Eigen::Vector3d real_gaze_direction_wrt_head = head_pose.inverse().rotation() * real_gaze[eye].direction();

    // Compute alpha (around x)
    double alpha = compute_angle(optical_axes_direction_wrt_head.y(), optical_axes_direction_wrt_head.z(),
                                 real_gaze_direction_wrt_head.y(), real_gaze_direction_wrt_head.z());

    // Compute beta (around y)
    double beta = compute_angle(optical_axes_direction_wrt_head.z(), optical_axes_direction_wrt_head.x(),
                                real_gaze_direction_wrt_head.z(), real_gaze_direction_wrt_head.x());

    // Combine kappa angles and store
    // kappa_observations_[eye].push_back(std::pair(alpha, beta));
    pc_kappa_observations_[eye]->push_back(pcl::PointXYZ(alpha, beta, 0));
  }

  // Find the mean kappa for each eye
  struct
  {
    double alpha;
    double beta;
  } kappa_calibrated[2];
  for (uint8_t eye = 0; eye < 2; eye++)
  {
    // Remove outliers
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> statistical_outlier_removal;
    statistical_outlier_removal.setInputCloud(pc_kappa_observations_[eye]);
    statistical_outlier_removal.setMeanK(3);
    statistical_outlier_removal.setStddevMulThresh(1);
    std::vector<int> inliers;
    statistical_outlier_removal.filter(inliers);

    // Find the mean of the inlier kappa observations
    kappa_calibrated[eye].alpha = 0.0;
    kappa_calibrated[eye].beta = 0.0;
    for (auto &inlier : inliers)
    {
      kappa_calibrated[eye].alpha += pc_kappa_observations_[eye]->points[inlier].x;
      kappa_calibrated[eye].beta += pc_kappa_observations_[eye]->points[inlier].y;
    }
    kappa_calibrated[eye].alpha /= inliers.size();
    kappa_calibrated[eye].beta /= inliers.size();
  }

  // Print results
  {
    std::string output_string;
    output_string = "Results of epoch #" + std::to_string(epoch_);
    for (uint8_t eye = 0; eye < 2; eye++)
    {
      std::string eye_side;
      if (eye == EYE_LEFT)
      {
        eye_side = "left";
      }
      else
      {
        eye_side = "right";
      }

      output_string += "\n" + eye_side +
                       ":\n\tkappa:" +
                       "\n\t\talpha: " + std::to_string(kappa_calibrated[eye].alpha) +
                       "\n\t\tbeta: " + std::to_string(kappa_calibrated[eye].beta);
    }
    RCLCPP_INFO(this->get_logger(), output_string);
  }

  // Publish eyeball as a visualisation marker, if desired
  if (this->get_parameter("publish_markers").get_value<bool>())
  {
    visualization_msgs::msg::MarkerArray markers;
    visualization_msgs::msg::Marker default_marker;
    default_marker.header = msg_head_pose->header;
    default_marker.action = visualization_msgs::msg::Marker::ADD;
    for (uint8_t eye = 0; eye < 2; eye++)
    {
      std::string eye_side;
      if (eye == EYE_LEFT)
      {
        eye_side = "_left";
      }
      else
      {
        eye_side = "_right";
      }
      default_marker.ns = std::string(this->get_namespace()) + "kappa_calibration" + eye_side;

      // Optical axis
      {
        visualization_msgs::msg::Marker optical_axis_marker = default_marker;
        optical_axis_marker.id = 0;
        optical_axis_marker.type = visualization_msgs::msg::Marker::ARROW;
        geometry_msgs::msg::Point start, end;
        start = Eigen::toMsg(optical_axes[eye].origin());
        end = Eigen::toMsg(optical_axes[eye].pointAt(VISUAL_OPTICAL_AXIS_LENGTH));
        optical_axis_marker.points.push_back(start);
        optical_axis_marker.points.push_back(end);
        optical_axis_marker.scale.x = VISUAL_OPTICAL_AXIS_WIDTH;
        optical_axis_marker.scale.y =
            optical_axis_marker.scale.z = 0;
        optical_axis_marker.color.r = VISUAL_OPTICAL_AXIS_COLOR[0];
        optical_axis_marker.color.g = VISUAL_OPTICAL_AXIS_COLOR[1];
        optical_axis_marker.color.b = VISUAL_OPTICAL_AXIS_COLOR[2];
        optical_axis_marker.color.a = VISUAL_OPTICAL_AXIS_COLOR[3];
        markers.markers.push_back(optical_axis_marker);
      }

      // Real gaze
      {
        visualization_msgs::msg::Marker real_gaze_marker = default_marker;
        real_gaze_marker.id = 1;
        real_gaze_marker.type = visualization_msgs::msg::Marker::ARROW;
        geometry_msgs::msg::Point start, end;
        start = Eigen::toMsg(real_gaze[eye].origin());
        end = Eigen::toMsg(scene_point);
        real_gaze_marker.points.push_back(start);
        real_gaze_marker.points.push_back(end);
        real_gaze_marker.scale.x =
            real_gaze_marker.scale.y = VISUAL_REAL_GAZE_WIDTH;
        real_gaze_marker.scale.z = 0.0;
        real_gaze_marker.color.r = VISUAL_REAL_GAZE_COLOR[0];
        real_gaze_marker.color.g = VISUAL_REAL_GAZE_COLOR[1];
        real_gaze_marker.color.b = VISUAL_REAL_GAZE_COLOR[2];
        real_gaze_marker.color.a = VISUAL_REAL_GAZE_COLOR[3];
        markers.markers.push_back(real_gaze_marker);
      }
    }
    pub_markers_->publish(markers);
  }
}

////////////
/// MAIN ///
////////////

/// Main function that initiates an object of `RgbdGazeCalibrationKappa` class as the core of this node.
int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<RgbdGazeCalibrationKappa>());
  rclcpp::shutdown();
  return EXIT_SUCCESS;
}


//  (C) Copyright Gennadiy Rozental 2001.
//  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)

//  See http://www.boost.org/libs/test for the library home page.
//
//!@file
//!@brief Floating point comparison with enhanced reporting
// ***************************************************************************

#ifndef BOOST_TEST_TOOLS_FPC_OP_HPP_050915GER
#define BOOST_TEST_TOOLS_FPC_OP_HPP_050915GER

// Boost.Test
#include <boost/test/tools/assertion.hpp>

#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/test/tools/fpc_tolerance.hpp>

// Boost
#include <boost/type_traits/common_type.hpp>
#include <boost/utility/enable_if.hpp>

#include <boost/test/detail/suppress_warnings.hpp>

//____________________________________________________________________________//

namespace boost {
namespace test_tools {
namespace assertion {
namespace op {

// ************************************************************************** //
// **************                   fpctraits                  ************** //
// ************************************************************************** //
// set of floating point comparison traits per comparison OP

template<typename OP>
struct fpctraits {
    // indicate if we should perform the operation with a "logical OR"
    // with the "equality under tolerance".
    static const bool equality_logical_disjunction = true;
};

template <typename Lhs, typename Rhs>
struct fpctraits<op::LT<Lhs,Rhs> > {
    static const bool equality_logical_disjunction = false;
};

template <typename Lhs, typename Rhs>
struct fpctraits<op::GT<Lhs,Rhs> > {
    static const bool equality_logical_disjunction = false;
};

//____________________________________________________________________________//

// ************************************************************************** //
// ************** set of overloads to select correct fpc algo  ************** //
// ************************************************************************** //
// we really only care about EQ vs NE. All other comparisons use direct first
// and then need EQ. For example a <= b (tolerance t) IFF a <= b OR a == b (tolerance t)

template <typename FPT, typename Lhs, typename Rhs, typename OP>
inline assertion_result
compare_fpv( Lhs const& lhs, Rhs const& rhs, OP* cmp_operator)
{
    bool result = cmp_operator->eval_direct(lhs, rhs);
    if(fpctraits<OP>::equality_logical_disjunction) {
        return result || compare_fpv<FPT>(lhs, rhs, (op::EQ<Lhs, Rhs>*)0);
    }
    return result && compare_fpv<FPT>(lhs, rhs, (op::NE<Lhs, Rhs>*)0);
}

//____________________________________________________________________________//

template <typename FPT, typename Lhs, typename Rhs>
inline assertion_result
compare_fpv_near_zero( FPT const& fpv, op::EQ<Lhs,Rhs>* )
{
    fpc::small_with_tolerance<FPT> P( fpc_tolerance<FPT>() );

    assertion_result ar( P( fpv ) );
    if( !ar )
        ar.message() << "Absolute value exceeds tolerance [|" << fpv << "| > "<< fpc_tolerance<FPT>() << ']';

    return ar;
}

//____________________________________________________________________________//

template <typename FPT, typename Lhs, typename Rhs>
inline assertion_result
compare_fpv_near_zero( FPT const& fpv, op::NE<Lhs,Rhs>* )
{
    fpc::small_with_tolerance<FPT> P( fpc_tolerance<FPT>() );

    assertion_result ar( !P( fpv ) );
    if( !ar )
        ar.message() << "Absolute value is within tolerance [|" << fpv << "| < "<< fpc_tolerance<FPT>() << ']';
    return ar;
}

//____________________________________________________________________________//

template <typename FPT, typename Lhs, typename Rhs>
inline assertion_result
compare_fpv( Lhs const& lhs, Rhs const& rhs, op::EQ<Lhs,Rhs>* )
{
    if( lhs == 0 ) {
        return compare_fpv_near_zero( rhs, (op::EQ<Lhs,Rhs>*)0 );
    }
    else if( rhs == 0) {
        return compare_fpv_near_zero( lhs, (op::EQ<Lhs,Rhs>*)0 );
    }
    else {
        fpc::close_at_tolerance<FPT> P( fpc_tolerance<FPT>(), fpc::FPC_STRONG );

        assertion_result ar( P( lhs, rhs ) );
        if( !ar )
            ar.message() << "Relative difference exceeds tolerance ["
                         << P.tested_rel_diff() << " > " << P.fraction_tolerance() << ']';
        return ar;
    }
}

//____________________________________________________________________________//

template <typename FPT, typename Lhs, typename Rhs>
inline assertion_result
compare_fpv( Lhs const& lhs, Rhs const& rhs, op::NE<Lhs,Rhs>* )
{
    if( lhs == 0 ) {
        return compare_fpv_near_zero( rhs, (op::NE<Lhs,Rhs>*)0 );
    }
    else if( rhs == 0 ) {
        return compare_fpv_near_zero( lhs, (op::NE<Lhs,Rhs>*)0 );
    }
    else {
        fpc::close_at_tolerance<FPT> P( fpc_tolerance<FPT>(), fpc::FPC_WEAK );

        assertion_result ar( !P( lhs, rhs ) );
        if( !ar )
            ar.message() << "Relative difference is within tolerance ["
                         << P.tested_rel_diff() << " < " << fpc_tolerance<FPT>() << ']';

        return ar;
    }
}

//____________________________________________________________________________//

#define DEFINE_FPV_COMPARISON( oper, name, rev )                        \
template<typename Lhs,typename Rhs>                                     \
struct name<Lhs,Rhs,typename boost::enable_if_c<                        \
    (fpc::tolerance_based<Lhs>::value &&                                \
     fpc::tolerance_based<Rhs>::value)>::type> {                        \
public:                                                                 \
    typedef typename common_type<Lhs,Rhs>::type FPT;                    \
    typedef name<Lhs,Rhs> OP;                                           \
                                                                        \
    typedef assertion_result result_type;                               \
                                                                        \
    static bool                                                         \
    eval_direct( Lhs const& lhs, Rhs const& rhs )                       \
    {                                                                   \
        return lhs oper rhs;                                            \
    }                                                                   \
                                                                        \
    static assertion_result                                             \
    eval( Lhs const& lhs, Rhs const& rhs )                              \
    {                                                                   \
        if( fpc_tolerance<FPT>() == FPT(0)                              \
            || (std::numeric_limits<Lhs>::has_infinity                  \
                && (lhs == std::numeric_limits<Lhs>::infinity()))       \
            || (std::numeric_limits<Rhs>::has_infinity                  \
                && (rhs == std::numeric_limits<Rhs>::infinity())))      \
        {                                                               \
            return eval_direct( lhs, rhs );                             \
        }                                                               \
                                                                        \
        return compare_fpv<FPT>( lhs, rhs, (OP*)0 );                    \
    }                                                                   \
                                                                        \
    template<typename PrevExprType>                                     \
    static void                                                         \
    report( std::ostream&       ostr,                                   \
            PrevExprType const& lhs,                                    \
            Rhs const&          rhs )                                   \
    {                                                                   \
        lhs.report( ostr );                                             \
        ostr << revert()                                                \
             << tt_detail::print_helper( rhs );                         \
    }                                                                   \
                                                                        \
    static char const* revert()                                         \
    { return " " #rev " "; }                                            \
};                                                                      \
/**/

BOOST_TEST_FOR_EACH_COMP_OP( DEFINE_FPV_COMPARISON )
#undef DEFINE_FPV_COMPARISON

//____________________________________________________________________________//

} // namespace op
} // namespace assertion
} // namespace test_tools
} // namespace boost

#include <boost/test/detail/enable_warnings.hpp>

#endif // BOOST_TEST_TOOLS_FPC_OP_HPP_050915GER


#include <boost/numeric/interval/compare.hpp>


// Demonstrating the use of ranges built using a qi::spirit parser adapted to be a stream operator
#include <string>
#include <iostream>
#include <sstream>

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/qi_match.hpp>

#include <range/v3/all.hpp>

#include "nmea.h"

// supply our own stream operators here that match the declarations
std::istream&
operator>>(std::istream& is, gga_t& g) {
    using namespace boost::spirit;
    using namespace boost::spirit::qi;

    rule<istream_iterator, gga_t()> gga_parser =
        lit("$GPGGA") > ',' > int_ > ',' >
        double_ > ',' > char_("NS") > ',' >
        double_ > ',' > char_("EW") > ',' >
        int_ > ',' > int_ > ',' >
        double_ > ',' >
        double_ > ',' > char_ > ',' >
        double_ > ',' > char_ > ',' >
        -int_ > ',' > -int_ > ",*" > int_;

    // construct a stream manipulator from this parser
    auto manip = match(gga_parser, g);
    is >> manip;

    return is;
}

std::ostream& operator<<(std::ostream& os, gga_t const& g) {
    os << '(' << g.latitude << g.lat_hemi << ',' << g.longitude << g.long_hemi << ')';
    return os;
}

int main() {
    std::string test("$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,,*47");
    test +=        "\n$GPGGA,170834,4124.8963,N,08151.6838,W,1,05,1.5,280.2,M,-34.0,M,,,*59";

    std::istringstream ss(test);
    ss.unsetf(std::ios::skipws);
    using namespace ranges::v3;
    ranges::for_each(istream_range<gga_t>(ss),
                     [](gga_t const& g) {
                         std::cout << g << "\n";
                     });

}


// Author Dmytro Bobkov, dmytro.bobkov@tum.de
// Chair of Media Technology, Technical University of Munich, 2018-2019
// Parts of these were done while being at research visit to group of Bernd Girod IVMS at Stanford University

#include <iostream>
#include <fstream>
#include <iomanip>
#include <chrono>

// OpenCV
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"

// Eigen
#include <Eigen/Eigen>
#include <Eigen/Geometry>

// Boost
#include <boost/filesystem.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>

namespace po = boost::program_options;
namespace fs = boost::filesystem;

#include "projection_helpers.h"
#include "io_helpers.h"
#include "rendering_tools.h"

void
parseCommandLineOptions(int argc,
                        const char** argv,
                        Configuration& config_rendering,
                        std::string& in_pano_dir,
                        std::string& in_poses_dir,
                        std::string& in_room_segmentations,
                        std::string& output_folder,
                        bool& render_views_from_panoramas )
{
    po::options_description desc("Allowed options");
    desc.add_options()
            ("in_pano_dir", po::value<std::string>(), "Input pano directory")
            ("in_poses_dir", po::value<std::string>(), "Input poses directory")
            ("in_rooms_file", po::value<std::string>(), "Input poses directory")
            ("out_rgb_view_dir", po::value<std::string>(), "Outdirecotry")
            ("render_views_from_panoramas", po::bool_switch(&render_views_from_panoramas), "Optional argument for rendering")
            ("num_yaw_angles", po::value<int>(), "num_yaw_angles");;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("in_pano_dir")) {
        in_pano_dir = vm["in_pano_dir"].as<std::string>();
        std::cout << "In pano dir set to " << in_pano_dir << ".\n";
    }

    if (vm.count("in_poses_dir")) {
        in_poses_dir = vm["in_poses_dir"].as<std::string>();
        std::cout << "In poses dir set to " << in_poses_dir << ".\n";
    }

    if (vm.count("out_rgb_view_dir")) {
        output_folder = vm["out_rgb_view_dir"].as<std::string>();
        std::cout << "Out dir set to " << output_folder << ".\n";
    }

    if (vm.count("num_yaw_angles")) {
        config_rendering.number_yaw_angles = vm["num_yaw_angles"].as<int>();
        std::cout << "num_yaw_angles set to " << config_rendering.number_yaw_angles << ".\n";
    }

    if( vm.count("in_rooms_file")) {
        in_room_segmentations = vm["in_rooms_file"].as<std::string>();
        std::cout << "in_room_segmentations set to " << in_room_segmentations << ".\n";
    }
}

/**
 * This is for matterport3D dataset pano/pose folder
 * @param folder_with_poses
 * @param pano_infos
 */
void
parsePanoramasAsAverageOfSingleViews( const std::string& folder_with_view_poses,
                                      std::map<std::string, Panorama>& pano_infos )
{
    std::vector<std::string> files;

    // Note for (auto & p : fs::directory_iterator(folder_with_view_poses)) does not work for gcc 4.9

    for( auto& p : boost::make_iterator_range(fs::directory_iterator(folder_with_view_poses), {} )) {
        files.push_back(p.path().string());
    }

    std::map<std::string, Pose> view_to_pose;
    std::map<std::string, std::map< std::string, Pose > > pano_to_view_poses;

    for( std::string& txt_file : files ) {

        boost::filesystem::path p_i(txt_file);

        std::string extension = boost::filesystem::extension(p_i);
        if( extension != ".txt" ) {
            continue;
        }

        // split
        std::string delimiter = "_";
        std::string basename = boost::filesystem::basename(txt_file);
        std::string panorama_name = basename.substr(0, basename.find(delimiter));

        Eigen::MatrixXf rt_matrix4x4 = parseEigenMatrix( txt_file );

        // initialize the pose and rest correctly
        Pose view_pose;

        Eigen::Matrix3f rot_pose = rt_matrix4x4.topLeftCorner(3,3);
        Eigen::Vector3f t = rt_matrix4x4.topRightCorner(3, 1);
        Eigen::Quaternionf quat_rot(rot_pose);

        view_pose.location = t;
        view_pose.orientation = rot_pose;

        view_to_pose.insert({basename, view_pose});

        if( pano_to_view_poses.count( panorama_name) == 0 ) {
            std::map<std::string, Pose> views;
            views.insert({basename, view_pose});
            pano_to_view_poses.insert({panorama_name, views});
        }
        else {
            pano_to_view_poses[panorama_name].insert({basename, view_pose});
        }
    }

    // now we have collected all views for each panorama, so we can actually
    // get the average as our location and for pose we would just get the first view in the middle plane
    for( auto& pano_views : pano_to_view_poses ) {
        std::string pano_name = pano_views.first;

        std::map<std::string, Pose> views = pano_views.second;

        // set pose of the panorama as the average of all camera locations at this panorama index
        Eigen::Vector3f location_average(0,0,0);
        for( auto& view_this : views ) {
            //std::cout << "This " << (view_this.first) << std::endl;
            location_average += view_this.second.location;
        }
        location_average /= views.size();

        std::string middle_view_name = pano_name + "_pose_1_0";//get first horizontal view

        Eigen::Quaternionf pano_orient;
        pano_orient = views.at(middle_view_name).orientation;

        Panorama panorama;
        panorama.pose.location = location_average;
        panorama.pose.orientation = pano_orient;

        std::string pano_correct_name = pano_name; // "camera_" +

        pano_infos.insert({pano_correct_name, panorama});
    }

    std::cout << "Finished view parsing" << std::endl;
}

RenderedImage
generateRenderedImage( const std::string& name_prefix,
                       const std::string& name_basename,
                       const Panorama& pano,
                       const float yaw_deg,
                       const float pitch_deg,
                       const std::string& output_folder)
{
    // get panorama orientation
    const Eigen::Quaternionf& pano_orient = pano.pose.orientation;

    RenderedImage rendered_image;

    double yaw = radians(yaw_deg);//-
    double pitch = radians(pitch_deg);
    if( pitch!=0 ) {
        pitch *= -1;
    }
    double roll = radians(0.0);

    std::string name_view = getNameImage(name_prefix, yaw_deg, pitch_deg);
    std::string fullname = output_folder + name_view;
    rendered_image.filename_image = name_view;

    //std::cout << "Input quat " << pano_orient.w() << "," << pano_orient.vec().transpose() << std::endl;
    Eigen::Vector3f euler_pano = pano_orient.toRotationMatrix().eulerAngles(2, 1, 0); //yaw, roll, pitch
    Eigen::Vector3f euler_view(-yaw, roll, pitch);

    Eigen::Vector3f euler_total = euler_pano + euler_view;
    for( size_t i=0; i<3; i++ ) {
        euler_total(i) = constrainRad(euler_total(i));
    }

    Eigen::Quaternionf rot_total;
    rot_total = Eigen::AngleAxisf(euler_total(0), Eigen::Vector3f::UnitZ()) *
                Eigen::AngleAxisf(euler_total(1), Eigen::Vector3f::UnitY()) *
                Eigen::AngleAxisf(euler_total(2), Eigen::Vector3f::UnitX());

    //std::cout << "View " << name_view << " has " << rot_total.w() << "," << rot_total.vec().transpose() << std::endl;

    rendered_image.pose.orientation = rot_total;
    rendered_image.filename_pano = name_basename;
    rendered_image.room = pano.room;
    rendered_image.pose.location = pano.pose.location;

    return rendered_image;
}

/*
 Example how to run
 --in_pano_dir /media/dima/Data/Datasets/Matterport3D/v1/scans/2n8kARJN3HM/pano/
 --in_pose_dir /media/dima/Data/Datasets/Matterport3D/v1/scans/2n8kARJN3HM/matterport_camera_poses/
 --out_rgb_view_dir /media/dima/Data/Datasets/Matterport3D/v1/scans/2n8kARJN3HM/rendered_10/
 --num_yaw_angles 10
 */

int main( int argc, const char** argv )
{
    std::string in_pano_dir, output_folder, in_poses_dir, in_room_segmentations;
    Configuration config_rendering;
    //config_rendering.pitch_angles = {0};
    bool render_views_from_panoramas;

    parseCommandLineOptions(argc, argv,
            config_rendering,
            in_pano_dir,
            in_poses_dir,
            in_room_segmentations,
            output_folder,
            render_views_from_panoramas);

    PanoConfig pano_config;
    pano_config.shift_yaw_deg = 30;
    pano_config.invert_yaw = false;

    std::map<std::string, Panorama> panos;
    parsePanoramasAsAverageOfSingleViews( in_poses_dir, panos );

    std::map<std::string, std::string> panorama_to_room;
    parsePanoramaToRooms(in_room_segmentations, panorama_to_room);
    {
        for( auto& pano : panos ) {
            std::string room = panorama_to_room.at(pano.first);
            pano.second.room = room;
        }

        // now augment filename with camera_X_roomlabel_roomid
        std::map<std::string, Panorama> panos_correct;
        for( auto& pano : panos ) {
            std::string pano_correct = "camera_" + pano.first + "_" + pano.second.room;
            //std::cout << "Correct " << pano_correct << std::endl;
            panos_correct.insert({pano_correct, pano.second});
        }

        panos = panos_correct;

    }

    if( boost::filesystem::exists(output_folder)==false ) {
        boost::filesystem::create_directory(output_folder);
    }

    bool generate_multiple_yaw_rates = config_rendering.number_yaw_angles==-1;
    if( generate_multiple_yaw_rates ) {
        std::vector<int> yaw_numbers = {8, 10, 12, 16};
        initConfigurationMultipleRates(  yaw_numbers, config_rendering );
    }
    else {
        initConfiguration(  config_rendering );

    }

    std::vector<std::string> pano_filenames;
    if( boost::filesystem::exists(in_pano_dir) ) {
        boost::filesystem::path p(in_pano_dir);
        for( auto& entry : boost::make_iterator_range(boost::filesystem::directory_iterator(p), {})) {
            std::string extension =  boost::filesystem::extension(entry);
            if( extension == ".png" || extension == ".jpg" ) {
                std::string name_file = entry.path().string();
                pano_filenames.push_back(name_file);
            }
            else {
                std::cout << "Not png or jpg" << entry.path().string() << std::endl;
            }
        }
    }
    else {
        std::cout << "Folder " << in_pano_dir << " does not exist" << std::endl;
        exit(-1);
    }

    std::vector<RenderedImage> rendered_images;
    int global_view_id = 0;
    for( size_t ind_pano_raw=0; ind_pano_raw<pano_filenames.size(); ind_pano_raw++ ) {
        std::string name_image = pano_filenames[ind_pano_raw];
        // get index from the filename correctly
        boost::filesystem::path p_i(name_image);
        std::string pano_name_file = p_i.stem().string(); // without extension

        std::vector<std::string> strs;

        boost::split(strs, pano_name_file, boost::is_any_of("_"));
        //std::cout << "Split of " << pano_name_file << " into " << strs.size() << ":" << strs[1] << std::endl;
        assert(strs.size()==4);//camera_panoname_roomname_id
        std::string pano_name_stripped = pano_name_file;//strs[1];

        // remove

        std::string name_basename = p_i.filename().string();
        std::string name_prefix = pano_name_stripped + '_';

        if( panos.count(pano_name_stripped)==0 ) {
            std::cout << "Looking at panorama " << pano_name_stripped << ", but not present. Skipping!" << std::endl;
            continue;
        }

        const Panorama& pano = panos.at(pano_name_stripped);

        for (size_t ind_yaw = 0; ind_yaw < config_rendering.yaw_angles.size(); ind_yaw++) {
            for (size_t ind_pitch = 0; ind_pitch < config_rendering.pitch_angles.size(); ind_pitch++) {
                const double yaw_ref = config_rendering.yaw_angles[ind_yaw];
                double yaw_deg = constrainAngle(yaw_ref);
                const double pitch_deg = config_rendering.pitch_angles[ind_pitch];

                RenderedImage rendered_image = generateRenderedImage( name_prefix,
                                                                      name_basename,
                                                                      pano,
                                                                      yaw_deg,
                                                                      pitch_deg,
                                                                      output_folder);
                rendered_image.id = global_view_id;
                rendered_image.ind_yaw = ind_yaw;
                global_view_id++;

                rendered_images.push_back(rendered_image);
            }
        }
    }

    std::cout << "Finished computing rendered images " << rendered_images.size() << std::endl;

    // now write to JSON all data
    std::string json_output_all_rendered_view = output_folder + "/view_poses.json";
    writeViewsAndConfigToJSON( config_rendering, rendered_images, json_output_all_rendered_view );

    if( !render_views_from_panoramas ) {
        return 0;
    }

    // now iterate over all panos
    for( size_t ind_pano_raw=0; ind_pano_raw<pano_filenames.size(); ind_pano_raw++ ) {
        std::cout << "Rendering from pano " << ind_pano_raw << "/" << pano_filenames.size() << std::endl;

        std::string name_image = pano_filenames[ind_pano_raw];

        // get index from the filename correctly
        boost::filesystem::path p_i(name_image);
        std::string name_file = p_i.stem().string();
        std::string name_prefix = name_file + '_';

        cv::Mat img_src = cv::imread(name_image.c_str(), CV_LOAD_IMAGE_COLOR);
        if( img_src.empty() ) {
            std::cout << "Error: Could not load image!" << std::endl;
            continue;
        }

        processOnePano( config_rendering,
                        pano_config,
                        img_src,
                        output_folder,
                        name_prefix );
    }

    return 0;
}


/*
 * Copyright (c) 2016, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author(s): Michael X. Grey <mxgrey@gatech.edu>
 *
 * Georgia Tech Graphics Lab and AMBER Lab
 *
 * Directed by Prof. C. Karen Liu and Prof. Aaron Ames
 * <karenliu@cc.gatech.edu> <ames@gatech.edu>
 *
 * This file is provided under the following "BSD-style" License:
 *   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.
 */

#ifndef HUBO_UTILS_HPP
#define HUBO_UTILS_HPP

#include <Eigen/Geometry>

namespace hubo {

const double DefaultIsometryTolerance = 1e-6;
const double DefaultVectorTolerance = 1e-6;

//==============================================================================
static inline bool checkDist(Eigen::Vector3d& p, double a, double b)
{
  double d = p.norm();
  double dmax = a+b;
  double dmin = fabs(a-b);

  if (d > dmax)
  {
    p *= dmax/d;
    return false;
  }
  else if (d < dmin)
  {
    p *= dmin/d;
    return false;
  }
  else
  {
    return true;
  }
}

//==============================================================================
static inline void clamp_sincos(double& sincos, bool& valid)
{
  if (sincos < -1)
  {
    valid = false;
    sincos = -1;
  }
  else if (sincos > 1)
  {
    valid = false;
    sincos = 1;
  }
}

//==============================================================================
static inline Eigen::Vector3d flipEuler3Axis(const Eigen::Vector3d& u)
{
  Eigen::Vector3d v;
  v[0] = u[0] - M_PI;
  v[1] = M_PI - u[1];
  v[2] = u[2] - M_PI;
  return v;
}

//==============================================================================
inline double diff(const Eigen::Isometry3d& tf0,
                   const Eigen::Isometry3d& tf1)
{
  double cost = (tf0.translation() - tf1.translation()).norm();
  Eigen::AngleAxisd aa(tf0.linear().transpose()*tf1.linear());
  cost += aa.angle();

  return cost;
}

//==============================================================================
inline bool equal(const Eigen::Isometry3d& tf0,
                  const Eigen::Isometry3d& tf1,
                  const double tolerance = DefaultIsometryTolerance)
{
  return (diff(tf0, tf1) < tolerance);
}

//==============================================================================
inline bool equal(const Eigen::VectorXd& v0,
                  const Eigen::VectorXd& v1,
                  const double tolerance = DefaultVectorTolerance)
{
  return ((v1-v0).norm() < tolerance);
}

//==============================================================================
inline Eigen::AngleAxisd findRotation(
    const Eigen::Vector3d& x1, const Eigen::Vector3d& x0)
{
  Eigen::Vector3d N = x1.cross(x0);
  double s = N.norm();
  double theta = asin(s);
  // Deal with numerical imprecision issues
  if(s < -1.1 || 1.1 < s)
  {
    std::cout << "[findRotation] Strange value for s: " << s
              << " | Results in theta: " << theta << std::endl;
    return Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
  }
  else if(1.0 <= s)
  {
    theta = M_PI/2.0;
  }
  else if(s <= -1.0)
  {
    theta = -M_PI/2.0;
  }

  N.normalize();

  const double wrap = x1.dot(x0);
  if(wrap < 0)
    theta = M_PI - theta;

  if(std::abs(theta-M_PI) < 1e-4
     || std::abs(theta+M_PI) < 1e-4)
    return Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ());

  return Eigen::AngleAxisd(theta, N);
}

} // namespace hubo

#endif // HUBO_UTILS_HPP


// TRENTO: Reduced Thickness Event-by-event Nuclear Topology
// Copyright 2015 Jonah E. Bernhard, J. Scott Moreland
// MIT License

#include "nucleon.h"

#include <cmath>
#include <limits>
#include <random>
#include <stdexcept>

#include <cstdlib>
#include <iostream>
#include <fstream>
#include <iomanip>

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/expint.hpp>
#include <boost/math/tools/roots.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/math/special_functions/erf.hpp>
#include <boost/filesystem.hpp>

#include "fwd_decl.h"

namespace trento {

namespace {

// These constants define distances in terms of the width of the nucleon profile
// Gaussian thickness function.

// Truncation radius of the thickness function.
constexpr double max_radius_widths = 5.;

// Maximum impact parameter for participation.
constexpr double max_impact_widths = 6.;
// Create ctor parameters for unit mean std::gamma_distribution.
//   mean = alpha*beta == 1  ->  beta = 1/alpha
// Used below in NucleonProfile ctor initializer list.

template <typename RealType> using param_type =
  typename std::gamma_distribution<RealType>::param_type;

template <typename RealType>
param_type<RealType> gamma_param_unit_mean(RealType alpha = 1.) {
  return param_type<RealType>{alpha, 1./alpha};
}

// Return appropriate path to temporary file directory.
// Used to store cross section parameter \sigma_partonic.
fs::path get_data_home() {
  const auto data_path = std::getenv("XDG_DATA_HOME");
  if(data_path == nullptr)
    return fs::path{std::getenv("HOME")} / ".local/share";
  return data_path;
}

// Test approximate cross section parameter equality.
bool almost_equal(double a, double b) {
  return fabs(a - b) < 1e-6;
}

// Calculate constituent position sampling width from CL arguments
double calc_sampling_width(const VarMap& var_map) {
  auto nucleon_width = var_map["nucleon-width"].as<double>();
  auto constituent_width = var_map["constit-width"].as<double>();
  auto constituent_number = var_map["constit-number"].as<int>();

  auto one_constituent = (constituent_number == 1);
  auto same_size = (nucleon_width - constituent_width < 1e-6);

  if (one_constituent || same_size)
    return 0.0;
  else {
    auto num = sqr(nucleon_width) - sqr(constituent_width);
    auto denom = 1. - 1./constituent_number;
    return std::sqrt(num / denom);
  }
}

// Determine cross section parameter for sampling nucleon participants.
// This semi-analytic method is only valid for one constituent.
double analytic_partonic_cross_section(const VarMap& var_map) {
  // Read parameters from the configuration.
  auto sigma_nn = var_map["cross-section"].as<double>();
  auto width = var_map["nucleon-width"].as<double>();

  // TODO: automatically set from beam energy
  // if (sigma_nn < 0.) {
  //   auto sqrt_s = var_map["beam-energy"].as<double>();
  // }

  // Initialize arguments for boost root finding function.

  // Bracket min and max.
  auto a = -10.;
  auto b = 20.;

  // Tolerance function.
  // Require 3/4 of double precision.
  math::tools::eps_tolerance<double> tol{
    (std::numeric_limits<double>::digits * 3) / 4};

  // Maximum iterations.
  // This is overkill -- in testing only 10-20 iterations were required
  // (but no harm in overestimating).
  boost::uintmax_t max_iter = 1000;

  // The right-hand side of the equation.
  auto rhs = sigma_nn / (4 * math::double_constants::pi * sqr(width));

  // This quantity appears a couple times in the equation.
  auto c = sqr(max_impact_widths) / 4;

  try {
    auto result = math::tools::toms748_solve(
      [&rhs, &c](double x) {
        using std::exp;
        using math::expint;
        return c - expint(-exp(x)) + expint(-exp(x-c)) - rhs;
      },
      a, b, tol, max_iter);

    auto cross_section_param = .5*(result.first + result.second);
    return std::exp(cross_section_param) * 4 * math::double_constants::pi * sqr(width);
  }
  catch (const std::domain_error&) {
    // Root finding fails for very small nucleon widths, w^2/sigma_nn < ~0.01.
    throw std::domain_error{
      "unable to fit cross section -- nucleon width too small?"};
  }
}

// Determine cross section parameter for sampling nucleon participants.
// This Monte Carlo numeric method is used for more than one constituent.
double numeric_partonic_cross_section(const VarMap& var_map) {
  MonteCarloCrossSection mc_cross_section(var_map);
  auto sigma_nn = var_map["cross-section"].as<double>();
  auto width = var_map["constit-width"].as<double>();

  // Bracket min and max.
  auto a = -10.;
  auto b = 20.;

  // Tolerance function.
  math::tools::eps_tolerance<double> tol{8};

  // Maximum iterations.
  boost::uintmax_t max_iter = 20;

  // Dimensional constant [fm^-1] used to rescale sigma_partonic
  auto c = 4 * math::double_constants::pi * sqr(width);

  try {
    auto result = math::tools::toms748_solve(
      [&sigma_nn, &mc_cross_section, &c](double cross_section_param) {
        auto x = std::exp(cross_section_param) * c;
        return mc_cross_section(x) - sigma_nn;
      },
      a, b, tol, max_iter);

    auto cross_section_param = .5*(result.first + result.second);
    return std::exp(cross_section_param) * c;
  }
  catch (const std::domain_error&) {
    // Root finding fails for very small nucleon widths, w^2/sigma_nn < ~0.01.
    throw std::domain_error{
      "unable to fit cross section -- nucleon and/or constituent width too small?"};
  }
}

// Determine the cross section parameter given the nucleon_width,
// constituent_width, constituent number, and inelastic cross section.
double partonic_cross_section(const VarMap& var_map) {
  // Read parameters from the configuration.
  auto nucleon_width = var_map["nucleon-width"].as<double>();
  auto constituent_width = var_map["constit-width"].as<double>();
  auto constituent_number = var_map["constit-number"].as<int>();
  auto sigma_nn = var_map["cross-section"].as<double>();

  // Cross section parameters
  double nucleon_width_;
  double constituent_width_;
  int constituent_number_;
  double sigma_nn_;
  double sigma_partonic;

  // Establish trento cache path
  auto cache_dir = get_data_home() / "trento";
  auto cache_path = cache_dir / "cross_section.dat";
  fs::create_directory(cache_dir);

  // Check if cache exists
  if (fs::exists(cache_path.string())){

    // Open cache read only
    std::fstream cache_read(cache_path.string(), std::ios_base::in);

    // Check cache for previously tabulated cross section parameters
    while(!cache_read.eof()) {
      cache_read >> constituent_number_ >> nucleon_width_ >>
        constituent_width_ >> sigma_nn_ >> sigma_partonic;
      if (almost_equal(nucleon_width, nucleon_width_) &&
          almost_equal(constituent_width, constituent_width_) &&
          almost_equal(sigma_nn, sigma_nn_) &&
          (constituent_number == constituent_number_)) {
        return sigma_partonic;
      }
    }

    // Close read-only file stream
    cache_read.close();
  }

  // Open cache with write access
  std::fstream cache_write(cache_path.string(),
      std::ios_base::out | std::ios_base::app);

  // Use numeric method to determine sigma_partonic if there is nucleon
  // substructure, otherwise use semi-analytic method.
  if (constituent_number > 1)
    sigma_partonic = numeric_partonic_cross_section(var_map);
  else
    sigma_partonic = analytic_partonic_cross_section(var_map);

  // Save new cross section parameters to cache
  using std::fixed;
  using std::setprecision;
  using std::setw;
  using std::scientific;
  using std::endl;

  cache_write << setprecision(6)
              << std::left << setw(4) << fixed << constituent_number
              << std::right << setw(10) << fixed << nucleon_width
              << std::right << setw(10) << fixed << constituent_width
              << std::right << setw(10) << fixed << sigma_nn
              << std::right << setw(14) << scientific << sigma_partonic
              << endl;

  return sigma_partonic;
}

}  // unnamed namespace

NucleonCommon::NucleonCommon(const VarMap& var_map)
    : fast_exp_(-.5*sqr(max_radius_widths), 0., 1000),
      nucleon_width_(var_map["nucleon-width"].as<double>()),
      constituent_width_(var_map["constit-width"].as<double>()),
      constituent_number_(std::size_t(var_map["constit-number"].as<int>())),
      sampling_width_(calc_sampling_width(var_map)),
      max_impact_sq_(sqr(max_impact_widths*nucleon_width_)),
      constituent_width_sq_(sqr(constituent_width_)),
      constituent_radius_sq_(sqr(max_radius_widths*constituent_width_)),
      sigma_partonic_(partonic_cross_section(var_map)),
      prefactor_(math::double_constants::one_div_two_pi/constituent_width_sq_/constituent_number_),
      calc_ncoll_(var_map["ncoll"].as<bool>()),
      participant_fluctuation_dist_(gamma_param_unit_mean(var_map["fluctuation"].as<double>())),
      constituent_position_dist_(0, sampling_width_)
{}

MonteCarloCrossSection::MonteCarloCrossSection(const VarMap& var_map)
  : nucleon_width_(var_map["nucleon-width"].as<double>()),
    constituent_width_(var_map["constit-width"].as<double>()),
    constituent_number_(std::size_t(var_map["constit-number"].as<int>())),
    sampling_width_(calc_sampling_width(var_map)),
    max_impact_(max_impact_widths*nucleon_width_),
    constituent_width_sq_(sqr(constituent_width_)),
    prefactor_(math::double_constants::one_div_two_pi/constituent_width_sq_/constituent_number_)
{}

double MonteCarloCrossSection::operator() (const double sigma_partonic) const {

  random::CyclicNormal<> cyclic_normal{
    0., sampling_width_, cache_size, n_loops
  };

  struct Constituent {
    double x, y;
  };

  std::vector<Constituent> nucleonA(constituent_number_);
  std::vector<Constituent> nucleonB(constituent_number_);

  auto max_impact_sq_ = sqr(max_impact_);
  double ref_cross_section = 0.;
  double prob_miss = 0.;
  int pass_tolerance = 0;

  for (std::size_t n = 0; n < n_max; ++n) {
    // Sample b from P(b)db = 2*pi*b.
    auto b = max_impact_ * std::sqrt(random::canonical<double>());

    for (auto&& q : nucleonA) {
      q.x = cyclic_normal(random::engine);
      q.y = cyclic_normal(random::engine);
    }

    for (auto&& q : nucleonB) {
      q.x = cyclic_normal(random::engine);
      q.y = cyclic_normal(random::engine);
    }

    auto overlap = 0.;
    for (auto&& qA : nucleonA) {
      for (auto&& qB : nucleonB) {
        auto distance_sq = sqr(qA.x - qB.x + b) + sqr(qA.y - qB.y);
        overlap += std::exp(-.25*distance_sq/constituent_width_sq_);
      }
    }

    prob_miss +=
      std::exp(-sigma_partonic * prefactor_/(2.*constituent_number_) * overlap);

    auto prob_hit = 1. - (prob_miss/n);
    auto cross_section = M_PI*max_impact_sq_*prob_hit;

    auto update_difference = std::abs(cross_section - ref_cross_section);

    if (update_difference < tolerance) {
      ++pass_tolerance;
    } else {
      pass_tolerance = 0;
      ref_cross_section = cross_section;
    }

    if (pass_tolerance > n_pass){
      return cross_section;
    }
  }

  throw std::out_of_range{
    "Partonic cross section failed to converge \
      -- check nucleon width, constituent width, and constituent number"};
}

}  // namespace trento


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_SIMD_REMROUND_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_REMROUND_INCLUDED

#include <boost/simd/function/scalar/remround.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/generic/function/remround.hpp>

#endif


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#include "hoa_source_parser.hpp"

#include "hoa_source.hpp"

#include <boost/property_tree/ptree.hpp>

namespace visr
{
namespace objectmodel
{

/*virtual*/ void HoaSourceParser::
parse( boost::property_tree::ptree const & tree, Object & src ) const
{
  // parse generic part of object (parent of HoaSource)
  ObjectParser::parse( tree, src );

  try
  {
    // note: cannot check for object type id since src might be a subclass of HoaSource
    HoaSource & hoaSrc = dynamic_cast<HoaSource&>(src);

    // parse point source-specific data members
    HoaSource::Order const order = tree.get<HoaSource::Order>( "order" );
    if( (order + 1) * (order + 1) != hoaSrc.numberOfChannels() )
    {
      throw std::invalid_argument( "Number of channel signals differs from the value expected for thus HOA order." );
    }
    hoaSrc.setOrder( order );
  }
  // TODO: distinguish between boost property_tree parse errors and bad dynamic casts.
  catch( std::exception const & ex )
  {
    throw std::invalid_argument( std::string( "Error while parsing HOA source: object: ") + ex.what() );
  }
}

/*virtual*/ void HoaSourceParser::
write( Object const & obj, boost::property_tree::ptree & tree ) const
{
  // note: cannot check for object type id since obj might be a subclass of HoaSource
  HoaSource const& hoaObj = dynamic_cast<HoaSource const&>(obj);

  ObjectParser::write( obj, tree );
  tree.put<HoaSource::Order>( "order", hoaObj.order() );
}

} // namespace objectmodel
} // namespace visr


#include <iostream>
#include <vector>
#include <fstream>
#include "Rectangle.h"
#include "SplitHeuristic.h"
#include "LinearSplit.h"
#include "RandomRectangleGenerator.h"
#include "IOControl.h"
#include "RTreeController.h"
#include "GreeneSplit.h"
#include "Experiments.h"
#include "FilenameGenerator.h"

#include <sys/resource.h>
#include <boost/filesystem/operations.hpp>

void writeToFile(std::string uuid, std::string file_name, std::string fileheader, std::vector<std::string> contents){
    std::string output_folder = "output_folder";
    if(!boost::filesystem::exists(output_folder)){
        boost::filesystem::create_directory(output_folder);
    }

    std::ofstream ofs;
    ofs.open(output_folder+"/"+uuid+file_name);
    ofs << fileheader;
    ofs << "\n";
    for(std::string &s : contents){
        ofs << s;
        ofs << "\n";
    }
    ofs.close();
}

void make_experiments(){

    SplitHeuristic *heuristics[2] = {new LinearSplit(), new GreeneSplit()};

    std::vector<std::string> buildTimeOutput;
    std::vector<std::string> occupiedSpaceOutput;
    std::vector<std::string> pageDiskPercentageOutput;
    std::vector<std::string> searchTimeOutput;

    for (int i = 9; i <= 25; i++){
        std::string input = "generated2e" + std::to_string(i)  + ".txt";
        const int inputSize = 1 << i; // (int)std::pow(2, i);
        std::cout << "input size:" << inputSize << std::endl;
        RandomRectangleGenerator::generateDataFile(inputSize, input);
        vRect randomVRect = RandomRectangleGenerator::generateVRect(inputSize/10);
        for(int j = 0; j <= 1; j++) {
            //construction
            std::cout << heuristics[j]->heuristicName() << std::endl;
            Experiments::startExperiment("Build Rtree");
            RTreeController controller = IOControl::processInput(input, heuristics[j]);
            ExperimentData &experiment = Experiments::stopExperiment();

            std::string buildTimeStr, spaceOutputStr, percentageOutputStr, searchOutputStr;

            long buildTime =experiment.retrieveTimeElapsedNanoSeconds();
            buildTimeStr = std::to_string(inputSize)+","+
                           heuristics[j]->heuristicName()+","+
                           std::to_string(buildTime);

            //search

            float percentageOutput = (float)Experiments::averageRectanglesPerNode(controller) / (float)DEFAULT_MAX_NODE_SIZE;
            percentageOutputStr = std::to_string(inputSize)+","+
                                  heuristics[j]->heuristicName()+","+
                                  std::to_string(percentageOutput);

            long sumElapsedSearch = 0;
            Experiments::startExperiment("search");
            for(Rectangle &randomRectangle : randomVRect){

                controller.search(randomRectangle);
                //sumElapsedSearch += searchExperiment.retrieveTimeElapsedNanoSeconds();
            }
            ExperimentData &searchExperiment = Experiments::stopExperiment();
            long averageDuration = searchExperiment.retrieveTimeElapsedNanoSeconds();
            //long averageDuration = sumElapsedSearch / randomVRect.size();

            searchOutputStr = std::to_string(inputSize)+","+
                              heuristics[j]->heuristicName()+","+
                              std::to_string(averageDuration);

            IOControl::checkCache(controller.getControllerPrefix(), controller.Cached, true, true);

            long spaceOutput = IOControl::spaceOccupied(controller.getControllerPrefix());
            spaceOutputStr =  std::to_string(inputSize)+","+
                              heuristics[j]->heuristicName()+","+
                              std::to_string(spaceOutput);

            buildTimeOutput.push_back(buildTimeStr);
            occupiedSpaceOutput.push_back(spaceOutputStr);
            pageDiskPercentageOutput.push_back(percentageOutputStr);
            searchTimeOutput.push_back(searchOutputStr);

            std::cout << "Build Time = ";
            std::cout << buildTime << std::endl;
            std::cout << "Occupied space = ";
            std::cout << spaceOutput << std::endl;
            std::cout << "Page disk percentage = ";
            std::cout << percentageOutput << std::endl;
            std::cout << "Search time = ";
            std::cout << averageDuration << std::endl << std::endl;
            IOControl::cleanControllerData(controller.getControllerPrefix());
            controller.Cached.clear();

        }
        std::cout << std::endl << std::endl;
    }

    std::string anUuid = FilenameGenerator::makeUuid();
    writeToFile(anUuid, "experiment_build", "inputsize,heuristic,timeelapsed", buildTimeOutput);
    writeToFile(anUuid, "experiment_occupiedspace", "inputsize,heuristic,space", occupiedSpaceOutput);
    writeToFile(anUuid, "experiment_pagediskpercentage", "inputsize,heuristic,percentage", pageDiskPercentageOutput);
    writeToFile(anUuid, "experiment_searchtime", "inputsize,heuristic,timeelapsed", searchTimeOutput);

    delete heuristics[0];
    delete heuristics[1];
}

void increaseStackSize(){
    const rlim_t kStackSize = 2000 * 1024 * 1024;   // min stack size = 100 MB
    struct rlimit rl;
    int result;

    result = getrlimit(RLIMIT_STACK, &rl);
    if (result == 0)
    {
        if (rl.rlim_cur < kStackSize)
        {
            rl.rlim_cur = kStackSize;
            result = setrlimit(RLIMIT_STACK, &rl);
            if (result != 0)
            {
                fprintf(stderr, "setrlimit returned result = %d\n", result);
            }
        }
    }

}

int main() {
    increaseStackSize();
    make_experiments();

    return 0;
}


/**
 *  This module is about selecting the numerical and memory domains, initiating
 *  the verification process and returning the results.
 **/
#include <cinttypes>

#include <ctime>
#include <functional>
#include <iostream>
#include <map>
#include <string>
#include <tuple>
#include <utility>
#include <vector>

#include <boost/lexical_cast.hpp>

#include "crab/ebpf_domain.hpp"
#include "crab/cfg.hpp"
#include "crab/fwd_analyzer.hpp"

#include "asm_syntax.hpp"
#include "spec_type_descriptors.hpp"

using std::string;

using crab::linear_constraint_t;

program_info global_program_info;

// Numerical domains over integers
//using sdbm_domain_t = crab::domains::SplitDBM;
using crab::domains::ebpf_domain_t;

// Toy database to store invariants.
struct checks_db final {
    std::map<label_t, std::vector<std::string>> m_db;
    int total_warnings{};
    int total_unreachable{};

    void add(const label_t& label, const std::string& msg) {
        m_db[label].emplace_back(msg);
    }

    void add_warning(const label_t& label, const std::string& msg) { add(label,     msg); total_warnings++; }
    void add_unreachable(const label_t& label, const std::string& msg) { add(label, msg); total_unreachable++; }

    checks_db() = default;
};

inline int first_num(const label_t& s) {
    try {
        return boost::lexical_cast<int>(s.substr(0, s.find_first_of(":+")));
    } catch (...) {
        std::cout << "bad label:" << s << "\n";
        throw;
    }
}

static std::vector<label_t> sorted_labels(cfg_t& cfg) {
    std::vector<label_t> labels = cfg.labels();

    std::sort(labels.begin(), labels.end(), [](const string& a, const string& b) {
        if (first_num(a) < first_num(b))
            return true;
        if (first_num(a) > first_num(b))
            return false;
        return a < b;
    });
    return labels;
}

static checks_db analyze(cfg_t& cfg) {
    crab::domains::clear_global_state();

    auto [preconditions, postconditions] = crab::run_forward_analyzer(cfg);

    checks_db m_db;
    for (const label_t& label : sorted_labels(cfg)) {
        basic_block_t& bb = cfg.get_node(label);

        if (global_options.print_invariants) {
            std::cout << "\n" << preconditions.at(label) << "\n";
            std::cout << bb;
            std::cout << "\n" << postconditions.at(label) << "\n";
        }

        if (std::none_of(bb.begin(), bb.end(), [](const auto& s) { return std::holds_alternative<Assert>(s); }))
            continue;
        ebpf_domain_t from_inv(preconditions.at(label));
        from_inv.set_require_check([&m_db, label](auto& inv, const linear_constraint_t& cst, const std::string& s) {
            if (inv.is_bottom())
                return;
            if (cst.is_contradiction()) {
                m_db.add_warning(label, std::string("Contradition: ") + s);
                return;
            }

            if (inv.entail(cst)) {
                // add_redundant(s);
            } else if (inv.intersect(cst)) {
                // TODO: add_error() if imply negation
                m_db.add_warning(label, s);
            } else {
                m_db.add_warning(label, s);
            }
        });

        for (const auto& statement : bb) {
            bool pre_bot = from_inv.is_bottom();
            std::visit(from_inv, statement);
            if (!pre_bot && from_inv.is_bottom()) {
                m_db.add_unreachable(label, "inv became bot after " + to_string(statement));
            }
        }
    }
    return m_db;
}

std::tuple<bool, double> abs_validate(cfg_t& simple_cfg, program_info info) {
    global_program_info = std::move(info);
    cfg_t& cfg = simple_cfg;

    using namespace std;
    clock_t begin = clock();

    const checks_db db = analyze(cfg);

    clock_t end = clock();
    double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;

    int nwarn = db.total_warnings;

    if (global_options.print_failures) {
        std::cout << "\n";
        for (auto [label, messages] : db.m_db) {
            std::cout << label << ":\n";
            for (const auto& msg : messages)
                std::cout << "  " << msg << "\n";
        }
        std::cout << "\n";
        std::cout << db.total_warnings << " warnings\n";
    }
    return {nwarn == 0, elapsed_secs};
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// 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.

#ifndef NETKET_ADAMAX_HPP
#define NETKET_ADAMAX_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <complex>
#include <iostream>
#include "abstract_optimizer.hpp"

namespace netket {

class AdaMax : public AbstractOptimizer {
  int npar_;

  double alpha_;
  double beta1_;
  double beta2_;

  Eigen::VectorXd ut_;
  Eigen::VectorXd mt_;

  double niter_;
  double niter_reset_;

  double epscut_;

  const Complex I_;

 public:
  explicit AdaMax(double alpha = 0.001, double beta1 = 0.9,
                  double beta2 = 0.999, double epscut = 1.0e-7)
      : alpha_(alpha), beta1_(beta1), beta2_(beta2), epscut_(epscut), I_(0, 1) {
    npar_ = -1;
    niter_ = 0;
    niter_reset_ = -1;

    PrintParameters();
  }

  // TODO remove
  // Json constructor
  explicit AdaMax(const json &pars) : I_(0, 1) {
    npar_ = -1;
    niter_ = 0;
    niter_reset_ = -1;

    from_json(pars);
    PrintParameters();
  }

  void PrintParameters() {
    InfoMessage() << "Adamax optimizer initialized with these parameters :"
                  << std::endl;
    InfoMessage() << "Alpha = " << alpha_ << std::endl;
    InfoMessage() << "Beta1 = " << beta1_ << std::endl;
    InfoMessage() << "Beta2 = " << beta2_ << std::endl;
    InfoMessage() << "Epscut = " << epscut_ << std::endl;
  }

  void Init(const Eigen::VectorXd &pars) override {
    npar_ = pars.size();
    ut_.setZero(npar_);
    mt_.setZero(npar_);

    niter_ = 0;
  }

  void Init(const Eigen::VectorXcd &pars) override {
    npar_ = 2 * pars.size();
    ut_.setZero(npar_);
    mt_.setZero(npar_);

    niter_ = 0;
  }

  void Update(const Eigen::VectorXd &grad, Eigen::VectorXd &pars) override {
    assert(npar_ > 0);

    mt_ = beta1_ * mt_ + (1. - beta1_) * grad;

    for (int i = 0; i < npar_; i++) {
      ut_(i) = std::max(std::max(std::abs(grad(i)), beta2_ * ut_(i)), epscut_);
    }
    niter_ += 1.;
    if (niter_reset_ > 0) {
      if (niter_ > niter_reset_) {
        niter_ = 1;
      }
    }

    double eta = alpha_ / (1. - std::pow(beta1_, niter_));
    for (int i = 0; i < npar_; i++) {
      pars(i) -= eta * mt_(i) / ut_(i);
    }
  }

  void Update(const Eigen::VectorXcd &grad, Eigen::VectorXd &pars) override {
    Update(Eigen::VectorXd(grad.real()), pars);
  }

  void Update(const Eigen::VectorXcd &grad, Eigen::VectorXcd &pars) override {
    assert(npar_ == 2 * pars.size());

    for (int i = 0; i < pars.size(); i++) {
      mt_(2 * i) = beta1_ * mt_(2 * i) + (1. - beta1_) * grad(i).real();
      mt_(2 * i + 1) = beta1_ * mt_(2 * i + 1) + (1. - beta1_) * grad(i).imag();
    }

    for (int i = 0; i < pars.size(); i++) {
      ut_(2 * i) = std::max(
          std::max(std::abs(grad(i).real()), beta2_ * ut_(2 * i)), epscut_);
      ut_(2 * i + 1) = std::max(
          std::max(std::abs(grad(i).imag()), beta2_ * ut_(2 * i + 1)), epscut_);
    }

    niter_ += 1.;
    if (niter_reset_ > 0) {
      if (niter_ > niter_reset_) {
        niter_ = 1;
      }
    }

    double eta = alpha_ / (1. - std::pow(beta1_, niter_));
    for (int i = 0; i < pars.size(); i++) {
      pars(i) -= eta * mt_(2 * i) / ut_(2 * i);
      pars(i) -= eta * I_ * mt_(2 * i + 1) / ut_(2 * i + 1);
    }
  }

  void Reset() override {
    ut_ = Eigen::VectorXd::Zero(npar_);
    mt_ = Eigen::VectorXd::Zero(npar_);
    niter_ = 0;
  }

  void SetResetEvery(double niter_reset) { niter_reset_ = niter_reset; }

  void from_json(const json &pars) {
    // DEPRECATED (to remove for v2.0.0)
    std::string section = "Optimizer";
    if (!FieldExists(pars, section)) {
      section = "Learning";
    }

    alpha_ = FieldOrDefaultVal(pars[section], "Alpha", 0.001);
    beta1_ = FieldOrDefaultVal(pars[section], "Beta1", 0.9);
    beta2_ = FieldOrDefaultVal(pars[section], "Beta2", 0.999);
    epscut_ = FieldOrDefaultVal(pars[section], "Epscut", 1.0e-7);
  }
};

}  // namespace netket

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <complex>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

#if defined(MTL_WITH_AUTO) && defined(MTL_WITH_RANGEDFOR)
template <typename Matrix>
void test(const char* name)
{
    mtl::io::tout << "Test " << name << ":\n";
    Matrix A(3, 3);
    A= 7.0;

    // Define the property maps
    auto row=   row_map(A);
    auto col=   col_map(A);
    auto value= const_value_map(A);

    // Now iterate over the matrix
    for (auto c : major_of(A))
	for (auto i : nz_of(c))
	    mtl::io::tout << "A[" << row(i) << ", " << col(i) << "] = " << value(i) << '\n';
}
#else
template <typename Matrix>
void test(const char* ) {}
#endif

int main(int , char**)
{
    using mtl::mat::parameters;
    using namespace mtl;

    test<dense2D<double> >("dense2D<double>");
    test<dense2D<float> >("dense2D<float>");
    test<dense2D<complex<double> > >("dense2D<complex<double>>");
    test<dense2D<float, mat::parameters<col_major> > >("dense2D<float> col_major");

    test<compressed2D<float> >("compressed2D<float>");
    test<compressed2D<float, mat::parameters<col_major> > >("compressed2D<float> col_major");

    test<morton_dense<double, recursion::morton_z_mask> >("Morton Z-order");

    return 0;
}


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#include <string>

#include <boost/shared_ptr.hpp>
#include <boost/python.hpp>

#include <boost/numpy/ndarray.hpp>

#include <ndhist/axis.hpp>
#include <ndhist/axes/generic_axis.hpp>

namespace bp = boost::python;
namespace bn = boost::numpy;

namespace ndhist {
namespace axes {

void register_generic_axis()
{
    bp::class_<py::generic_axis, bp::bases<Axis>, boost::shared_ptr<py::generic_axis> >(
        "generic_axis"
      , "The generic_axis class provides an axis for axes with non-constant "
        "bin widths. Due to the generic bin nature of this axis, it is not "
        "extendable at all. So the under- and overflow bin edges must always "
        "be provided."
      , bp::init<
            bn::ndarray const &
          , std::string const &
          , std::string const &
          , bool
          , bool
        >(
          ( bp::arg("self")
          , bp::arg("edges")
          , bp::arg("label")=std::string("")
          , bp::arg("name")=std::string("")
          , bp::arg("has_underflow_bin")=true
          , bp::arg("has_overflow_bin")=true
          )
        )
      )
      .def(axis_pyinterface<py::generic_axis>())
    ;
}

}//namespace axes
}//namespace ndhist