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

#ifndef CRYPTO3_HASH_KECCAK_HPP
#define CRYPTO3_HASH_KECCAK_HPP

#include <nil/crypto3/hash/detail/sponge_construction.hpp>
#include <nil/crypto3/hash/detail/block_stream_processor.hpp>

#include <nil/crypto3/hash/detail/keccak/keccak_functions.hpp>
#include <nil/crypto3/hash/detail/keccak/keccak_policy.hpp>
#include <nil/crypto3/hash/detail/keccak/keccak_finalizer.hpp>
#include <nil/crypto3/hash/detail/keccak/keccak_padding.hpp>

#include <boost/endian/conversion.hpp>

namespace nil {
    namespace crypto3 {
        namespace hashes {
            template<std::size_t DigestBits = 512>
            class keccak_1600_compressor {
            protected:
                typedef detail::keccak_1600_functions<DigestBits> policy_type;

            public:
                constexpr static const std::size_t word_bits = policy_type::word_bits;
                typedef typename policy_type::word_type word_type;

                constexpr static const std::size_t state_bits = policy_type::state_bits;
                constexpr static const std::size_t state_words = policy_type::state_words;
                typedef typename policy_type::state_type state_type;

                constexpr static const std::size_t block_bits = policy_type::block_bits;
                constexpr static const std::size_t block_words = policy_type::block_words;
                typedef typename policy_type::block_type block_type;

                static void process_block(state_type &state, const block_type &block) {
                    for (std::size_t i = 0; i != block_words; ++i)
                        state[i] ^= block[i];

                    for (std::size_t i = 0; i != state_words; ++i)
                        boost::endian::endian_reverse_inplace(state[i]);

                    policy_type::permute(state);

                    for (std::size_t i = 0; i != state_words; ++i)
                        boost::endian::endian_reverse_inplace(state[i]);
                }
            };

            /*!
             * @brief
             * @tparam DigestBits
             * @ingroup hashes
             */
            template<std::size_t DigestBits = 512>
            class keccak_1600 {
                typedef detail::keccak_1600_policy<DigestBits> policy_type;

            public:
                constexpr static const std::size_t word_bits = policy_type::word_bits;
                typedef typename policy_type::word_type word_type;

                constexpr static const std::size_t block_bits = policy_type::block_bits;
                constexpr static const std::size_t block_words = policy_type::block_words;
                typedef typename policy_type::block_type block_type;

                constexpr static const std::size_t digest_bits = policy_type::digest_bits;
                typedef typename policy_type::digest_type digest_type;

                struct construction {
                    struct params_type {
                        typedef typename policy_type::digest_endian digest_endian;

                        constexpr static const std::size_t length_bits = policy_type::length_bits;
                        constexpr static const std::size_t digest_bits = policy_type::digest_bits;
                    };

                    typedef sponge_construction<
                        params_type, typename policy_type::iv_generator, keccak_1600_compressor<DigestBits>,
                        detail::keccak_1600_padding<policy_type>, detail::keccak_1600_finalizer<policy_type>>
                        type;
                };

                template<typename StateAccumulator, std::size_t ValueBits>
                struct stream_processor {
                    struct params_type {
                        typedef typename policy_type::digest_endian digest_endian;

                        constexpr static const std::size_t value_bits = ValueBits;
                    };

                    typedef block_stream_processor<construction, StateAccumulator, params_type> type;
                };
            };
        }    // namespace hashes
    }        // namespace crypto3
}    // namespace nil

#endif


#include <omnimapper/geometry.h>
#include <omnimapper/plugins/bounded_plane_plugin.h>
#include <omnimapper/transform_tools.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>

namespace omnimapper {
template <typename PointT>
BoundedPlanePlugin<PointT>::BoundedPlanePlugin(
    omnimapper::OmniMapperBase* mapper)
    : mapper_(mapper), max_plane_id_(0) {
  printf("BoundedPlanePlugin: Constructor.\n");
}

template <typename PointT>
bool BoundedPlanePlugin<PointT>::polygonsOverlap(CloudPtr boundary1,
                                                 CloudPtr boundary2) {
  // TODO: Make this use boost::geometry
  for (size_t i = 0; i < boundary1->points.size(); i++) {
    // if (pcl::isXYPointIn2DXYPolygon (boundary1.points[i], boundary2))
    if (pcl::isPointIn2DPolygon(boundary1->points[i], *boundary2)) return true;
  }

  for (size_t i = 0; i < boundary2->points.size(); i++) {
    // if (pcl::isXYPointIn2DXYPolygon (boundary2.points[i], boundary1))
    if (pcl::isPointIn2DPolygon(boundary2->points[i], *boundary1)) return true;
  }

  return false;
}

template <typename PointT>
bool BoundedPlanePlugin<PointT>::polygonsOverlapBoost(Eigen::Vector4d& coeffs1,
                                                      CloudPtr boundary1,
                                                      Eigen::Vector4d& coeffs2,
                                                      CloudPtr boundary2) {
  Eigen::Vector4d z_axis(0.0, 0.0, 1.0, 0.0);
  Eigen::Vector4d minus_z_axis(0.0, 0.0, -1.0, 0.0);

  // Move to xy
  Eigen::Affine3d p1_to_xy = planarAlignmentTransform(z_axis, coeffs1);
  CloudPtr p1_xy(new Cloud());
  pcl::transformPointCloud(*boundary1, *p1_xy, p1_to_xy);

  Eigen::Affine3d p2_to_xy = planarAlignmentTransform(z_axis, coeffs2);
  CloudPtr p2_xy(new Cloud());
  pcl::transformPointCloud(*boundary2, *p2_xy, p2_to_xy);

  bool intersects = boost::geometry::intersects(p1_xy->points, p2_xy->points);

  return (intersects);
}

template <typename PointT>
void BoundedPlanePlugin<PointT>::removeDuplicatePoints(
    pcl::PointCloud<PointT>& boundary_cloud) {
  for (size_t i = 1; i < boundary_cloud.points.size() - 1; i++) {
    for (size_t j = i + 1; j < boundary_cloud.points.size() - 1; j++) {
      if ((boundary_cloud.points[i].x == boundary_cloud.points[j].x) &&
          (boundary_cloud.points[i].y == boundary_cloud.points[j].y)) {
        if (j - i > (boundary_cloud.points.size() / 2.0)) {
          boundary_cloud.points.erase(boundary_cloud.points.begin() + j,
                                      boundary_cloud.points.end());
          boundary_cloud.points.erase(boundary_cloud.points.begin(),
                                      boundary_cloud.points.begin() + i);
          i = 1;
          j = i + 1;
        } else {
          boundary_cloud.points.erase(boundary_cloud.points.begin() + i,
                                      boundary_cloud.points.begin() + j);
          i = 1;
          j = i + 1;
        }
      }
    }
  }
}

template <typename PointT>
void BoundedPlanePlugin<PointT>::regionsToMeasurements(
    std::vector<pcl::PlanarRegion<PointT>,
                Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > >& regions,
    omnimapper::Time t,
    std::vector<omnimapper::BoundedPlane3<PointT> >& plane_measurements) {
  // If we have a sensor_to_base_transform function, use it and apply the
  // transform to incoming measurements
  Eigen::Affine3d sensor_to_base;
  bool use_transform = false;
  if (get_sensor_to_base_) {
    use_transform = true;
    sensor_to_base = (*get_sensor_to_base_)(t);
  }

  for (size_t i = 0; i < regions.size(); ++i) {
    Eigen::Vector4f model = regions[i].getCoefficients();
    Eigen::Vector3f centroid = regions[i].getCentroid();
    Eigen::Vector4f centroid4f(centroid[0], centroid[1], centroid[2], 0.0);
    // pcl::PointCloud<PointT> border_cloud;
    CloudPtr border_cloud(new Cloud());
    std::vector<PointT, Eigen::aligned_allocator<PointT> > border =
        regions[i].getContour();
    border_cloud->points = border;
    pcl::PointCloud<PointT> empty_inliers;

    printf("border before: %lu\n", border.size());
    if (border.size()) {
      removeDuplicatePoints(*border_cloud);
    }
    border = border_cloud->points;
    printf("border after: %lu\n", border.size());

    // TODO : remove debug
    // PointVector poly(border);
    bool intersects = boost::geometry::intersects(border);
    if (intersects) {
      printf(
          "BoundedPlanePlugin: regions->measurements: GOT INVALID "
          "MEASUREMENT!\n");
      char meas_name[2048];
      sprintf(meas_name, "invalid_meas.pcd");
      pcl::io::savePCDFileBinaryCompressed(meas_name, *border_cloud);
      // exit(1);
    } else
      printf("BoundedPlanePlugin: GOT VALID MEASUREMENT!\n");
    // Remove debug

    // Make a Plane
    if (use_transform) {
      Eigen::Vector3d trans = sensor_to_base.inverse().translation();
      Eigen::Vector4f centroid4f_base =
          sensor_to_base.cast<float>() * centroid4f;
      Eigen::Vector3d model_norm(model[0], model[1], model[2]);
      Eigen::Vector3d model_norm_rot = sensor_to_base.linear() * model_norm;
      double d = model[0] * trans[0] + model[1] * trans[1] +
                 model[2] * trans[2] + model[3];
      Eigen::Vector4f model_base(model_norm_rot[0], model_norm_rot[1],
                                 model_norm_rot[2], d);
      // model_base[3] = model_base.dot (centroid4f_base);

      if (model_base[3] < 0) {
        model_base *= -1;
      }

      printf("Model: %lf %lf %lf %lf, Base Model: %lf %lf %lf %lf\n", model[0],
             model[1], model[2], model[3], model_base[0], model_base[1],
             model_base[2], model_base[3]);
      CloudPtr border_base(new Cloud());
      pcl::transformPointCloud(*border_cloud, *border_base, sensor_to_base);
      // gtsam::Plane<PointT> plane (model_base[0], model_base[1],
      // model_base[2], model_base[3], border_base, empty_inliers,
      // centroid4f_base);
      omnimapper::BoundedPlane3<PointT> plane(model_base[0], model_base[1],
                                              model_base[2], model_base[3],
                                              border_base);
      plane_measurements.push_back(plane);
    } else {
      omnimapper::BoundedPlane3<PointT> plane(model[0], model[1], model[2],
                                              model[3], border_cloud);
      // gtsam::Plane<PointT> plane (model[0], model[1], model[2], model[3],
      // border_cloud, empty_inliers, centroid4f);
      plane_measurements.push_back(plane);
    }
  }
}

template <typename PointT>
void BoundedPlanePlugin<PointT>::planarRegionCallback(
    std::vector<pcl::PlanarRegion<PointT>,
                Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > >
        regions,
    omnimapper::Time t) {
  printf("BoundedPlanePlugin: Got %lu regions.\n", regions.size());

  // Convert the regions to omnimapper::BoundedPlane3
  std::vector<omnimapper::BoundedPlane3<PointT> > plane_measurements;
  regionsToMeasurements(regions, t, plane_measurements);
  printf("BoundedPlanePlugin: Have %lu measurements\n",
         plane_measurements.size());

  // Get the planes from the mapper
  gtsam::Values current_solution =
      mapper_->getSolutionAndUncommitted();  // mapper_->getSolution ();
  gtsam::Values::Filtered<omnimapper::BoundedPlane3<PointT> > plane_filtered =
      current_solution.filter<omnimapper::BoundedPlane3<PointT> >();
  // gtsam::Values::Filtered<gtsam::OrientedPlane3> plane_filtered =
  // current_solution.filter<gtsam::OrientedPlane3>();

  // Get the pose symbol for this time
  gtsam::Symbol pose_sym;
  mapper_->getPoseSymbolAtTime(t, pose_sym);
  boost::optional<gtsam::Pose3> new_pose = mapper_->predictPose(pose_sym);
  printf("BoundedPlanePlugin: Processing planes for pose %s\n",
         boost::lexical_cast<std::string>(pose_sym.key()).c_str());

  // TODO: if above didn't work
  if (!new_pose) {
    printf("BoundedPlanePlugin: No pose yet at this time!  Error!\n");
    return;
  }

  gtsam::Pose3 new_pose_inv = new_pose->inverse();
  Eigen::Matrix4f new_pose_inv_tform = new_pose->matrix().cast<float>();

  for (int i = 0; i < plane_measurements.size(); i++) {
    double lowest_error = std::numeric_limits<double>::infinity();
    gtsam::Symbol best_symbol = gtsam::Symbol('b', max_plane_id_);

    omnimapper::BoundedPlane3<PointT> meas_plane = plane_measurements[i];
    Eigen::Vector3d meas_norm = meas_plane.normal().unitVector();
    double meas_d = meas_plane.d();
    CloudPtr meas_boundary = meas_plane.boundary();
    CloudPtr meas_boundary_map(new Cloud());
    Eigen::Affine3f pose2map = pose3ToTransform(*new_pose);
    pcl::transformPointCloud(*meas_boundary, *meas_boundary_map, pose2map);

    // TODO : remove debug
    Eigen::Vector4d meas_map_coeffs =
        omnimapper::BoundedPlane3<PointT>::TransformCoefficients(meas_plane,
                                                                 new_pose_inv);
    for (int i = 0; i < meas_boundary_map->points.size(); i++) {
      // printf("Meas Boundary Map: Boundary Point: %lf %lf %lf\n",
      //        meas_boundary_map->points[i].x,
      //        meas_boundary_map->points[i].y,
      //        meas_boundary_map->points[i].z);

      double ptp_dist =
          fabs(meas_map_coeffs[0] * meas_boundary_map->points[i].x +
               meas_map_coeffs[1] * meas_boundary_map->points[i].y +
               meas_map_coeffs[2] * meas_boundary_map->points[i].z +
               meas_map_coeffs[3]);
      if (ptp_dist > 0.01) {
        printf(
            "ERROR: Initializing boundary at bad place: Point is %lf from "
            "plane.\n",
            ptp_dist);
        exit(1);
      }
    }

    // PointVector poly(border);
    bool intersects = boost::geometry::intersects(meas_boundary_map->points);
    if (intersects)
      printf("BoundedPlanePlugin: map_meas: GOT INVALID MEASUREMENT!\n");
    // Remove debug

    if (meas_d < 0.1) {
      continue;
    }
    Eigen::Vector3d ceiling_norm(0.0, 0.0, -1.0);
    if (acos(meas_norm.dot(ceiling_norm)) < angular_threshold_) {
      continue;
    }

    //      BOOST_FOREACH (const typename
    //      gtsam::Values::Filtered<gtsam::OrientedPlane3>::KeyValuePair&
    //      key_value, plane_filtered)
    BOOST_FOREACH (
        const typename gtsam::Values::Filtered<
            omnimapper::BoundedPlane3<PointT> >::KeyValuePair& key_value,
        plane_filtered) {
      gtsam::Symbol key_symbol(key_value.key);
      // omnimapper::BoundedPlane3<PointT> plane = key_value.value;
      omnimapper::BoundedPlane3<PointT> plane = key_value.value;

      // gtsam::OrientedPlane3 predicted_plane =
      // gtsam::OrientedPlane3::Transform (plane, (*new_pose), boost::none,
      // boost::none);
      Eigen::Vector4d pred_coeffs =
          omnimapper::BoundedPlane3<PointT>::TransformCoefficients(
              plane, (*new_pose));  // predicted_plane.planeCoefficients ();
      Eigen::Vector3d pred_norm(pred_coeffs[0], pred_coeffs[1], pred_coeffs[2]);
      double pred_d = pred_coeffs[3];

      double angular_error = acos(meas_norm.dot(pred_norm));
      double range_error = fabs(meas_d - pred_d);
      printf("BoundedPlanePlugin: ang error: %lf range_error: %lf\n",
             angular_error, range_error);

      if ((angular_error < angular_threshold_) &&
          (range_error < range_threshold_)) {
        double error = angular_error + range_error;
        // polygon overlap
        // if (polygonsOverlap(plane.boundary(), meas_boundary_map))
        // TODO: this should not be in the map frame due to lever-arm
        CloudPtr match_map_boundary = plane.boundary();
        Eigen::Vector4d match_map_coeffs = plane.planeCoefficients();
        if (polygonsOverlapBoost(match_map_coeffs, match_map_boundary,
                                 meas_map_coeffs, meas_boundary_map)) {
          if ((error < lowest_error)) {
            lowest_error = error;
            best_symbol = key_symbol;
          }
        } else {
          printf("POLYGON OVERLAP FAILED!\n");
        }
      }
    }

    // Add factors
    if (lowest_error == std::numeric_limits<double>::infinity()) {
      // omnimapper::BoundedPlane3<PointT> map_p3 =
      // gtsam::OrientedPlane3::Transform (meas_plane, *new_pose);
      // Eigen::Vector4d map_p3_coeffs =
      // omnimapper::BoundedPlane3<PointT>::TransformCoefficients (meas_plane,
      // *new_pose);//map_p3.planeCoefficients ();
      Eigen::Vector4d map_p3_coeffs =
          omnimapper::BoundedPlane3<PointT>::TransformCoefficients(
              meas_plane, new_pose_inv);
      // Eigen::Affine3f pose2map = pose3ToTransform(*new_pose);
      // CloudPtr map_boundary (new Cloud ());
      // pcl::transformPointCloud (*meas_boundary, *map_boundary, pose2map);
      // omnimapper::BoundedPlane3<PointT> map_plane (map_p3_coeffs[0],
      // map_p3_coeffs[1], map_p3_coeffs[2], map_p3_coeffs[3], map_boundary);
      omnimapper::BoundedPlane3<PointT> map_plane(
          map_p3_coeffs[0], map_p3_coeffs[1], map_p3_coeffs[2],
          map_p3_coeffs[3], meas_boundary_map);

      // gtsam::Plane<PointT> new_plane (*new_pose, meas_plane,
      // false);//(new_pose_inv, meas_plane, false);
      printf("BoundedPlanePlugin: Creating new plane %s: %lf %lf %lf %lf\n",
             boost::lexical_cast<std::string>(best_symbol.key()).c_str(),
             map_p3_coeffs[0], map_p3_coeffs[1], map_p3_coeffs[2],
             map_p3_coeffs[3]);
      gtsam::GenericValue<omnimapper::BoundedPlane3<PointT>> map_plane_val(
          map_plane);
      mapper_->addNewValue(best_symbol, map_plane_val);
      ++max_plane_id_;
    } else {
      // lock plane & update
      printf("BoundedPlanePlugin: Extending boundary...\n");
      mapper_->updateBoundedPlane(best_symbol, *new_pose, meas_plane);
      // omnimapper::BoundedPlane3<PointT> map_plane =
      // current_solution.at<omnimapper::BoundedPlane3<PointT> > (best_symbol);
      // map_plane.extendBoundary((*new_pose), meas_plane);
      printf("BoundedPlanePlugin: Boundary Extended...\n");
    }

    gtsam::SharedDiagonal measurement_noise;
    gtsam::Vector g_v(3);
    g_v << angular_noise_, angular_noise_, range_noise_;
    measurement_noise = gtsam::noiseModel::Diagonal::Sigmas(g_v);

    gtsam::Vector measurement_vector = meas_plane.planeCoefficients();
    omnimapper::OmniMapperBase::NonlinearFactorPtr plane_factor(
        new omnimapper::BoundedPlaneFactor<PointT>(
            measurement_vector, meas_boundary, measurement_noise, pose_sym,
            best_symbol));
    mapper_->addFactor(plane_factor);
    printf("BoundedPlanePlugin: Added factor!\n");
  }  // plane measurements
}

}  // namespace omnimapper

template class omnimapper::BoundedPlanePlugin<pcl::PointXYZRGBA>;


//
// Created by eriks on 7/24/17.
//

#include "velocity_histogram_observer.hpp"
#include "interpolation.hpp"
#include "fileIO.hpp"
#include "initial_conditions/initial_conditions.hpp"
#include <boost/algorithm/clamp.hpp>

VelocityHistogram::VelocityHistogram(std::size_t dimension, std::size_t bin_count, double range) :
    mData(dimension-1, bin_count), mRange(range), mBinCount(bin_count)
{
}

void VelocityHistogram::record(const gen_vect& velocity)
{
    // remember data dimension is simulation dimension minus one.
    if(mData.getDimension() == 1)
    {
        double vel = (boost::algorithm::clamp(velocity[1] / mRange, -1.0, 1.0) + 1) / 2;
        std::array<int, 1> index_v;
        index_v[0] = static_cast<int>(std::round(vel * (mBinCount - 1)));
        mData(index_v) += 1;
    } else if(mData.getDimension() == 2)
    {
        double vel1 = (boost::algorithm::clamp(velocity[1] / mRange, -1.0, 1.0) + 1) / 2;
        double vel2 = (boost::algorithm::clamp(velocity[2] / mRange, -1.0, 1.0) + 1) / 2;
        std::array<int, 2> index_v;
        index_v[0] = static_cast<int>(std::round(vel1 * (mBinCount - 1)));
        index_v[1] = static_cast<int>(std::round(vel2 * (mBinCount - 1)));
        mData(index_v) += 1;
    }
}

VelocityHistogramObserver::VelocityHistogramObserver(std::size_t dimension, std::vector<double> timeIntervals,
                                                     std::size_t bin_count, std::string file_name) :
        ThreadLocalObserver( std::move(file_name) ),
        mBinCount(bin_count), mDimension(dimension), mTimeIntervals(std::move(timeIntervals))
{
    for(unsigned i = 0; i < mTimeIntervals.size(); ++i)
    {
        mBinCounts.push_back( VelocityHistogram(dimension, bin_count, 1.5)  );
    }
}

void VelocityHistogramObserver::combine(ThreadLocalObserver& other)
{
    auto& data = dynamic_cast<VelocityHistogramObserver&>( other );
    for(unsigned i = 0; i < mBinCounts.size(); ++i)
    {
        auto& count = mBinCounts[i].getData();
        auto& o_count = data.mBinCounts[i].data();
        // add the counts
        std::transform(count.begin(), count.end(), o_count.begin(), count.begin(), std::plus<std::size_t>());
    }
}

void VelocityHistogramObserver::startTrajectory( const InitialCondition& start, std::size_t )
{
    mOldVelocity = start.getState().getVelocity();
    mLastObservedTime = 0;
    mOldTime = 0;
}

bool VelocityHistogramObserver::watch( const State& state, double t )
{
    // finished after last time
    if( mLastObservedTime >= mTimeIntervals.size() )
        return false;

    // otherwise, check for passing time boundary
    while( t > mTimeIntervals[mLastObservedTime] )
    {
        // linearly interpolated velocity between last and current state
        // this is important, if e have multiple observation times that
        // are closely spaced in an area where the adaptive time step
        // gets big.
        double rtime = (mTimeIntervals[mLastObservedTime] - mOldTime) / (t - mOldTime);
        auto interpol = interpolate_linear_1d(mOldVelocity,
                                              state.getVelocity(),
                                              rtime);
        mBinCounts[mLastObservedTime].record(interpol);
        ++mLastObservedTime;
        // finished after last time
        if( mLastObservedTime >= mTimeIntervals.size() )
            return false;
    }

    // update old position.
    // since we only need this if we continue tracing this trajectory,
    // it is OK to only update when we return true.
    mOldVelocity = state.getVelocity();
    mOldTime = t;
    return true;
}

void VelocityHistogramObserver::save(std::ostream& target)
{
    /*! Angular Velocity histogram save file format.
        Header: velh001\\n
        Data type | Count | Meaning
        --------- | ----- | -------
        Int [H]   | 1     | Number of histograms
        Int [B]   | 1     | Number of bins per histogram
        Int [D]   | 1     | Number of dimensions
        Double    | H     | Histogram times
        Double    | B     | bin center velocities
        Int       | H * B ^ (D-1)| Angle counts
    */

    // file header
    target << "velh001\n";
    writeInteger(target, mBinCounts.size());
    writeInteger(target, mBinCount);
    writeInteger(target, mDimension);
    // write times
    writeFloats(target, mTimeIntervals);

    // write velocities
    for(unsigned j = 0; j < mBinCount; ++j) {
        double bin_center = double(j) / (mBinCount - 1) * 2 - 1;
        writeFloat(target, bin_center * 1.5);
    }

    for( auto& bins : mBinCounts )
    {
        bins.data().dump(target);
    }
}

std::shared_ptr<ThreadLocalObserver> VelocityHistogramObserver::clone() const
{
    return std::make_shared<VelocityHistogramObserver>( mDimension, mTimeIntervals, mBinCount, filename() );
}


#include <algorithm>
#include <iostream>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/function_input_iterator.hpp>

using namespace boost;
using namespace std;

struct generator {
  typedef int result_type;
  generator() { srand(time(0)); }
  result_type operator() () const {
    return rand();
  }
};

int f() { return rand() % 10; }

int main(int argc, char * argv[]) {
  copy(
      make_function_input_iterator(f, 0),
      make_function_input_iterator(f, 10),
      ostream_iterator<int>(cout, " ")
      );

  generator g;

  copy(
      make_function_input_iterator(g, infinite()),
      make_function_input_iterator(g, infinite()),
      ostream_iterator<int>(cout, " ")
      );

  return 0;
}


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

    @copyright 2016 NumScale SAS

  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_NEGATENZ_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_NEGATENZ_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-ieee
    This function object returns @c x multiplied by the @ref signnz of @c y.
    The result is unspecified if @c y is @ref Nan

    @par Header <boost/simd/function/negatenz.hpp>

    @par Note
    If @c x is  null the result is \f$\pm0\f$. Take care that @c negatenz distinguish
    between the signs of `T(0)` and `-T(0)` when `T` is a floating type.

    @see negatenz, Mzero, is_positive, is_negative

    @par Example:

      @snippet negatenz.cpp negatenz

    @par Possible output:

      @snippet negatenz.txt negatenz

  **/
  Value negatenz(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/negatenz.hpp>
#include <boost/simd/function/simd/negatenz.hpp>

#endif


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#define BOOST_TEST_MODULE icl::interval_map_mixed unit test
#include <libs/icl/test/disable_test_warnings.hpp>
#include <string>
#include <boost/mpl/list.hpp>
#include "../unit_test_unwarned.hpp"
#include <boost/test/test_case_template.hpp>

// interval instance types
#include "../test_type_lists.hpp"
#include "../test_value_maker.hpp"

#include <boost/icl/interval_set.hpp>
#include <boost/icl/separate_interval_set.hpp>
#include <boost/icl/split_interval_set.hpp>
#include <boost/icl/interval_map.hpp>
#include <boost/icl/split_interval_map.hpp>

using namespace std;
using namespace boost;
using namespace unit_test;
using namespace boost::icl;

#include "../test_interval_map_mixed.hpp"

BOOST_AUTO_TEST_CASE_TEMPLATE
(fastest_icl_interval_map_mixed_ctor_4_ordered_types, T, ordered_types)
{            interval_map_mixed_ctor_4_ordered_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(fastest_icl_interval_map_mixed_equal_4_ordered_types, T, ordered_types)
{            interval_map_mixed_equal_4_ordered_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(fastest_icl_interval_map_mixed_assign_4_ordered_types, T, ordered_types)
{            interval_map_mixed_assign_4_ordered_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(fastest_icl_interval_map_mixed_ctor_4_bicremental_types, T, bicremental_types)
{            interval_map_mixed_ctor_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(fastest_icl_interval_map_mixed_assign_4_bicremental_types, T, bicremental_types)
{            interval_map_mixed_assign_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(fastest_icl_interval_map_mixed_equal_4_bicremental_types, T, bicremental_types)
{            interval_map_mixed_equal_4_bicremental_types<T, int>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(fastest_itl_partial_interval_map_mixed_inclusion_compare_4_bicremental_types, T, bicremental_types)
{            partial_interval_map_mixed_inclusion_compare_4_bicremental_types<T, int, partial_absorber>(); }

BOOST_AUTO_TEST_CASE_TEMPLATE
(fastest_itl_partial_interval_map_mixed_contains_4_bicremental_types, T, bicremental_types)
{            partial_interval_map_mixed_contains_4_bicremental_types<T, int, partial_absorber>(); }


// GraphUtilities.hpp
// Copyright 2019 Mikko Lauri
//
// 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 GRAPHUTILITIES_HPP
#define GRAPHUTILITIES_HPP
#include <algorithm>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/graphviz.hpp>
#include <vector>

// all_paths_helper and all_paths modified from original by StackOverflow user
// sehe, https://stackoverflow.com/a/46985434/5471520
namespace pgi {
template <class Graph, class VertexPredicate>
std::pair<
    typename boost::filter_iterator<
        VertexPredicate, typename boost::graph_traits<Graph>::vertex_iterator>,
    typename boost::filter_iterator<
        VertexPredicate, typename boost::graph_traits<Graph>::vertex_iterator> >
filter_vertices(VertexPredicate pred, const Graph& g) {
  auto vit = boost::vertices(g);
  return std::make_pair(
      boost::make_filter_iterator(pred, vit.first, vit.second),
      boost::make_filter_iterator(pred, vit.second, vit.second));
}

template <typename Graph, typename Report>
void all_paths_helper(typename Graph::vertex_descriptor from,
                      typename Graph::vertex_descriptor to, const Graph& g,
                      std::vector<typename Graph::edge_descriptor>& path,
                      const Report& callback) {
  if (from == to) {
    callback(path);
  } else {
    for (auto out : boost::make_iterator_range(boost::out_edges(from, g))) {
      path.push_back(out);
      auto v = boost::target(out, g);
      if (path.end() ==
          std::find_if(path.begin(), path.end(),
                       [&g, v](typename Graph::edge_descriptor e) {
                         return (boost::source(e, g) == v);
                       })) {
        all_paths_helper(v, to, g, path, callback);
      }
      path.pop_back();
    }
  }
}

template <typename Graph, typename Report>
void all_paths(typename Graph::vertex_descriptor from,
               typename Graph::vertex_descriptor to, const Graph& g,
               const Report& callback) {
  std::vector<typename Graph::edge_descriptor> state;
  all_paths_helper(from, to, g, state, callback);
}

template <typename Graph, typename Report>
void all_paths_helper_exhaustive(
    typename Graph::vertex_descriptor v, const Graph& g,
    std::vector<typename Graph::edge_descriptor>& path,
    const Report& callback) {
  callback(path);
  for (auto out : boost::make_iterator_range(boost::out_edges(v, g))) {
    path.push_back(out);
    auto v = boost::target(out, g);
    if (path.end() == std::find_if(path.begin(), path.end(),
                                   [&g, v](typename Graph::edge_descriptor e) {
                                     return (boost::source(e, g) == v);
                                   })) {
      all_paths_helper_exhaustive(v, g, path, callback);
    }
    path.pop_back();
  }
}

template <typename Graph, typename Report>
void all_paths(typename Graph::vertex_descriptor from, const Graph& g,
               const Report& callback) {
  std::vector<typename Graph::edge_descriptor> state;
  all_paths_helper_exhaustive(from, g, state, callback);
}

template <typename Graph>
void redirect_out_edges(typename Graph::vertex_descriptor from,
                        typename Graph::vertex_descriptor to, Graph& g) {
  std::vector<typename boost::edge_bundle_type<Graph>::type> edge_properties;
  for (auto out : boost::make_iterator_range(boost::out_edges(from, g)))
    edge_properties.push_back(g[out]);

  boost::clear_out_edges(from, g);
  for (auto ep : edge_properties) boost::add_edge(from, to, ep, g);
}

template <typename Graph>
void redirect_in_edges(typename Graph::vertex_descriptor from,
                       typename Graph::vertex_descriptor to, Graph& g) {
  typedef std::pair<typename Graph::vertex_descriptor,
                    typename boost::edge_bundle_type<Graph>::type>
      source_edge_pair;
  std::vector<source_edge_pair> svp;

  for (auto in : boost::make_iterator_range(boost::in_edges(from, g)))
    svp.emplace_back(std::make_pair(boost::source(in, g), g[in]));

  boost::clear_in_edges(from, g);
  for (auto& se : svp) boost::add_edge(se.first, to, se.second, g);
}

template <typename Graph>
bool has_path(typename Graph::vertex_descriptor from,
              typename Graph::vertex_descriptor to, const Graph& g) {
  class reachability_visitor : public boost::default_bfs_visitor {
   public:
    reachability_visitor(typename Graph::vertex_descriptor trg) : trg_(trg) {}
    void discover_vertex(typename Graph::vertex_descriptor v,
                         const Graph& g) const {
      ;
      (void)g;  // no-op silences "unused parameter" warning
      if (v == trg_) throw std::runtime_error("Found query node");
    }
    typename Graph::vertex_descriptor trg_;
  };

  reachability_visitor vis(to);
  try {
    boost::breadth_first_search(g, from, boost::visitor(vis));
  } catch (std::runtime_error& e) {
    return true;
  }

  return false;
}

template <typename Graph>
std::pair<typename Graph::out_edge_iterator, bool> out_edge_exists(
    typename Graph::vertex_descriptor v,
    const typename boost::edge_bundle_type<Graph>::type& edge_property,
    const Graph& g) {
  auto eop = boost::out_edges(v, g);
  auto ef =
      std::find_if(eop.first, eop.second,
                   [&g, &edge_property](typename Graph::edge_descriptor e) {
                     return g[e] == edge_property;
                   });
  return std::make_pair(ef, ef != eop.second);
}

template <typename Tree>
std::vector<typename Tree::edge_descriptor> path_from_root_to(
    typename Tree::vertex_descriptor v, const Tree& t) {
  std::vector<typename Tree::edge_descriptor> path;
  while (boost::in_degree(v, t) == 1) {
    typename Tree::edge_descriptor e_in = *(boost::in_edges(v, t).first);
    path.emplace_back(e_in);
    v = boost::source(e_in, t);
  }
  std::reverse(path.begin(), path.end());
  return path;
}

template <typename Graph>
class EdgeWriter {
 public:
  typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
  EdgeWriter(const Graph* g, const std::vector<std::string>* names)
      : g_(g), names_(names) {}
  void operator()(std::ostream& out, const Edge& e) const {
    auto obs_index = (*g_)[e];
    out << "[label=\"" << obs_index << " ["
        << (names_->empty() ? "" : names_->at(obs_index)) << "]\"]";
  }

 private:
  const Graph* g_;
  const std::vector<std::string>* names_;
};

template <typename Graph>
class VertexWriter {
 public:
  typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
  VertexWriter(const Graph* g, const std::vector<std::string>* names)
      : g_(g), names_(names) {}
  void operator()(std::ostream& out, const Vertex& v) const {
    auto action_index = (*g_)[v];
    auto s = steps_remaining(v, *g_);
    if (s >= 1) {
      out << "[label=\"" << v << " (H" << s << "): " << action_index << " ["
          << (names_->empty() ? "" : names_->at(action_index)) << "]\"]";
    } else {
      out << "[label=\"" << v << " (end)\"]";
    }
  }

 private:
  const Graph* g_;
  const std::vector<std::string>* names_;
};

template <typename Graph>
std::ostream& print(std::ostream& os, const Graph& g,
                    const VertexWriter<Graph>& vpw,
                    const EdgeWriter<Graph>& epw) {
  boost::write_graphviz(os, g, vpw, epw);
  return os;
}

}  // namespace pgi

#endif  // GRAPHUTILITIES_HPP


// 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 <boost/test/minimal.hpp>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

int test_main(int argc, char* argv[])
{
    mtl::dense2D<float> A(3, 3), B(3, 3);
    A= 4.; B= 7;

    //trans(A)*= B;

    return 0;
}


#include <boost/graph/mesh_graph_generator.hpp>


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

#ifndef MTL_RIGHT_SCALE_INPLACE_INCLUDE
#define MTL_RIGHT_SCALE_INPLACE_INCLUDE

#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/operation/assign_each_nonzero.hpp>
#include <boost/numeric/mtl/operation/mult.hpp>
#include <boost/numeric/mtl/operation/rscale.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl {

/// Scale collection \p c from right with scalar factor \p alpha; \p c is altered
template <typename Factor, typename Coll>
void right_scale_inplace(Coll& c, const Factor& alpha, tag::scalar)
{
    vampir_trace<5> tracer;
    assign_each_nonzero(c, tfunctor::rscale<typename Collection<Coll>::value_type, Factor>(alpha));
}

template <typename Factor, typename Matrix>
void right_scale_inplace(Matrix& m, tag::matrix, const Factor& alpha, tag::matrix)
{
    using mtl::swap;

    vampir_trace<4016> tracer;
    Matrix tmp(num_rows(m), num_cols(m));
    mult(m, alpha, tmp);
    swap(m, tmp);
}

#if 0 // Row vector times Matrix is not yet implemented
template <typename Factor, typename Vector>
void right_scale_inplace(Vector& v, tag::vector, const Factor& alpha, tag::matrix)
{
    using mtl::swap;

    Vector tmp(size(v));
    gen_mult(v, alpha, tmp, assign::assign_sum(), tag::row_vector(), tag::matrix(), tag::row_vector());
    swap(v, tmp);
}
#endif

/// Scale collection \p c from right with matrix factor \p alpha; \p c is altered
template <typename Factor, typename Collection>
void right_scale_inplace(Collection& c, const Factor& alpha, tag::matrix)
{
    // Need to dispatch further to use different constructors for temporary
    right_scale_inplace(c, typename traits::category<Collection>::type(), alpha, tag::matrix());
}

/// Scale collection \p c from right with factor \p alpha; \p c is altered
template <typename Factor, typename Collection>
void right_scale_inplace(Collection& c, const Factor& alpha)
{
    // Dispatch between scalar and matrix factors
    right_scale_inplace(c, alpha, typename traits::category<Factor>::type());
}

} // namespace mtl

#endif // MTL_RIGHT_SCALE_INPLACE_INCLUDE


#pragma once
#ifndef CANNON_RAY_TEXTURE_H
#define CANNON_RAY_TEXTURE_H

/*!
 * \file cannon/ray/texture.hpp
 * \brief File containing Texture and subclasses class definitions.
 */

#include <memory>

#include <Eigen/Dense>
#include <stb_image/stb_image.h>

#include <cannon/math/perlin.hpp>

using namespace Eigen;

using namespace cannon::math;

namespace cannon {
  namespace ray {

    /*!
     * \brief Abstract class representing a texture on some geometry.
     */
    class Texture {
      public:
        /*!
         * Method to get the value of this texture at U,V surface coordinates.
         *
         * \param u Horizontal surface coordinate
         * \param v Vertical surface coordinate
         * \param p Point being textured
         *
         * \returns Color of texture at point with surface coords
         */
        virtual Vector3d value(double u, double v, const Vector3d& p) const = 0;
    };

    /*!
     * \brief Class representing a texture of a single, fixed color.
     */
    class SolidColor : public Texture {
      public:
        /*!
         * Default constructor.
         */
        SolidColor() {}

        /*!
         * Constructor taking the color to make this texture.
         */
        SolidColor(const Vector3d& c) : color_(c) {}

        /*!
         * Constructor taking RGB components for the color of this texture.
         */
        SolidColor(double red, double green, double blue) : color_(red, green, blue) {}

        /*!
         * Destructor.
         */
        virtual ~SolidColor() {}

        /*!
         * Inherited from Texture.
         */
        virtual Vector3d value(double /*u*/, double /*v*/, const Vector3d& /*p*/) const override {
          return color_;
        }

      private:
        Vector3d color_; //!< Color for this texture
    };

    /*!
     * \brief Class representing a procedural checker texture.
     */
    class CheckerTexture : public Texture {
      public:

        /*!
         * Default constructor.
         */
        CheckerTexture() {}

        /*!
         * Constructor taking textures to alternate in checkerboard pattern.
         */
        CheckerTexture(std::shared_ptr<Texture> even, std::shared_ptr<Texture>
            odd) : even_(even), odd_(odd){}

        /*!
         * Constructor taking colors to alternate in checkerboard pattern.
         */
        CheckerTexture(const Vector3d& c1, const Vector3d& c2) :
          even_(std::make_shared<SolidColor>(c1)),
          odd_(std::make_shared<SolidColor>(c2)) {}

        /*!
         * Destructor.
         */
        virtual ~CheckerTexture() {}

        /*!
         * Inherited from Texture.
         */
        virtual Vector3d value(double u, double v, const Vector3d& p) const override;

      public:
        std::shared_ptr<Texture> even_;
        std::shared_ptr<Texture> odd_;
    };

    /*!
     * \brief Class representing a Perlin noise texture.
     */
    class NoiseTexture : public Texture {
      public:

        /*!
         * Default constructor.
         */
        NoiseTexture() {}

        /*!
         * Constructor taking a noise scale.
         */
        NoiseTexture(double scale) : scale_(scale) {}

        /*!
         * Destructor.
         */
        virtual ~NoiseTexture() {}

        /*!
         * Inherited from Texture.
         */
        virtual Vector3d value(double /*u*/, double /*v*/, const Vector3d& p) const override {
          return Vector3d(1, 1, 1) * 0.5 * (1.0 +
              std::sin(scale_ * p.z() + 10*noise_.turbulence(p)));
        }

      public:
        Perlin noise_; //!< Perlin noise generator
        double scale_; //!< Noise scale
    };

    /*!
     * \brief Class representing a texture derived from an image.
     */
    class ImageTexture : public Texture {
      public:

        /*!
         * Default constructor.
         */
        ImageTexture() : data_(nullptr), width_(0), height_(0), bytes_per_scanline_(0) {}

        /*!
         * Constructor taking filename of image to load.
         */
        ImageTexture(const std::string& filename) {
          auto components_per_pixel = bytes_per_pixel;

          data_ = stbi_load(filename.c_str(), &width_, &height_,
              &components_per_pixel, components_per_pixel);

          if (!data_) {
            throw std::runtime_error("Could not open image file for texture.");
          }

          bytes_per_scanline_ = bytes_per_pixel * width_;
        }

        /*!
         * Destructor.
         */
        virtual ~ImageTexture () {}

        /*!
         * Inherited from Texture.
         */
        virtual Vector3d value(double u, double v, const Vector3d& p) const override;

        const static int bytes_per_pixel = 3; //!< Bytes per pixel in this texture

      private:
        unsigned char *data_; //!< Image data
        int width_, height_; //!< Image width, height
        int bytes_per_scanline_; //!< Number of bytes per scanline in image

    };

  } // namespace ray
} // namespace cannon

#endif /* ifndef CANNON_RAY_TEXTURE_H */


//=======================================================================
// Copyright (c) 2014 Karol Wegrzycki
//
// 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)
//=======================================================================
/**
 * @file lsh_regression_basic_test.cpp
 * @brief lsh_regression binnary basic functionality
 * @author Karol Wegrzycki
 * @version 1.0
 * @date 2014-11-19
 */
#include "test_utils/logger.hpp"
#include "test_utils/get_test_dir.hpp"
#include "test_utils/system.hpp"

#include <boost/range/algorithm/equal.hpp>
#include <boost/range/istream_range.hpp>
#include <boost/test/unit_test.hpp>

#include <fstream>
#include <string>

namespace {
std::string lsh_bin = paal::system::get_build_dir("/bin/lsh-regression");
double EPSILON = 10e-9;

void call(std::string const &command) {
    BOOST_CHECK_MESSAGE(paal::system::exec(command) == 0, "Command failed: " << command);
}

void call_fail(std::string const &command) {
    BOOST_CHECK_MESSAGE(paal::system::exec(command) != 0, "Command that should fail succeeded: " << command);
}

using paal::system::get_temp_file_path;

void test_files_are_equal(std::string const &file_path_1, std::string const &file_path_2) {
    std::ifstream file_1_stream{file_path_1}, file_2_stream{file_path_2};
    auto file_1_contents = boost::istream_range<double>(file_1_stream);
    auto file_2_contents = boost::istream_range<double>(file_2_stream);

    auto check_close_doubles_always_true = [](double lhs, double rhs) {
        BOOST_CHECK_CLOSE(lhs, rhs, EPSILON);
        return true;
    };
    // TODO use BOOST_CHECK_CLOSE_RANGES when appears
    BOOST_CHECK_MESSAGE(boost::equal(file_1_contents, file_2_contents, check_close_doubles_always_true),
                        "Files " << file_path_1 << " and " << file_path_2 << " are different.");
}

} //! anonymous

using paal::system::create_tmp_file;

BOOST_AUTO_TEST_SUITE(lsh_regression_bin_tests)

BOOST_AUTO_TEST_CASE(lsh_bin_simple) {
    std::string training = create_tmp_file("training_1", "1 0:1\n0 1:1");
    std::string test = create_tmp_file("test_1", "1 0:1");
    std::string expt = create_tmp_file("expect_1", "1");
    std::string result = get_temp_file_path("simple_test_1.svm");

    call(lsh_bin + " -d " + training + " -t " + test + " -o " + result + " --dimensions=1");
    test_files_are_equal(result, expt);
}

BOOST_AUTO_TEST_CASE(lsh_bin_test_formats) {
    std::string training = create_tmp_file("training_f", "  0   1:1e-6 2:1.5 4:1. 7:.5\n1\n0");
    std::string test = create_tmp_file("test_f", "0   1:1e-6 2:1.5 4:1. 7:.5\n 1 8:1");
    std::string result = get_temp_file_path("result_test_f.svm");

    call(lsh_bin + " -d " + training + " -t " + test + " -o " + result + " --dimensions=9");
}

BOOST_AUTO_TEST_CASE(lsh_bin_test_metrics) {
    std::string training = create_tmp_file("training_l2", "0 0:1\n 1 0:10000\n 0 1:5");
    std::string test = create_tmp_file("test_l2", "0 0:3");
    std::string expect = create_tmp_file("expect_l2", "0");
    std::string result = get_temp_file_path("result_test_l2.svm");

    call(lsh_bin + " -d " + training + " -t " + test + " -o " + result + " -m l2" + " --dimensions=2");
    test_files_are_equal(result, expect);
    call(lsh_bin + " -d " + training + " -t " + test + " -o " + result + " -m l1" + " --dimensions=2");
    test_files_are_equal(result, expect);
}

BOOST_AUTO_TEST_CASE(lsh_bin_w_option) {
    std::string training = create_tmp_file("training_w", "0 0:1\n 1 0:10000");
    std::string test = create_tmp_file("test_w", "0 0:2");

    std::string result = get_temp_file_path("result_w");

    call(lsh_bin + " -d " + training + " -t " + test + " -o " + result + " -m l2 -w 1" + " --dimensions=1");
    std::string expect05 = create_tmp_file("expect_w05", "0.5");
    test_files_are_equal(result, expect05);

    call(lsh_bin + " -d " + training + " -t " + test + " -o " + result + " -m l2 -w 10" + " --dimensions=1");
    std::string expect0 = create_tmp_file("expect_w0", "0");
    test_files_are_equal(result, expect0);
}

BOOST_AUTO_TEST_CASE(lsh_bin_bad_usage) {
    std::string sample_data_file = create_tmp_file("sample_data_file", "1 0:1");
    std::string sample_test_file = create_tmp_file("sample_test_file", "1 0:2");
    std::string empty_file =  create_tmp_file("sample_file", "");

    call_fail(lsh_bin + " -d " + sample_data_file + " -t " + sample_test_file);
    call_fail(lsh_bin + " -d " + sample_data_file + " -t " + empty_file + " --dimensions=1");
    call_fail(lsh_bin + " " + sample_data_file + " -t " + sample_test_file + " --dimensions=1");
    call_fail(lsh_bin + " -o " + empty_file + " --dimensions=1");
    call_fail(lsh_bin);

    call(lsh_bin + " --help");
}

BOOST_AUTO_TEST_CASE(lsh_bin_serialization) {
    std::string example = create_tmp_file("example", "1 0:1");
    std::string test = create_tmp_file("test", "0 0:3");
    std::string model(get_temp_file_path("model.1")),
                model2(get_temp_file_path("model.2")),
                res(get_temp_file_path("res.txt"));

    call(lsh_bin + " --model_out " + model + " -d " + example + " --dimensions=1");
    call(lsh_bin + " --model_in " + model + " -o " + res + " -t " + test);
    std::string expect = create_tmp_file("expect", "1");
    test_files_are_equal(res, expect);

    std::string example2 = create_tmp_file("example2", "1 0:1\n0 0:1\n0 0:1");

    call(lsh_bin + " --model_in " + model + " --model_out " + model2 + " -d " + example2);
    call(lsh_bin + " --model_in " + model2 + " -o " + res + " -t " + test);
    std::string expect2 = create_tmp_file("expect2", "0.5");
    test_files_are_equal(res, expect2);
}

BOOST_AUTO_TEST_SUITE_END()


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

#include "modified_helmholtz_3d_calderon_projector.hpp"
#include "../common/shared_ptr.hpp"

#include "modified_helmholtz_3d_single_layer_boundary_operator.hpp"
#include "modified_helmholtz_3d_double_layer_boundary_operator.hpp"
#include "modified_helmholtz_3d_hypersingular_boundary_operator.hpp"
#include "identity_operator.hpp"
#include "blocked_operator_structure.hpp"
#include "../fiber/explicit_instantiation.hpp"

#include <boost/make_shared.hpp>

namespace Bempp {

template <typename BasisFunctionType, typename KernelType, typename ResultType>
BlockedBoundaryOperator<BasisFunctionType, ResultType>
modifiedHelmholtz3dExteriorCalderonProjector(
    const shared_ptr<const Context<BasisFunctionType, ResultType>> &context,
    const shared_ptr<const Space<BasisFunctionType>> &hminusSpace,
    const shared_ptr<const Space<BasisFunctionType>> &hplusSpace,
    KernelType waveNumber, const std::string &label, bool useInterpolation,
    int interpPtsPerWavelength) {

  typedef BoundaryOperator<BasisFunctionType, ResultType> BdOp;

  shared_ptr<const Space<BasisFunctionType>> internalHplusSpace =
      hplusSpace->discontinuousSpace(hplusSpace);

  BdOp internalSlp = modifiedHelmholtz3dSingleLayerBoundaryOperator(
      context, internalHplusSpace, internalHplusSpace, internalHplusSpace,
      waveNumber, label + "_slp", SYMMETRIC, useInterpolation,
      interpPtsPerWavelength);

  BdOp dlp = modifiedHelmholtz3dDoubleLayerBoundaryOperator(
      context, hplusSpace, hplusSpace, hminusSpace, waveNumber, label + "_dlp",
      NO_SYMMETRY, useInterpolation, interpPtsPerWavelength);

  BdOp adjDlp = adjoint(dlp);

  BdOp hyp = modifiedHelmholtz3dHypersingularBoundaryOperator(
      context, hplusSpace, hminusSpace, hplusSpace, waveNumber, label + "_hyp",
      SYMMETRIC, useInterpolation, interpPtsPerWavelength, internalSlp);

  BdOp idSpaceTransformation1 = identityOperator(
      context, hminusSpace, internalHplusSpace, internalHplusSpace);
  BdOp idSpaceTransformation2 =
      identityOperator(context, internalHplusSpace, hplusSpace, hminusSpace);
  BdOp idDouble =
      identityOperator(context, hplusSpace, hplusSpace, hminusSpace);
  BdOp idAdjDouble =
      identityOperator(context, hminusSpace, hminusSpace, hplusSpace);

  // Now Assemble the entries of the Calderon Projector

  BlockedOperatorStructure<BasisFunctionType, ResultType> structure;

  structure.setBlock(0, 0, .5 * idDouble + dlp);
  structure.setBlock(0, 1, -1. * idSpaceTransformation2 * internalSlp *
                               idSpaceTransformation1);
  structure.setBlock(1, 0, -1. * hyp);
  structure.setBlock(1, 1, .5 * idAdjDouble - adjDlp);

  return BlockedBoundaryOperator<BasisFunctionType, ResultType>(structure);
}

template <typename BasisFunctionType, typename KernelType, typename ResultType>
BlockedBoundaryOperator<BasisFunctionType, ResultType>
modifiedHelmholtz3dInteriorCalderonProjector(
    const shared_ptr<const Context<BasisFunctionType, ResultType>> &context,
    const shared_ptr<const Space<BasisFunctionType>> &hminusSpace,
    const shared_ptr<const Space<BasisFunctionType>> &hplusSpace,
    KernelType waveNumber, const std::string &label, bool useInterpolation,
    int interpPtsPerWavelength) {

  typedef BoundaryOperator<BasisFunctionType, ResultType> BdOp;

  shared_ptr<const Space<BasisFunctionType>> internalHplusSpace =
      hplusSpace->discontinuousSpace(hplusSpace);

  BdOp internalSlp = modifiedHelmholtz3dSingleLayerBoundaryOperator(
      context, internalHplusSpace, internalHplusSpace, internalHplusSpace,
      waveNumber, label + "_slp", SYMMETRIC, useInterpolation,
      interpPtsPerWavelength);

  BdOp dlp = modifiedHelmholtz3dDoubleLayerBoundaryOperator(
      context, hplusSpace, hplusSpace, hminusSpace, waveNumber, label + "_dlp",
      NO_SYMMETRY, useInterpolation, interpPtsPerWavelength);

  BdOp adjDlp = adjoint(dlp);

  BdOp hyp = modifiedHelmholtz3dHypersingularBoundaryOperator(
      context, hplusSpace, hminusSpace, hplusSpace, waveNumber, label + "_hyp",
      SYMMETRIC, useInterpolation, interpPtsPerWavelength, internalSlp);

  BdOp idSpaceTransformation1 = identityOperator(
      context, hminusSpace, internalHplusSpace, internalHplusSpace);
  BdOp idSpaceTransformation2 =
      identityOperator(context, internalHplusSpace, hplusSpace, hminusSpace);
  BdOp idDouble =
      identityOperator(context, hplusSpace, hplusSpace, hminusSpace);
  BdOp idAdjDouble =
      identityOperator(context, hminusSpace, hminusSpace, hplusSpace);

  // Now Assemble the entries of the Calderon Projector

  BlockedOperatorStructure<BasisFunctionType, ResultType> structure;

  structure.setBlock(0, 0, .5 * idDouble - dlp);
  structure.setBlock(0, 1, idSpaceTransformation2 * internalSlp *
                               idSpaceTransformation1);
  structure.setBlock(1, 0, hyp);
  structure.setBlock(1, 1, .5 * idAdjDouble + adjDlp);

  return BlockedBoundaryOperator<BasisFunctionType, ResultType>(structure);
}

#define INSTANTIATE_NONMEMBER_CONSTRUCTOR(BASIS, KERNEL, RESULT)               \
  template BlockedBoundaryOperator<BASIS, RESULT>                              \
  modifiedHelmholtz3dExteriorCalderonProjector(                                \
      const shared_ptr<const Context<BASIS, RESULT>> &,                        \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &, KERNEL, const std::string &,     \
      bool, int);                                                              \
  template BlockedBoundaryOperator<BASIS, RESULT>                              \
  modifiedHelmholtz3dInteriorCalderonProjector(                                \
      const shared_ptr<const Context<BASIS, RESULT>> &,                        \
      const shared_ptr<const Space<BASIS>> &,                                  \
      const shared_ptr<const Space<BASIS>> &, KERNEL, const std::string &,     \
      bool, int)

FIBER_ITERATE_OVER_BASIS_KERNEL_AND_RESULT_TYPES(
    INSTANTIATE_NONMEMBER_CONSTRUCTOR);

} // Bempp


/* ----------------------------------------------------------------------
 *
 *                    *** Smooth Mach Dynamics ***
 *
 * This file is part of the USER-SMD package for LAMMPS.
 * Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
 * Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
 * Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
 *
 * ----------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
 http://lammps.sandia.gov, Sandia National Laboratories
 Steve Plimpton, sjplimp@sandia.gov

 Copyright (2003) Sandia Corporation.  Under the terms of Contract
 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
 certain rights in this software.  This software is distributed under
 the GNU General Public License.

 See the README file in the top-level LAMMPS directory.
 ------------------------------------------------------------------------- */

#include <string.h>
#include "compute_smd_tlsph_defgrad.h"
#include "atom.h"
#include "update.h"
#include "modify.h"
#include "comm.h"
#include "force.h"
#include "memory.h"
#include "error.h"
#include "pair.h"
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <Eigen/Eigen>
using namespace Eigen;
using namespace std;
using namespace LAMMPS_NS;

/* ---------------------------------------------------------------------- */

ComputeSMDTLSPHDefgrad::ComputeSMDTLSPHDefgrad(LAMMPS *lmp, int narg, char **arg) :
		Compute(lmp, narg, arg) {
	if (narg != 3)
		error->all(FLERR, "Illegal compute smd/tlsph_defgrad command");

	peratom_flag = 1;
	size_peratom_cols = 10;

	nmax = 0;
	defgradVector = NULL;
}

/* ---------------------------------------------------------------------- */

ComputeSMDTLSPHDefgrad::~ComputeSMDTLSPHDefgrad() {
	memory->sfree(defgradVector);
}

/* ---------------------------------------------------------------------- */

void ComputeSMDTLSPHDefgrad::init() {

	int count = 0;
	for (int i = 0; i < modify->ncompute; i++)
		if (strcmp(modify->compute[i]->style, "smd/tlsph_defgrad") == 0)
			count++;
	if (count > 1 && comm->me == 0)
		error->warning(FLERR, "More than one compute smd/tlsph_defgrad");
}

/* ---------------------------------------------------------------------- */

void ComputeSMDTLSPHDefgrad::compute_peratom() {
	double **defgrad = atom->smd_data_9;
	Matrix3d F;
	invoked_peratom = update->ntimestep;

	// grow vector array if necessary
	if (atom->nmax > nmax) {
		memory->destroy(defgradVector);
		nmax = atom->nmax;
		memory->create(defgradVector, nmax, size_peratom_cols, "defgradVector");
		array_atom = defgradVector;
	}

	int *mask = atom->mask;
	int nlocal = atom->nlocal;

	for (int i = 0; i < nlocal; i++) {
		if (mask[i] & groupbit) {
			F(0, 0) = defgrad[i][0];
			F(0, 1) = defgrad[i][1];
			F(0, 2) = defgrad[i][2];
			F(1, 0) = defgrad[i][3];
			F(1, 1) = defgrad[i][4];
			F(1, 2) = defgrad[i][5];
			F(2, 0) = defgrad[i][6];
			F(2, 1) = defgrad[i][7];
			F(2, 2) = defgrad[i][8];

			defgradVector[i][0] = F(0, 0);
			defgradVector[i][1] = F(0, 1);
			defgradVector[i][2] = F(0, 2);
			defgradVector[i][3] = F(1, 0);
			defgradVector[i][4] = F(1, 1);
			defgradVector[i][5] = F(1, 2);
			defgradVector[i][6] = F(2, 0);
			defgradVector[i][7] = F(2, 1);
			defgradVector[i][8] = F(2, 2);
			defgradVector[i][9] = F.determinant();
		} else {
			defgradVector[i][0] = 1.0;
			defgradVector[i][1] = 0.0;
			defgradVector[i][2] = 0.0;
			defgradVector[i][3] = 0.0;
			defgradVector[i][4] = 1.0;
			defgradVector[i][5] = 0.0;
			defgradVector[i][6] = 0.0;
			defgradVector[i][7] = 0.0;
			defgradVector[i][8] = 1.0;
			defgradVector[i][9] = 1.0;
		}
	}
}

/* ----------------------------------------------------------------------
 memory usage of local atom-based array
 ------------------------------------------------------------------------- */

double ComputeSMDTLSPHDefgrad::memory_usage() {
	double bytes = size_peratom_cols * nmax * sizeof(double);
	return bytes;
}


/*==========================================================
 * tCGGPsumrnd.cpp
 *
 * The calling syntax is:
 *
 *		S_w = tCGGPsumrnd(p, logalpha, sigma, tau, a, b, gamma, T)
 *		S_w = tCGGPsumrnd(p, logalpha, sigma, tau, a, b, gamma, T, seed)
 *
 * Compilation command (from MATLAB console)
 * -----------------------------------------
 * Using gcc (Linux, Mac):
 * - using a C++11 compiler (gcc>=4.3):
 *
 *      mex tCGGPsumrnd.cpp CXXFLAGS='$CXXFLAGS -std=c++0x -fopenmp' LDFLAGS='$LDFLAGS -fopenmp'
 *
 * - using Boost installed in a standard path:
 *
 *      mex tCGGPsumrnd.cpp CXXFLAGS='$CXXFLAGS -fopenmp' LDFLAGS='$LDFLAGS -fopenmp'
 *
 * - using custom Boost path:
 *
 *      mex tCGGPsumrnd.cpp -Ipath/to/boost CXXFLAGS='$CXXFLAGS -fopenmp' LDFLAGS='$LDFLAGS -fopenmp'
 *
 * Using Microsoft Visual C++ (Windows) and Boost:
 *
 *      mex tCGGPsumrnd.cpp -Ipath/to/boost COMPFLAGS="$COMPFLAGS /openmp" LINKFLAGS="$LINKFLAGS /openmp"
 *
 * e.g: mex tCGGPsumrnd.cpp -I"C:/Program Files/boost/boost_1_58_0" COMPFLAGS="$COMPFLAGS /openmp" LINKFLAGS="$LINKFLAGS /openmp"
 *
 *========================================================*/

#include "mex.h"
#include <cmath>
#include <ctime>
#include <limits>
#include <omp.h>

typedef double double_;

#if __cplusplus != 199711L /* C++11 std */

#include <random>
typedef std::default_random_engine generator;
typedef std::uniform_real_distribution<double_> uniform_dist;
typedef std::gamma_distribution<double_> gamma_dist;

#else /* Boost */

#include <boost/math/special_functions/gamma.hpp>
using boost::math::lgamma;

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/gamma_distribution.hpp>
typedef boost::random::mt19937 generator;
typedef boost::random::uniform_real_distribution<double_> uniform_dist;
typedef boost::random::gamma_distribution<double_> gamma_dist;

#endif

void tCGGPsumrnd( const unsigned p,  const double logalpha, const double sigma,
        const double tau, const double * a, const double * b, const double * gamma,
        double_ T, const double_ Tmax, const unsigned seed,
        double_ * S_w );

void tCGGPsumrnd_tau0( const unsigned p,  const double logalpha, const double sigma,
        const double * a, const double * b, const double * gamma,
        double_ T, const double_ Tmax, const unsigned seed,
        double_ * S_w );

/* The gateway function */
void mexFunction( int nlhs, mxArray *plhs[],
                  int nrhs, const mxArray *prhs[] )
{
    /* check for proper number of arguments */
    if(nrhs<8) {
        mexErrMsgTxt("At least 8 inputs required.");
    }
    if(nlhs!=1) {
        mexErrMsgTxt("One output required.");
    }

    /* get the value of the input  */
    const unsigned p = mxGetScalar( prhs[0] );
    const double logalpha = mxGetScalar( prhs[1] );
    const double sigma = mxGetScalar( prhs[2] );
    const double tau = mxGetScalar( prhs[3] );
    double * a = mxGetPr( prhs[4] );
    const unsigned na = mxGetNumberOfElements( prhs[4] );
    double * b = mxGetPr( prhs[5] );
    const unsigned nb = mxGetNumberOfElements( prhs[5] );
    double * gamma = mxGetPr( prhs[6] );
    const unsigned ng = mxGetNumberOfElements( prhs[6] );
    const double T = mxGetScalar( prhs[7] );
    double seed = time(NULL);
    if ( nrhs >= 9 )
        seed = mxGetScalar( prhs[8] );
    // number of concurent jobs
    unsigned nthreads = omp_get_max_threads();
    if ( nrhs >= 10 )
        nthreads = unsigned(mxGetScalar( prhs[9] ));

    if ( na < p && na==1 ) {
        // duplicate value of a
        double a_val = *a;
        a = new double[p];
        for ( unsigned k = 0; k < p; ++k )
            a[k] = a_val;
    }
    if ( nb < p && nb==1 ) {
        // duplicate value of b
        double b_val = *a;
        b = new double[p];
        for ( unsigned k = 0; k < p; ++k )
            b[k] = b_val;
    }
    if ( ng < p && ng==1 ) {
        // duplicate value of gamma
        double g_val = *a;
        gamma = new double[p];
        for ( unsigned k = 0; k < p; ++k )
            gamma[k] = g_val;
    }

    // expected log number of jumps
    const double_ cst = logalpha - lgamma( 1.0 - sigma ) - log(sigma);
    const double_ log_njumps = cst - sigma * log(T);

    /* cut into nthreads intervals [Tcut(i), Tcut(i+1)] such that the expected
     * number of jumps is njumps/nthreads in each interval */
    double_ * Tcut = new double_[nthreads+1];
    Tcut[0] = T;
    for (unsigned i = 1; i < nthreads; ++i) {
        // TODO: find a better bound of the expected number of jumps
        Tcut[i] = pow( pow(Tcut[i-1], -sigma) - exp( log_njumps - log(double(nthreads)) - cst ), -1.0 / sigma);
    }
    Tcut[nthreads] = std::numeric_limits<double_>::infinity();

    double_ ** S_w = new double_*[nthreads];
    for (unsigned i = 0; i < nthreads; ++i)
        S_w[i] = new double_[p];

    /* call the computational routine */
    if ( tau < 1e-8 ) {
        // case tau==0
        #pragma omp parallel for
        for (int i = 0; i < nthreads; ++i) {
            tCGGPsumrnd_tau0( p, logalpha, sigma, a, b, gamma, Tcut[i], Tcut[i+1], unsigned(seed)+i,
                S_w[i] );
        }
    } else {
        // case tau>0
        #pragma omp parallel for
        for (int i = 0; i < nthreads; ++i) {
            tCGGPsumrnd( p, logalpha, sigma, tau, a, b, gamma, Tcut[i], Tcut[i+1], unsigned(seed)+i,
                S_w[i] );
        }
    }

    /* reduction */
    for (unsigned i = 1; i < nthreads; ++i) {
        for (unsigned k = 0; k < p; ++k)
            S_w[0][k] += S_w[i][k];
    }

    /* create the output */
    plhs[0] = mxCreateDoubleMatrix( 1, p, mxREAL );
    for (unsigned k = 0; k < p; ++k)
        mxGetPr( plhs[0] )[k] = S_w[0][k];

    /* deallocate memory */
    delete[] Tcut;
    for (unsigned i = 0; i < nthreads; ++i)
        delete[] S_w[i];
    delete[] S_w;
    if ( na < p && na==1 )
        delete[] a;
    if ( nb < p && nb==1 )
        delete[] b;
    if ( ng < p && ng==1 )
        delete[] gamma;
}

/* The computational routine */

void tCGGPsumrnd( const unsigned p,  const double logalpha, const double sigma,
        const double tau, const double * a, const double * b, const double * gamma,
        double_ T, const double_ Tmax, const unsigned seed,
        double_ * S_w )
{
    const double_ sigmap1 = sigma + 1.0;
    const double_ tauinv = 1.0 / tau;
    const double_ log_cst = logalpha - lgamma( 1.0 - sigma ) - log(tau);
    double_ * gamma_b = new double_[p];
    for ( unsigned k = 0; k < p; ++k )
        gamma_b[k] = gamma[k]/b[k];
    double_ log_r;
    double_ log_mgf;
    double_ t_new;
    double_ log_mgf_new;
    double_ log_G;
    generator gen(seed);
    uniform_dist unif( 0.0, 1.0 );
    gamma_dist gam;

    for ( unsigned k = 0; k < p; ++k )
        S_w[k] = 0.0;

    while (true) {
        log_r = log( -log( unif(gen) ) );
        log_mgf = 0.0;
        for ( unsigned k = 0; k < p; ++k )
            log_mgf -= a[k] * log1p( T * gamma_b[k] );
        log_G = log_mgf + log_cst - sigmap1 * log(T) - tau * T;
        if ( log_r > log_G )
            break;
        t_new = T - tauinv * log1p( -exp( log_r - log_G ) );
        if ( t_new > Tmax )
            break;
        log_mgf_new = 0.0;
        for ( unsigned k = 0; k < p; ++k )
            log_mgf_new -= a[k] * log1p( t_new * gamma_b[k] );
        if ( log( unif(gen) ) < sigmap1 * ( log(T) - log(t_new) ) + log_mgf_new - log_mgf ) {
            for ( unsigned k = 0; k < p; ++k ) {
                gam = gamma_dist( a[k], 1 / ( b[k] + t_new * gamma[k] ) );
                S_w[k] += exp( log(t_new) + log( gam(gen) ) );
                //gam = gamma_dist( a[k], 1 / ( b[k] / t_new + gamma[k] ) );
                //S_w[k] += gam(gen);
            }
        }
        T = t_new;
    }

    delete[] gamma_b;
}

void tCGGPsumrnd_tau0( const unsigned p,  const double logalpha, const double sigma,
        const double * a, const double * b, const double * gamma,
        double_ T, const double_ Tmax, const unsigned seed,
        double_ * S_w )
{
    const double_ log_cst = logalpha - lgamma( 1.0 - sigma ) - log(sigma);
    const double_ msigma = -sigma;
    const double_ msigmainv = 1.0 / msigma;
    double_ * gamma_b = new double_[p];
    for ( unsigned k = 0; k < p; ++k )
        gamma_b[k] = gamma[k]/b[k];
    double_ log_r;
    double_ log_mgf;
    double_ t_new;
    double_ log_mgf_new;
    double_ log_temp;
    double_ log_tmsigma;
    generator gen(seed);
    uniform_dist unif( 0.0, 1.0 );
    gamma_dist gam;

    for ( unsigned k = 0; k < p; ++k )
        S_w[k] = 0.0;

    while (true) {
        log_r = log( -log( unif(gen) ) );
        log_mgf = 0.0;
        for ( unsigned k = 0; k < p; ++k )
            log_mgf -= a[k] * log1p( T * gamma_b[k] );
        log_temp = log_r - log_mgf - log_cst;
        log_tmsigma = msigma * log(T);
        if ( log_temp > log_tmsigma )
            break;
        t_new = pow( exp(log_tmsigma) - exp(log_temp) , msigmainv );
        if ( t_new > Tmax )
            break;
        log_mgf_new = 0.0;
        for ( unsigned k = 0; k < p; ++k )
            log_mgf_new -= a[k] * log1p( t_new * gamma_b[k] );
        if ( log( unif(gen) ) < log_mgf_new - log_mgf ) {
            for ( unsigned k = 0; k < p; ++k ) {
                gam = gamma_dist( a[k], 1 / ( b[k] / t_new + gamma[k] ) );
                S_w[k] += gam(gen);
            }
        }
        T = t_new;
    }

    delete[] gamma_b;
}


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file capacitated_facility_location_long_test.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2013-02-15
 */

#include "test_utils/logger.hpp"
#include "test_utils/read_orlib_fl.hpp"
#include "test_utils/test_result_check.hpp"
#include "test_utils/get_test_dir.hpp"
#include "test_utils/system.hpp"

#include "paal/local_search/facility_location/facility_location.hpp"
#include "paal/data_structures/voronoi/capacitated_voronoi.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/floating.hpp"
#include "paal/data_structures/facility_location/fl_algo.hpp"
#include "paal/utils/irange.hpp"

#include <boost/test/unit_test.hpp>

#include <iterator>
#include <iostream>
#include <fstream>
#include <iomanip>

using namespace paal::local_search;
using namespace paal;

template <typename Metric, typename Cost> class fl_logger {
  public:
    fl_logger(const Metric &m, const Cost &c) : m_metric(m), m_cost(c) {}

    template <typename Sol> bool operator()(Sol &sol) {
        ON_LOG(auto const &ch =
                   sol.getfacility_location_solution().get_chosen_facilities());
        LOG_COPY_RANGE_DEL(ch, ",");
        ON_LOG(
            auto c =
                sol.getfacility_location_solution().get_voronoi().get_cost());
        LOGLN("current cost "
              << simple_algo::get_cfl_cost(m_metric, m_cost,
                                           sol.getfacility_location_solution())
              << " (dist to full assign " << c.get_dist_to_full_assignment()
              << ")");
        return true;
    }

  private:
    const Metric &m_metric;
    const Cost &m_cost;
};

template <typename Metric, typename Cost>
fl_logger<Metric, Cost> make_fl_logger(const Metric &m, const Cost &c) {
    return fl_logger<Metric, Cost>(m, c);
}
;

template <typename Solve>
void run_tests(const std::string &fname, Solve solve) {
    std::string test_dir = paal::system::get_test_data_dir("CFL_ORLIB");
    using paal::system::build_path;
    std::ifstream is_test_cases(build_path(test_dir, fname));

    assert(is_test_cases.good());
    while (is_test_cases.good()) {
        std::string fname;
        double opt;
        is_test_cases >> fname >> opt;
        opt = cast(opt);
        if (fname == "") return;

        LOGLN("TEST " << fname);
        LOGLN(std::setprecision(20) << "OPT " << opt);

        std::ifstream ifs(build_path(test_dir, "cases/" + fname + ".txt"));
        assert(ifs.good());
        std::vector<long long> facCost;
        std::vector<int> facCap;
        std::vector<int> demands;
        boost::integer_range<int> fac(0, 0);
        boost::integer_range<int> clients(0, 0);
        auto metric = paal::read_orlib_FL<cap::capacitated>(
            ifs, facCost, facCap, demands, fac, clients);
        int firstClient = clients.front();

        auto cost = paal::utils::make_array_to_functor(facCost);
        auto verticesDemands =
            paal::utils::make_array_to_functor(demands, -firstClient);
        auto facCapacities = paal::utils::make_array_to_functor(facCap);
        LOGLN("demands sum" << std::accumulate(clients.begin(), clients.end(),
                                               0, [&](int d, int v) {
            return d + verticesDemands(v);
        }));

        typedef paal::data_structures::capacitated_voronoi<
            decltype(metric), decltype(facCapacities),
            decltype(verticesDemands)> VorType;

        typedef paal::data_structures::facility_location_solution<
            decltype(cost), VorType> Sol;
        typedef typename VorType::Generators FSet;
        typedef typename VorType::Vertices VSet;
        typedef typename Sol::UnchosenFacilitiesSet USet;

        VorType voronoi(FSet{ fac.begin(), fac.end() },
                        VSet(clients.begin(), clients.end()), metric,
                        facCapacities, verticesDemands);
        Sol sol(std::move(voronoi), USet{}, cost);

        solve.template operator()<VorType>(sol, metric, cost, opt,
                                           make_fl_logger(metric, cost));

        double c = simple_algo::get_cfl_cost(metric, cost, sol);
        check_result(c, opt, 6., paal::utils::less_equal(), 0.01);
    }
}

struct solve_add_remove {
    default_remove_fl_components rem;
    default_add_fl_components add;
    utils::always_false nop;

    template <typename VorType, typename Cost, typename Solution,
              typename Action, typename Metric>
    void operator()(Solution &sol, const Metric &metric, Cost cost, double opt,
                    Action a) {
        facility_location_local_search(sol, a, nop, rem);
        facility_location_local_search(sol, a, nop, rem, add);

        ON_LOG(double c = simple_algo::get_cfl_cost(metric, cost, sol));
        LOGLN(std::setprecision(20)
              << "BEFORE SWAP APPROXIMATION RATIO: " << c / opt);
    }
};

struct solve_add_remove_swap : public solve_add_remove {
    default_swap_fl_components swap;

    template <typename VorType, typename Cost, typename Solution,
              typename Action, typename Metric>
    void operator()(Solution &sol, const Metric &metric, Cost cost, double opt,
                    Action a) {
        paal::local_search::first_improving_strategy strategy;
        facility_location_local_search(sol, strategy, a, nop, rem);
        facility_location_local_search(sol, strategy, a, nop, rem, add);
        facility_location_local_search(sol, strategy, a, nop, rem, add, swap);
    }
};

BOOST_AUTO_TEST_CASE(CapacitatedFacilityLocationLong) {
    run_tests("capopt.txt", solve_add_remove_swap());
}

// currently this is too long !
// hope that in the future stronger algorithms'll handle these cases
//
//
// BOOST_AUTO_TEST_CASE(CapacitatedFacilityLocationVeryLong) {
//    run_tests("capopt_long.txt", solve_add_remove());
//}


#include "base/database.h"
#include "util/option_manager.h"

#include <iostream>
#include <string>
#include <vector>

#include <Eigen/Dense>

using namespace colmap;

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

  std::string db_path;
  bool stats_for_verified;

  OptionManager options;
  options.AddRequiredOption("db_path", &db_path);
  options.AddRequiredOption("verified", &stats_for_verified);
  options.Parse(argc, argv);

  Database db(db_path);

  if (!stats_for_verified){
    std::vector<std::pair<image_pair_t, FeatureMatches>> matches = db.ReadAllMatches();
    for (auto const& m : matches) {
      std::cout << m.second.size() << '\n';
    }
  } else {
    std::vector<std::pair<image_t, image_t>> image_pairs;
    std::vector<int> nr_verified_inliers;
    db.ReadTwoViewGeometryNumInliers(&image_pairs, &nr_verified_inliers);

    for (auto const& nr : nr_verified_inliers) {
      std::cout << nr << '\n';
    }
  }
}


/* Copyright (C) 2017 IBM Corp.
 *  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.
 */
/////////////////////////////
// sampleG - Sampling pre-image z such that G*z = v, G is the "easy" matrix
//////////////////////////////////////////

//#include <mpfr.h>
#include <NTL/mat_lzz_p.h>
#include "utils/timing.h"

NTL_CLIENT

#include "mat_l.h"
#include "stash.h"
#include "TDMatrix.h"

static zz_p
updateSyndromValue(vec_l &xOut, const Vec<vec_zz_p>& syndrom,
		   TDMatrixParams* params, long lFactor, long row,long offset);

//#define DEBUG

// Sample from a discrete Gaussian until you get a sample point equal
// to u modulo f. Uses a stack to put points that are not equal to u.
static long sampleMod(long u, long f,
                      Gaussian1Dsampler& sampler, SampleStash& stash)
{
  FHE_TIMER_START;

  long umodf = u%f;
  if (umodf<0) umodf += f;

  long val = (long)stash.getPoint(umodf);
  if (val != umodf+1) // found in stash
    return val;

  // If not found in stash, sample until you get it
  while (true) {
    NTL::Pair<bool,long> extra(false,0);
    val = sampler.getSample(/*mu=*/0.0, extra);
    long valmodf = val%f;
    if (valmodf<0) valmodf += f;

    if ( valmodf == umodf ) // found
      return val;
    else                    // not found, keep point for later
      stash.setPoint(valmodf, (int)val);
  }
}

// SampleG: Sample vector xOut such that G * xOut = syndrom. syndrom is given
// in CRT representation but x is a vector of longs.

// Generate a vector of size params>n*kFactors*e
// For each row in G, choose kFactors*e elements of xOut

// The function updates the syndrom vector, based on all previously
// chosen values in xOut. We update syndrom[lFactor][row]
// only just before we need to use it (as opposed to updating
// it for every new value of xOut as it is chosen)

// based on the updated syndrome, we sample a new entry in xOut,
// equal to uVal mod factor and append it to xOut

int sampleG(vec_l &xOut,
            const Vec<vec_zz_p>& syndrom, TDMatrixParams* params)
{
  FHE_TIMER_START;
  xOut.SetLength(0);
  xOut.SetMaxLength(params->m); // allocate space

  long sigma = params->r * params->maxFactor;
  Gaussian1Dsampler sampler(sigma);

  zz_pPush push; // backup the NTL current modulus

  // For each row in G, choose kFactors*e elements of xOut
  for (long row = 0; row < params->n; row++)
    {
      long offset = row * (params->kFactors) * (params->e);

      for (long lFactor = 0; lFactor < params->kFactors; lFactor++)
        {
	  long factor = params->factors[lFactor];
	  params->zzp_context[lFactor].restore(); // NTL-modulus := factor

	  // lazy update of the syndrom vector, based on all previously
	  // chosen values in xOut. We update syndrom[lFactor][row]
	  // only just before we need to use it (as opposed to updating
	  // it for every new value of xOut as it is chosen).

	  zz_p zzNewVal = updateSyndromValue(xOut, syndrom,params, lFactor, row, offset);

	  long uVal = conv<long>(zzNewVal);

	  // Done updating the uSyndrom, now choose next e elements in xOut

	  for (long lPower = 0; lPower < params->e; lPower++)
            {
	      // sample a new entry in xOut, equal to uVal mod factor
	      long samp = sampleMod(uVal, factor, sampler, params->stash[lFactor]);
	      assert( 0 == ((uVal-samp)%factor) ); // sanity check

	      xOut.append(samp);

	      // update uVal := (uVal-samp)/factor (over the integers)
	      uVal -= samp;
	      uVal /= factor;
            }
        }
    }
  return 0;
}

// lazy update of the syndrom vector, based on all previously chosen values
// in xOut. We update syndrom[lFactor][row] just before we need to use it
// (as opposed to updating it for every new value of xOut as it is chosen).

static zz_p
updateSyndromValue(vec_l &xOut, const Vec<vec_zz_p>& syndrom,
		   TDMatrixParams* params, long lFactor, long row, long offset)
{
    zz_p zzNewVal = syndrom[lFactor][row];
#ifdef DEBUG
    cout << zzNewVal << endl;
#endif
    long xIndex=offset;
    for (long vFactor = 0; vFactor < lFactor; vFactor++)
        for (long ie=0; ie < params->e; ie++) // repeat e times
        {
            long xVal = xOut[xIndex++];     // xIndex = offset+ie
            zzNewVal -= xVal;
            zzNewVal *= params->fInv[vFactor][lFactor];
            // update via u := (u - xval)/pi (mod pj)
#ifdef DEBUG
            cout << zzNewVal << endl;
#endif
        }
    return zzNewVal;
}