/*
 * MIT License
 *
 * Copyright (c) 2018 Hippolyte Barraud
 *
 * 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.
 */

#pragma once

#include <chrono>
#include <numeric>
#include <boost/algorithm/string.hpp>
#include <seastar/core/app-template.hh>
#include <seastar/core/print.hh>
#include <ultramarine/cluster/cluster.hpp>
#include <seastar/core/sleep.hh>

#define ULTRAMARINE_BENCH(name) {#name, name}

template<typename T>
T mean(const std::vector<T> &data) {
    return std::accumulate(data.begin(), data.end(), 0.0) / data.size();
}

template<typename T>
T variance(const std::vector<T> &data) {
    double xBar = mean(data);
    double sqSum = std::inner_product(data.begin(), data.end(), data.begin(), 0.0);
    return sqSum / data.size() - xBar * xBar;
}

template<typename T>
T stDev(const std::vector<T> &data) {
    return std::sqrt(variance(data));
}

namespace ultramarine::benchmark {

    using benchmark_list = std::initializer_list<std::pair<std::string_view, seastar::future<> (*)()>>;

    template<typename Pair>
    auto run_one(Pair &&bench, int run) {
        using namespace std::chrono;

        int *counter = new int(0);
        auto bench_start = high_resolution_clock::now();
        return seastar::do_with(std::move(bench), std::vector<microseconds::rep>(), [counter, run, bench_start]
                (auto &bench, auto &vec) {
            return seastar::do_until([counter, run] { return *counter >= run; }, [counter, &bench, &vec] {
                ++*counter;
                auto start = high_resolution_clock::now();
                return std::get<1>(bench)().then([start, &vec] {
                    auto stop = high_resolution_clock::now();
                    vec.emplace_back(duration_cast<microseconds>(stop - start).count());
                    return seastar::make_ready_future();
                });
            }).then([&vec, &bench, bench_start] {
                std::sort(std::begin(vec), std::end(vec));
                auto sum = std::accumulate(std::begin(vec), std::end(vec), 0UL);
                seastar::print("%s:"
                               "\n\titerations   : %d"
                               "\n\tavr          : %lu us (%lu ms)"
                               "\n\tmin          : %lu us (%lu ms)"
                               "\n\t99.9p        : %lu us (%lu ms)"
                               "\n\tstd          : %lu us (%lu ms)"
                               "\n\tTotal elapsed: %lu ms\n",
                               std::get<0>(bench), vec.size(),
                               sum / vec.size(), sum / vec.size() / 1000, // avr
                               *std::begin(vec), *std::begin(vec) / 1000, // min
                               *(std::end(vec) - 1), *(std::end(vec) - 1) / 1000, // max
                               stDev(vec), stDev(vec) / 1000,
                               duration_cast<milliseconds>(high_resolution_clock::now() - bench_start).count());
            });
        });
    }

#ifdef CLUSTERED_BENCHMARK
    int run(int ac, char **av, benchmark_list &&benchs, int run = 1000) {
        seastar::app_template app;
        namespace bpo = boost::program_options;
        app.add_options()
                ("local,l", bpo::value<std::string>(), "Local node address in format 'ip4:port'")
                ("minimum-peers,m", bpo::value<int>()->default_value(1), "Wait for the cluster to be least this large")
                ("initiator", bpo::value<bool>()->default_value(false), "Initiate benchmark")
                ("peers", bpo::value<std::vector<std::string>>()->multitoken(), "List of peers in format 'ip4:port'");

        return app.run(ac, av, [benchs = std::move(benchs), run, &app] {
            auto &&config = app.configuration();

            if (!config.count("local")) {
                seastar::print("Missing --local argument\n");
            }

            auto str_to_socketaddress = [] (std::string const& str) {
                std::vector<std::string> strs;
                boost::split(strs, str, boost::is_any_of(":"));
                return seastar::socket_address(seastar::ipv4_addr(strs[0], strtoul(strs[1].c_str(), nullptr, 0)));
            };

            std::vector<seastar::socket_address> peers;
            seastar::socket_address local;
            if (config.count("peers")) {
                for (const std::string &item : config["peers"].as<std::vector<std::string>>()) {
                    peers.emplace_back(str_to_socketaddress(item));
                }
            }
            local = str_to_socketaddress(config["local"].as<std::string>());

            return seastar::do_with(std::move(benchs), std::vector<std::chrono::microseconds::rep>(),
                    [run, &config, local, peers] (auto &benchs, auto &vec) mutable {
                return ultramarine::cluster::with_cluster(std::move(local),
                        std::move(peers), config["minimum-peers"].as<int>(), [run, &benchs, &config]() {
                            if (config["initiator"].as<bool>()) {
                                return seastar::do_for_each(benchs, [run](auto &bench) {
                                    return run_one(bench, run);
                                });
                            }
                            return seastar::sleep(std::chrono::hours(10));
                        });
            });
        });
    }
#else

    int run(int ac, char **av, benchmark_list &&benchs, int run = 1000) {
        seastar::app_template app;
        return app.run(ac, av, [benchs = std::move(benchs), run] {
            return seastar::do_with(std::move(benchs), std::vector<std::chrono::microseconds::rep>(), [run]
                    (auto &benchs, auto &vec) {
                return seastar::do_for_each(benchs, [run](auto &bench) {
                    return run_one(bench, run);
                });
            });
        });
    }

#endif
}


#ifndef STAN_MATH_PRIM_MAT_PROB_MULTINOMIAL_LOG_HPP
#define STAN_MATH_PRIM_MAT_PROB_MULTINOMIAL_LOG_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/prob/multinomial_lpmf.hpp>
#include <boost/math/tools/promotion.hpp>
#include <vector>

namespace stan {
  namespace math {

    /**
     * @deprecated use multinomial_lpmf
     */
    template <bool propto,
              typename T_prob>
    typename boost::math::tools::promote_args<T_prob>::type
    multinomial_log(const std::vector<int>& ns,
                    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta) {
      return multinomial_lpmf<propto, T_prob>(ns, theta);
    }

    /**
     * @deprecated use multinomial_lpmf
     */
    template <typename T_prob>
    typename boost::math::tools::promote_args<T_prob>::type
    multinomial_log(const std::vector<int>& ns,
                    const Eigen::Matrix<T_prob, Eigen::Dynamic, 1>& theta) {
      return multinomial_lpmf<false>(ns, theta);
    }

  }
}
#endif


/*
  Copyright 2008 Intel Corporation

  Use, modification and distribution are subject to the Boost Software License,
  Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
  http://www.boost.org/LICENSE_1_0.txt).
*/

#ifndef BOOST_POLYGON_ISOTROPY_HPP
#define BOOST_POLYGON_ISOTROPY_HPP

// external
#include <cmath>
#include <cstddef>
#include <cstdlib>
#include <deque>
#include <list>
#include <map>
#include <set>
#include <vector>
//#include <iostream>
#include <algorithm>
#include <iterator>
#include <limits>
#include <string>

#ifndef BOOST_POLYGON_NO_DEPS

#include <boost/config.hpp>
#ifdef BOOST_MSVC
#define BOOST_POLYGON_MSVC
#endif
#ifdef BOOST_INTEL
#define BOOST_POLYGON_ICC
#endif
#ifdef BOOST_HAS_LONG_LONG
#define BOOST_POLYGON_USE_LONG_LONG
typedef boost::long_long_type polygon_long_long_type;
typedef boost::ulong_long_type polygon_ulong_long_type;
// typedef long long polygon_long_long_type;
// typedef unsigned long long polygon_ulong_long_type;
#endif
#include <boost/mpl/and.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/protect.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/utility/enable_if.hpp>
#else

#ifdef _WIN32
#define BOOST_POLYGON_MSVC
#endif
#ifdef __ICC
#define BOOST_POLYGON_ICC
#endif
#define BOOST_POLYGON_USE_LONG_LONG
typedef long long polygon_long_long_type;
typedef unsigned long long polygon_ulong_long_type;

namespace boost {
template <bool B, class T = void>
struct enable_if_c {
  typedef T type;
};

template <class T>
struct enable_if_c<false, T> {};

template <class Cond, class T = void>
struct enable_if : public enable_if_c<Cond::value, T> {};

template <bool B, class T>
struct lazy_enable_if_c {
  typedef typename T::type type;
};

template <class T>
struct lazy_enable_if_c<false, T> {};

template <class Cond, class T>
struct lazy_enable_if : public lazy_enable_if_c<Cond::value, T> {};

template <bool B, class T = void>
struct disable_if_c {
  typedef T type;
};

template <class T>
struct disable_if_c<true, T> {};

template <class Cond, class T = void>
struct disable_if : public disable_if_c<Cond::value, T> {};

template <bool B, class T>
struct lazy_disable_if_c {
  typedef typename T::type type;
};

template <class T>
struct lazy_disable_if_c<true, T> {};

template <class Cond, class T>
struct lazy_disable_if : public lazy_disable_if_c<Cond::value, T> {};
}  // namespace boost

#endif

namespace boost {
namespace polygon {

struct undefined_concept {};

template <typename T>
struct geometry_concept {
  typedef undefined_concept type;
};

template <typename T>
struct coordinate_traits {};

template <>
struct coordinate_traits<int> {
  typedef int coordinate_type;
  typedef long double area_type;
#ifdef BOOST_POLYGON_USE_LONG_LONG
  typedef polygon_long_long_type manhattan_area_type;
  typedef polygon_ulong_long_type unsigned_area_type;
  typedef polygon_long_long_type coordinate_difference;
#else
  typedef long manhattan_area_type;
  typedef unsigned long unsigned_area_type;
  typedef long coordinate_difference;
#endif
  typedef long double coordinate_distance;
};

#ifdef BOOST_POLYGON_USE_LONG_LONG
template <>
struct coordinate_traits<polygon_long_long_type> {
  typedef polygon_long_long_type coordinate_type;
  typedef long double area_type;
  typedef polygon_long_long_type manhattan_area_type;
  typedef polygon_ulong_long_type unsigned_area_type;
  typedef polygon_long_long_type coordinate_difference;
  typedef long double coordinate_distance;
};
#endif

template <>
struct coordinate_traits<float> {
  typedef float coordinate_type;
  typedef float area_type;
  typedef float manhattan_area_type;
  typedef float unsigned_area_type;
  typedef float coordinate_difference;
  typedef float coordinate_distance;
};

template <>
struct coordinate_traits<double> {
  typedef double coordinate_type;
  typedef double area_type;
  typedef double manhattan_area_type;
  typedef double unsigned_area_type;
  typedef double coordinate_difference;
  typedef double coordinate_distance;
};

template <>
struct coordinate_traits<long double> {
  typedef long double coordinate_type;
  typedef long double area_type;
  typedef long double manhattan_area_type;
  typedef long double unsigned_area_type;
  typedef long double coordinate_difference;
  typedef long double coordinate_distance;
};

struct coordinate_concept {};

template <>
struct geometry_concept<int> {
  typedef coordinate_concept type;
};
#ifdef BOOST_POLYGON_USE_LONG_LONG
template <>
struct geometry_concept<polygon_long_long_type> {
  typedef coordinate_concept type;
};
#endif
template <>
struct geometry_concept<float> {
  typedef coordinate_concept type;
};
template <>
struct geometry_concept<double> {
  typedef coordinate_concept type;
};
template <>
struct geometry_concept<long double> {
  typedef coordinate_concept type;
};

#ifndef BOOST_POLYGON_NO_DEPS
struct gtl_no : mpl::bool_<false> {};
struct gtl_yes : mpl::bool_<true> {};
template <typename T, typename T2>
struct gtl_and : mpl::and_<T, T2> {};
template <typename T, typename T2, typename T3>
struct gtl_and_3 : mpl::and_<T, T2, T3> {};
template <typename T, typename T2, typename T3, typename T4>
struct gtl_and_4 : mpl::and_<T, T2, T3, T4> {};
//  template <typename T, typename T2>
//  struct gtl_or : mpl::or_<T, T2> {};
//  template <typename T, typename T2, typename T3>
//  struct gtl_or_3 : mpl::or_<T, T2, T3> {};
//  template <typename T, typename T2, typename T3, typename T4>
//  struct gtl_or_4 : mpl::or_<T, T2, T3, T4> {};
#else
struct gtl_no {
  static const bool value = false;
};
struct gtl_yes {
  typedef gtl_yes type;
  static const bool value = true;
};

template <bool T, bool T2>
struct gtl_and_c {
  typedef gtl_no type;
};
template <>
struct gtl_and_c<true, true> {
  typedef gtl_yes type;
};

template <typename T, typename T2>
struct gtl_and : gtl_and_c<T::value, T2::value> {};
template <typename T, typename T2, typename T3>
struct gtl_and_3 {
  typedef typename gtl_and<T, typename gtl_and<T2, T3>::type>::type type;
};

template <typename T, typename T2, typename T3, typename T4>
struct gtl_and_4 {
  typedef typename gtl_and_3<T, T2, typename gtl_and<T3, T4>::type>::type type;
};
#endif
template <typename T, typename T2>
struct gtl_or {
  typedef gtl_yes type;
};
template <typename T>
struct gtl_or<T, T> {
  typedef T type;
};

template <typename T, typename T2, typename T3>
struct gtl_or_3 {
  typedef typename gtl_or<T, typename gtl_or<T2, T3>::type>::type type;
};

template <typename T, typename T2, typename T3, typename T4>
struct gtl_or_4 {
  typedef typename gtl_or<T, typename gtl_or_3<T2, T3, T4>::type>::type type;
};

template <typename T>
struct gtl_not {
  typedef gtl_no type;
};
template <>
struct gtl_not<gtl_no> {
  typedef gtl_yes type;
};

template <typename T>
struct gtl_if {
#ifdef BOOST_POLYGON_MSVC
  typedef gtl_no type;
#endif
};
template <>
struct gtl_if<gtl_yes> {
  typedef gtl_yes type;
};

template <typename T, typename T2>
struct gtl_same_type {
  typedef gtl_no type;
};
template <typename T>
struct gtl_same_type<T, T> {
  typedef gtl_yes type;
};
template <typename T, typename T2>
struct gtl_different_type {
  typedef typename gtl_not<typename gtl_same_type<T, T2>::type>::type type;
};

enum direction_1d_enum {
  LOW = 0,
  HIGH = 1,
  LEFT = 0,
  RIGHT = 1,
  CLOCKWISE = 0,
  COUNTERCLOCKWISE = 1,
  REVERSE = 0,
  FORWARD = 1,
  NEGATIVE = 0,
  POSITIVE = 1
};
enum orientation_2d_enum { HORIZONTAL = 0, VERTICAL = 1 };
enum direction_2d_enum { WEST = 0, EAST = 1, SOUTH = 2, NORTH = 3 };

class direction_2d;
class orientation_2d;

class direction_1d {
 private:
  unsigned int val_;
  explicit direction_1d(int d);

 public:
  inline direction_1d() : val_(LOW) {}
  inline direction_1d(const direction_1d& that) : val_(that.val_) {}
  inline direction_1d(const direction_1d_enum val) : val_(val) {}
  explicit inline direction_1d(const direction_2d& that);
  inline direction_1d& operator=(const direction_1d& d) {
    val_ = d.val_;
    return *this;
  }
  inline bool operator==(direction_1d d) const { return (val_ == d.val_); }
  inline bool operator!=(direction_1d d) const { return !((*this) == d); }
  inline unsigned int to_int(void) const { return val_; }
  inline direction_1d& backward() {
    val_ ^= 1;
    return *this;
  }
  inline int get_sign() const { return val_ * 2 - 1; }
};

class direction_2d;

class orientation_2d {
 private:
  unsigned int val_;
  explicit inline orientation_2d(int o);

 public:
  inline orientation_2d() : val_(HORIZONTAL) {}
  inline orientation_2d(const orientation_2d& ori) : val_(ori.val_) {}
  inline orientation_2d(const orientation_2d_enum val) : val_(val) {}
  explicit inline orientation_2d(const direction_2d& that);
  inline orientation_2d& operator=(const orientation_2d& ori) {
    val_ = ori.val_;
    return *this;
  }
  inline bool operator==(orientation_2d that) const {
    return (val_ == that.val_);
  }
  inline bool operator!=(orientation_2d that) const {
    return (val_ != that.val_);
  }
  inline unsigned int to_int() const { return (val_); }
  inline void turn_90() { val_ = val_ ^ 1; }
  inline orientation_2d get_perpendicular() const {
    orientation_2d retval = *this;
    retval.turn_90();
    return retval;
  }
  inline direction_2d get_direction(direction_1d dir) const;
};

class direction_2d {
 private:
  int val_;

 public:
  inline direction_2d() : val_(WEST) {}

  inline direction_2d(const direction_2d& that) : val_(that.val_) {}

  inline direction_2d(const direction_2d_enum val) : val_(val) {}

  inline direction_2d& operator=(const direction_2d& d) {
    val_ = d.val_;
    return *this;
  }

  inline ~direction_2d() {}

  inline bool operator==(direction_2d d) const { return (val_ == d.val_); }
  inline bool operator!=(direction_2d d) const { return !((*this) == d); }
  inline bool operator<(direction_2d d) const { return (val_ < d.val_); }
  inline bool operator<=(direction_2d d) const { return (val_ <= d.val_); }
  inline bool operator>(direction_2d d) const { return (val_ > d.val_); }
  inline bool operator>=(direction_2d d) const { return (val_ >= d.val_); }

  // Casting to int
  inline unsigned int to_int(void) const { return val_; }

  inline direction_2d backward() const {
    // flip the LSB, toggles 0 - 1   and 2 - 3
    return direction_2d(direction_2d_enum(val_ ^ 1));
  }

  // Returns a direction 90 degree left (LOW) or right(HIGH) to this one
  inline direction_2d turn(direction_1d t) const {
    return direction_2d(direction_2d_enum(val_ ^ 3 ^ (val_ >> 1) ^ t.to_int()));
  }

  // Returns a direction 90 degree left to this one
  inline direction_2d left() const { return turn(HIGH); }

  // Returns a direction 90 degree right to this one
  inline direction_2d right() const { return turn(LOW); }

  // N, E are positive, S, W are negative
  inline bool is_positive() const { return (val_ & 1); }
  inline bool is_negative() const { return !is_positive(); }
  inline int get_sign() const { return ((is_positive()) << 1) - 1; }
};

direction_1d::direction_1d(const direction_2d& that)
    : val_(that.to_int() & 1) {}

orientation_2d::orientation_2d(const direction_2d& that)
    : val_(that.to_int() >> 1) {}

direction_2d orientation_2d::get_direction(direction_1d dir) const {
  return direction_2d(direction_2d_enum((val_ << 1) + dir.to_int()));
}
}  // namespace polygon
}  // namespace boost
#endif


#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <std_msgs/Bool.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

class LaserNoise
{
public:
LaserNoise()
{
        ros::NodeHandle private_nh("laser_noise");
        // Initialise the noise period...
        private_nh.param("noise_period", noise_period_, 30.0);
        // and the rectacular area of noise bounded by the rectacle of which the 4 points are the corners.
        private_nh.param("x_max", x_max_, 532.61);
        private_nh.param("x_min", x_min_, 531.10);
        private_nh.param("y_max", y_max_, 48.41);
        private_nh.param("y_min", y_min_, 41.24);

        //randomGen_.seed(time(NULL)); // seed the generator
        laser_sub_ = n_.subscribe<sensor_msgs::LaserScan>("scan", 20, &LaserNoise::laserReadCallBAck, this);
        pose_sub_ = n_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("robot_pose", 20, &LaserNoise::poseCallback, this);
        reset_node_sub_ = n_.subscribe<std_msgs::Bool>("/laser_noise_reset", 20, &LaserNoise::resetCallBack, this);
        joy_triggered_noise_sub_ = n_.subscribe<std_msgs::Bool>("/joy_triggered_noise", 20, &LaserNoise::joyNoiseCallBack, this);

        scan_with_noise_pub_ = n_.advertise<sensor_msgs::LaserScan>("scan_with_noise", 20);
        laser_noise_active_pub_ = n_.advertise<std_msgs::Bool>("laser_noise_active", 20);

        timer_noise_ = n_.createTimer(ros::Duration(noise_period_), &LaserNoise::timerNoiseCallback, this, false, false);

        area_trigger_ = 0, timer_trigger_ =0, timer_activated_= 0, joy_noise_trigger_ = 0;
        noise_scale_ = 0.3;
}

private:

//  boost::mt19937 randomGen_;

ros::NodeHandle n_;
ros::Subscriber laser_sub_, pose_sub_, reset_node_sub_, joy_triggered_noise_sub_;
ros::Publisher scan_with_noise_pub_, laser_noise_active_pub_;
ros::Timer timer_noise_;

void laserReadCallBAck(const sensor_msgs::LaserScan::ConstPtr& scan_msg);
double GaussianKernel(double mu,double sigma), uniformNoise_;
void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
void timerNoiseCallback(const ros::TimerEvent&);
void resetCallBack(const std_msgs::Bool::ConstPtr& msg);
void joyNoiseCallBack(const std_msgs::Bool::ConstPtr& msg);

bool area_trigger_, timer_trigger_, timer_activated_, joy_noise_trigger_;
double noise_period_, noise_scale_, x_max_, x_min_, y_max_, y_min_;

};

//
void LaserNoise::resetCallBack(const std_msgs::Bool::ConstPtr& msg)
{
        if (msg->data == true)
        {
                timer_trigger_ = 0;
                timer_noise_.stop(); // needed to make sure the timer is not running in background
        }

}

// reads published by laser scans and adds noise to them
void LaserNoise::laserReadCallBAck(const sensor_msgs::LaserScan::ConstPtr& scan_msg)
{
        double sigma;
        double old_range;
        std_msgs::Bool noise;
        sensor_msgs::LaserScan laser_scan = *scan_msg;

        // Guassian noise added
        if ( (area_trigger_ == 1 || joy_noise_trigger_ == 1) && timer_trigger_ == 0)
        {
                for (int i=0; i < laser_scan.ranges.size(); i++)
                {
                        sigma = laser_scan.ranges[i] * noise_scale_; // Proportional standard deviation
                        old_range = laser_scan.ranges[i];
                        laser_scan.ranges[i] = laser_scan.ranges[i] + GaussianKernel(0,sigma);

                        if (laser_scan.ranges[i] > laser_scan.range_max)
                        { laser_scan.ranges[i] = laser_scan.range_max; }

                        else if (laser_scan.ranges[i] < laser_scan.range_min)
                        { laser_scan.ranges[i] = old_range; }
                }
                noise.data = true;
                laser_noise_active_pub_.publish(noise);
        }
        else
        {
                noise.data = false;
                laser_noise_active_pub_.publish(noise);
        }
        laser_scan.header.stamp = ros::Time::now();
        scan_with_noise_pub_.publish(laser_scan);
}

// reads robot pose and if robot is inside a predifined area, activates noise trigger
void LaserNoise::poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
        // if ( (msg->pose.pose.position.x < x_max_) && (msg->pose.pose.position.x > x_min_)
        //      && (msg->pose.pose.position.y < y_max_) && (msg->pose.pose.position.y > y_min_) )
        //
        // {
        //         area_trigger_ =1;
        //         timer_noise_.start();
        // }
        // else
        // {
        //         area_trigger_ = 0;
        //         std_msgs::Bool noise;
        //         noise.data = false;
        //         laser_noise_active_pub_.publish(noise);
        //         //timer_trigger_ = 0;
        // }
}

// reads robot pose and if robot is inside a predifined area, activates noise trigger
void LaserNoise::joyNoiseCallBack(const std_msgs::Bool::ConstPtr& msg)
{
        if (msg->data == true)
        {
                joy_noise_trigger_ = 1;
                timer_noise_.start();
        }
        else
        {
                joy_noise_trigger_ = 0;
                std_msgs::Bool noise;
                noise.data = false;
                laser_noise_active_pub_.publish(noise);
        }
}

// this is the timer deciding for how long the noise will be activated
void LaserNoise::timerNoiseCallback(const ros::TimerEvent&)
{
        timer_trigger_ = 1;
        timer_noise_.stop();
        ROS_INFO("TIMER ACTIVATED");
}

// Utility function for adding Guassian noise
double LaserNoise::GaussianKernel(double mu,double sigma)
{
        // using Box-Muller transform to generate two independent standard normally disbributed normal variables

        double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
        double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable
        double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V);
        //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable
        // we'll just use X
        // scale to our mu and sigma
        X = sigma * X + mu;
        return X;
}

int main(int argc, char** argv)
{
        ros::init(argc, argv, "laser_noise");
        LaserNoise lasernoise;

        ros::spin();
}


#include "partition/graph_generator.hpp"
#include "partition/graph_view.hpp"
#include "partition/recursive_bisection_state.hpp"

#include <algorithm>
#include <vector>

#include <boost/test/test_case_template.hpp>
#include <boost/test/unit_test.hpp>

using namespace osrm::partition;
using namespace osrm::util;

BOOST_AUTO_TEST_SUITE(scc_integration)

BOOST_AUTO_TEST_CASE(graph_views_on_components)
{
    // 40 entries of left/right edges
    const double step_size = 0.01;
    const int rows = 1;
    const int cols = 10;

    const int num_components = 10;

    auto graph = [&]() {
        std::vector<Coordinate> grid_coordinates;
        std::vector<EdgeWithSomeAdditionalData> grid_edges;

        // generate 10 big components
        for (int i = 0; i < num_components; ++i)
        {
            // 10 rows of large components, interrupted by small disconnected components
            const auto coordinates = makeGridCoordinates(rows, cols, step_size, 0, i * step_size);
            grid_coordinates.insert(grid_coordinates.end(), coordinates.begin(), coordinates.end());

            // add a single disconnected node to have ids between large components
            if (i + 1 < num_components)
                grid_coordinates.push_back(
                    makeCoordinate(1, 1, 0.5 * step_size, 0, (i + 1) * step_size));

            // connect the grid edges, starting with i * (rows * cols + 1) as first id (0,11,22...)
            const auto edges = makeGridEdges(rows, cols, i * (rows * cols + 1));
            grid_edges.insert(grid_edges.end(), edges.begin(), edges.end());
        }

        groupEdgesBySource(grid_edges.begin(), grid_edges.end());
        return makeBisectionGraph(grid_coordinates, adaptToBisectionEdge(std::move(grid_edges)));
    }();

    RecursiveBisectionState bisection_state(graph);

    auto views = bisection_state.PrePartitionWithSCC(2);
    BOOST_CHECK_EQUAL(views.size(), num_components + 1); // big components + 1 small one

    for (std::size_t comp = 0; comp < num_components; ++comp)
    {
        const auto &view = views[comp];
        BOOST_CHECK(views[comp].NumberOfNodes() == 10);

        const auto to_component_id = [&](const auto &node) {
            return node.original_id / (rows * cols + 1);
        };

        std::size_t expected_component_id = to_component_id(view.Node(0));
        BOOST_CHECK(std::all_of(view.Begin(), view.End(), [&](const auto &node) {
            return to_component_id(node) == expected_component_id;
        }));

        for (const auto &node : view.Nodes())
        {
            for (const auto &edge : view.Edges(node))
            {
                BOOST_CHECK(edge.target < view.NumberOfNodes());
            }
        }
    }
    BOOST_CHECK(views.back().NumberOfNodes() == 9);
}

BOOST_AUTO_TEST_SUITE_END()


#include "LYUtils.h"
#include "common/src/alg.h"
#include <boost/algorithm/string.hpp>
#include <boost/random.hpp>

boost::uniform_int<> distribution(1, 1000000);
boost::mt19937 engine;
boost::variate_generator<boost::mt19937, boost::uniform_int<> > my_random(
		engine, distribution);

std::string LYUtils::intToHex(int i, unsigned int length) {
	std::string temp;
	std::stringstream ss;
	ss << std::hex << i;
	ss >> temp;
	if (temp.size() < length) {
		int num = length - temp.size();
		for (; num > 0; --num) {
			temp = "0" + temp;
		}
	} else if (temp.size() > length) {
		temp = temp.substr(0, length);
	}
	return temp;
}

/*
 * 仿照mongo OID算法产生一个唯一的Id, 参考这个文章：http://www.jb51.net/article/35953.htm
 * ObjectId 是一个 12 Bytes 的 类型，其包含：
 * 4 Bytes 自纪元时间开始的秒数
 * 3 Bytes 机器描述符
 * 2 Bytes 进程ID
 * 3 Bytes 随机数
 */
std::string LYUtils::genId() {
	//时间
	time_t t = time(NULL);
	std::string new_t = intToHex(t, 8);
	//机器描述符
	char host_name[100];
	gethostname(host_name, sizeof(host_name));
	std::string host(host_name);
	MD5 md5(host);
	std::string new_host = md5.md5();
	new_host = new_host.substr(0, 6);
	//进程ID
	pid_t pid = getpid();
	std::string new_pid = intToHex(pid, 4);
	//随机数
//	 srand((unsigned) time(NULL));
//	int random = rand() % 1000000;
	std::string new_rand1 = intToHex(my_random(), 6);
//	 std::string new_rand2 = intToHex(my_random(), 2);
//	 std::string new_rand3 = intToHex(my_random(), 2);

	return new_t + new_host + new_pid + new_rand1; // + new_rand2 + new_rand3;
}

void LYUtils::genDigest(std::string& msg, std::string& key,
		std::string& digest) {
	std::string mac;
	CryptoPP::HMAC<CryptoPP::MD5> hmac((byte*) key.data(), key.size());

	//get msg mac
	CryptoPP::StringSource(msg, true,
			new CryptoPP::HashFilter(hmac, new CryptoPP::StringSink(mac)));
	CryptoPP::StringSource(mac, true,
			new CryptoPP::HexEncoder(new CryptoPP::StringSink(digest)));
	boost::to_upper(digest);

}
void LYUtils::printTextOrBinary(const std::string& tag,
		const std::string& str) {
	int size = str.size();
	bool is_text = true;

	int char_nbr;
	unsigned char byte;
	for (char_nbr = 0; char_nbr < size; char_nbr++) {
		byte = str[char_nbr];
		if (byte < 32 || byte > 127)
			is_text = false;
	}

//	LY_LOG_DBG(tag << ":follow this log");
	std::stringstream ss;
//	        std::cout << tag <<":";

	for (char_nbr = 0; char_nbr < size; char_nbr++) {
		if (is_text) {
//	                std::cout << (char)str [char_nbr];
			ss << (char) str[char_nbr];
		} else {
//	                std::cout << std::setfill('0') << std::setw(2)
//	                   << std::hex << (unsigned int) str [char_nbr];
			ss << std::setfill('0') << std::setw(2) << std::hex
					<< (unsigned int) str[char_nbr];
		}
	}
	ss << std::endl;
//	LY_LOG_DBG(ss);
}


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <core/util/test_macros.hpp>
#include <string>
#include <random>
#include <set>
#include <sstream>
#include <vector>
#include <array>
#include <algorithm>
#include <core/util/cityhash_tc.hpp>

// Eigen
#include <Eigen/Core>
#include <Eigen/SparseCore>

// SFrame and Flex type
#include <model_server/lib/flex_dict_view.hpp>
#include <core/random/random.hpp>

// ML-Data Utils
#include <toolkits/ml_data_2/ml_data.hpp>
#include <toolkits/ml_data_2/ml_data_entry.hpp>
#include <toolkits/ml_data_2/metadata.hpp>
#include <toolkits/ml_data_2/ml_data_iterators.hpp>
#include <toolkits/ml_data_2/sframe_index_mapping.hpp>

// Testing utils common to all of ml_data_iterator
#include <core/storage/sframe_data/testing_utils.hpp>
#include <core/util/testing_utils.hpp>
#include <toolkits/ml_data_2/testing_utils.hpp>

// Nearest neighbors
#include <toolkits/nearest_neighbors/nearest_neighbors.hpp>
#include <toolkits/nearest_neighbors/ball_tree_neighbors.hpp>
#include <toolkits/nearest_neighbors/brute_force_neighbors.hpp>
#include <toolkits/nearest_neighbors/lsh_neighbors.hpp>

using namespace turi;

struct test_nearest_neighbors_utils  {

  public:

  void test_upper_triangle_indices() {
    std::cout << std::endl;

    // Typical usage
    std::pair<size_t, size_t> matrix_indices;

    matrix_indices = nearest_neighbors::upper_triangular_indices(7, 5);
    TS_ASSERT_EQUALS(matrix_indices.first, 1);
    TS_ASSERT_EQUALS(matrix_indices.second, 3);

    matrix_indices = nearest_neighbors::upper_triangular_indices(0, 5);
    TS_ASSERT_EQUALS(matrix_indices.first, 0);
    TS_ASSERT_EQUALS(matrix_indices.second, 0);

    matrix_indices = nearest_neighbors::upper_triangular_indices(14, 5);
    TS_ASSERT_EQUALS(matrix_indices.first, 4);
    TS_ASSERT_EQUALS(matrix_indices.second, 4);

#if DEBUG
    // Index out of bounds errors
    TS_ASSERT_THROWS_ANYTHING(nearest_neighbors::upper_triangular_indices(0, 0));
    TS_ASSERT_THROWS_ANYTHING(nearest_neighbors::upper_triangular_indices(100, 5));
#endif
  }

  void test_distance_name_extraction() {

    auto distance_fn = function_closure_info();
    distance_fn.native_fn_name = "_distances.fossa_distance";
    std::string dist_name = nearest_neighbors::extract_distance_function_name(distance_fn);

    std::string ans = "fossa_distance";
    ASSERT_EQ(dist_name, ans);
  }

  void test_block_number_calculation() {

    // Arguments to 'calculate_num_blocks':
    // 1. num_ref_examples
    // 2. num_query_examples
    // 3. dimension
    // 4. max_thread_memory
    // 5. min_ref_blocks
    // 6. min_query_blocks
    //
    // 'calculate_num_blocks' returns a pair of size_t values: ref blocks and
    // query blocks.

    size_t max_thread_mem = 1024 * 1024 * 1024; // 1GB

    // Small data, 1 query, no min blocks
    std::pair<size_t, size_t> num_blocks = nearest_neighbors::calculate_num_blocks(731, 1, 1000, max_thread_mem, 1, 1);
    TS_ASSERT_EQUALS(num_blocks.first , 1);
    TS_ASSERT_EQUALS(num_blocks.second , 1);

    // Small data, 1 query, min ref blocks
    num_blocks = nearest_neighbors::calculate_num_blocks(731, 1, 1000, max_thread_mem, 4, 1);
    TS_ASSERT_EQUALS(num_blocks.first, 4);
    TS_ASSERT_EQUALS(num_blocks.second , 1);

    // More blocks than min ref blocks
    num_blocks = nearest_neighbors::calculate_num_blocks(10000, 1, 5, 1024 * 128, 8, 1);
    TS_ASSERT_EQUALS(num_blocks.first, 82);
  }

  void test_all_pairs_squared_euclidean() {
    nearest_neighbors::DenseMatrix A(4, 2);
    nearest_neighbors::DenseMatrix B(3, 2);

    A << 1, 1,
         4, 4,
         5, 5,
         2, 2;

    B << 1, 2,
         4, 4,
         3, 5;

    nearest_neighbors::DenseMatrix dists(4, 3);
    nearest_neighbors::all_pairs_squared_euclidean(A, B, dists);

    nearest_neighbors::DenseMatrix ans(4, 3);
    ans << 1, 18, 20,
           13, 0, 2,
           25, 2, 4,
           1, 8, 10;

    ASSERT_EQ(dists, ans);
  }
};

struct test_similarity_graph  {

public:

  void run_sim_graph_test(const std::string& model,
                          const std::string& run_string,
                          const std::string& distance) {

    global_logger().set_log_level(LOG_ERROR);
    random::seed(0);

    // Create random data.
    size_t n = 5;
    sframe data = make_random_sframe(n, run_string, false);
    std::vector<std::vector<flexible_type>> label_vec(n);

    for (size_t i = 0; i < n; ++i) {
      label_vec[i] = {std::to_string(hash64(i))};
    }
    sframe labels = make_testing_sframe({"label"}, label_vec);

    // Initialize the model, composite distance, and model options.
    auto fn = function_closure_info();
    fn.native_fn_name = "_distances.";
    fn.native_fn_name += distance;
    nearest_neighbors::dist_component_type p = std::make_tuple(data.column_names(), fn, 1.0);
    std::vector<nearest_neighbors::dist_component_type> composite_params = {p};

    std::map<std::string, flexible_type> nn_options;

    if (model == "lsh") {
      nn_options["num_tables"] = 4;
      nn_options["num_projections_per_table"] = 4;
    }

    std::shared_ptr<nearest_neighbors::nearest_neighbors_model> m;

    if (model == "brute_force") {
      m.reset(new nearest_neighbors::brute_force_neighbors);
    } else if (model == "ball_tree") {
      m.reset(new nearest_neighbors::ball_tree_neighbors);
    } else if (model == "lsh") {
      m.reset(new nearest_neighbors::lsh_neighbors);
    }

    // Train the model, compute the similarity graph, and check both
    // plausibility and equality with query results.
    m->train(data, labels, composite_params, nn_options);

    size_t k = 2;

    // include_self_edges = fales
    sframe sim_graph = m->similarity_graph(k, -1, false);
    sframe knn = m->query(data, labels, k + 1, -1);

    TS_ASSERT_EQUALS(sim_graph.num_columns(), 4);
    if (model != "lsh") {
      TS_ASSERT_EQUALS(sim_graph.num_rows(), 10);
    }

    auto sim_graph_vec = testing_extract_sframe_data(sim_graph);
    auto temp_knn_vec = testing_extract_sframe_data(knn);

    // Remove the self-edges from the query output.
    std::vector<std::vector<flexible_type> > knn_vec;

    for (size_t i = 0; i < temp_knn_vec.size(); ++i) {
      if (temp_knn_vec[i][3] != 1) {
        knn_vec.push_back(temp_knn_vec[i]);
      }
    }

    std::sort(knn_vec.begin(), knn_vec.end(),
              [&](const std::vector<flexible_type>& a, const std::vector<flexible_type>& b) {
                return std::tie(a[0], a[2], a[3]) < std::tie(b[0], b[2], b[3]);
              });
    std::sort(sim_graph_vec.begin(), sim_graph_vec.end(),
              [&](const std::vector<flexible_type>& a, const std::vector<flexible_type>& b) {
                return std::tie(a[0], a[2], a[3]) < std::tie(b[0], b[2], b[3]);
              });
    TS_ASSERT_EQUALS(knn_vec.size(), sim_graph_vec.size());

    // Check for equality
    for (size_t i = 0; i < sim_graph_vec.size(); ++i) {
      TS_ASSERT_EQUALS(sim_graph_vec[i][0], knn_vec[i][0]);       // query label
      // TS_ASSERT_EQUALS(sim_graph_vec[i][1], knn_vec[i][1]);       // ref label not always equal because of ties
      TS_ASSERT_DELTA(sim_graph_vec[i][2], knn_vec[i][2], 1e-8);  // distance
      TS_ASSERT_EQUALS(sim_graph_vec[i][3], knn_vec[i][3] - 1);   // rank
    }
  }

  // Test various distances.
  void test_brute_force_dist1() {
    run_sim_graph_test("brute_force", "nnn", "euclidean");
  }

  void test_brute_force_dist2() {
    run_sim_graph_test("brute_force", "nnn", "squared_euclidean");
  }

  void test_brute_force_dist3() {
    run_sim_graph_test("brute_force", "nnn", "manhattan");
  }

  void test_brute_force_dist4() {
    run_sim_graph_test("brute_force", "nnn", "cosine");
  }

  // Test various data types.
  void test_brute_force_data1() {
    run_sim_graph_test("brute_force", "V", "euclidean");  // 1000 numeric features
  }

  void test_brute_force_data2() {
    run_sim_graph_test("brute_force", "z", "euclidean");  // categorical
  }

  void test_brute_force_data3() {
    run_sim_graph_test("brute_force", "d", "euclidean");  // dictionary
  }

  void test_ball_tree_dist1() {
    run_sim_graph_test("ball_tree", "nnn", "euclidean");
  }

  void test_ball_tree_dist2() {
    run_sim_graph_test("ball_tree", "nnn", "squared_euclidean");
  }

  void test_ball_tree_dist3() {
    run_sim_graph_test("ball_tree", "nnn", "manhattan");
  }

  void test_ball_tree_dist4() {
    run_sim_graph_test("ball_tree", "nnn", "cosine");
  }

  // Test various data types.
  void test_ball_tree_data1() {
    run_sim_graph_test("ball_tree", "V", "euclidean");  // 1000 numeric features
  }

  void test_ball_tree_data2() {
    run_sim_graph_test("ball_tree", "z", "euclidean");  // categorical
  }

  void test_ball_tree_data3() {
    run_sim_graph_test("ball_tree", "d", "euclidean");  // dictionary
  }

  void test_lsh_dist1() {
    run_sim_graph_test("lsh", "nnn", "euclidean");
  }

  void test_lsh_dist2() {
    run_sim_graph_test("lsh", "nnn", "squared_euclidean");
  }

  void test_lsh_dist3() {
    run_sim_graph_test("lsh", "nnn", "manhattan");
  }

  void test_lsh_dist4() {
    run_sim_graph_test("lsh", "nnn", "cosine");
  }

  // Test various data types.
  void test_lsh_data1() {
    run_sim_graph_test("lsh", "V", "euclidean");  // 1000 numeric features
  }

  void test_lsh_data2() {
    run_sim_graph_test("lsh", "z", "euclidean");  // categorical
  }

  void test_lsh_data3() {
    run_sim_graph_test("lsh", "d", "euclidean");  // dictionary
  }
};

struct test_nn_consistency  {

 public:

  void run_nn_test(const std::string& model,
                   size_t n,
                   const std::string& run_string,
                   const std::string& distance) {

    global_logger().set_log_level(LOG_ERROR);

    random::seed(0);

    sframe data[] = {make_random_sframe(n, run_string, false),
                     make_random_sframe(5, run_string, false),
                     make_random_sframe(2, run_string, false)};

    sframe y[3];

    for(size_t i = 0; i < 3; ++i) {
      std::vector<std::vector<flexible_type> > labels(data[i].size());

      for(size_t j = 0; j < data[i].size(); ++j) {
        labels[j] = {std::to_string(hash64(i, j))};
      }
      y[i] = make_testing_sframe({"label"}, labels);
    }

    std::shared_ptr<nearest_neighbors::nearest_neighbors_model> nn, nn_sl_1, nn_sl_2;

    // Construct the model
    if(model == "ball_tree") {
      nn.reset(new nearest_neighbors::ball_tree_neighbors);
      nn_sl_1.reset(new nearest_neighbors::ball_tree_neighbors);
      nn_sl_2.reset(new nearest_neighbors::ball_tree_neighbors);
    } else if (model == "brute_force") {
      nn.reset(new nearest_neighbors::brute_force_neighbors);
      nn_sl_1.reset(new nearest_neighbors::brute_force_neighbors);
      nn_sl_2.reset(new nearest_neighbors::brute_force_neighbors);
    } else if (model == "lsh") {
      nn.reset(new nearest_neighbors::lsh_neighbors);
      nn_sl_1.reset(new nearest_neighbors::lsh_neighbors);
      nn_sl_2.reset(new nearest_neighbors::lsh_neighbors);
    }

    // Temp: Need to construct a set of composite params
    auto fn = function_closure_info();
    fn.native_fn_name = "_distances.";
    fn.native_fn_name += distance;
    nearest_neighbors::dist_component_type p = std::make_tuple(data[0].column_names(), fn, 1.0);
    std::vector<nearest_neighbors::dist_component_type> composite_params = {p};

    std::map<std::string, flexible_type> nn_options;

    if (model == "lsh") {
      nn_options["num_tables"] = 4;
      nn_options["num_projections_per_table"] = 4;
    }

    nn->train(data[0], y[0], composite_params, nn_options);

    save_and_load_object(*nn_sl_1, *nn);

    size_t q_idx_v[] = {1, 2};
    size_t k_v[] = {1, 2, nearest_neighbors::NONE_FLAG};
    double radius_v[] = {0.1, 1, 5};

    parallel_for(size_t(0), size_t(2 * 3 * 3), [&](size_t main_idx) {

        size_t q_idx  = q_idx_v[main_idx / (3 * 3)];
        size_t k      = k_v[(main_idx / 3) % 3];
        double radius = radius_v[main_idx % 3];

        std::vector<std::vector<flexible_type> > result_1
            = testing_extract_sframe_data(nn->query(data[q_idx], y[q_idx], k, radius));

        std::vector<std::vector<flexible_type> > result_2
            = testing_extract_sframe_data(nn_sl_1->query(data[q_idx], y[q_idx], k, radius));

        ASSERT_TRUE(result_1 == result_2);
      });

    save_and_load_object(*nn_sl_2, *nn);

    parallel_for(size_t(0), size_t(2 * 3 * 3), [&](size_t main_idx) {

        size_t q_idx = q_idx_v[main_idx / (3 * 3)];
        size_t k = (main_idx / 3) % 3;
        double radius = main_idx % 3;

        std::vector<std::vector<flexible_type> > result_1
            = testing_extract_sframe_data(nn->query(data[q_idx], y[q_idx], k, radius));

        std::vector<std::vector<flexible_type> > result_2
            = testing_extract_sframe_data(nn_sl_2->query(data[q_idx], y[q_idx], k, radius));

        ASSERT_TRUE(result_1 == result_2);
      });
  }

  void test_ball_tree_n_1() {
    run_nn_test("ball_tree", 30, "n", "euclidean");
  }

  void test_ball_tree_n_2() {
    run_nn_test("ball_tree", 30, "n", "manhattan");
  }

  void test_ball_tree_nnnnnn_1() {
    run_nn_test("ball_tree", 30, "nnnnnn", "euclidean");
  }

  void test_ball_tree_nnnnnn_2() {
    run_nn_test("ball_tree", 30, "nnnnnn", "manhattan");
  }

  void test_ball_tree_nd_1() {
    run_nn_test("ball_tree", 30, "nd", "euclidean");
  }

  void test_ball_tree_nd_2() {
    run_nn_test("ball_tree", 30, "nd", "manhattan");
  }

  void test_lsh_euclidean_1() {
    run_nn_test("lsh", 100, "V", "euclidean");
  }

  void test_lsh_euclidean_2() {
    run_nn_test("lsh", 100, "d", "euclidean");
  }

  void test_lsh_squared_euclidean_1() {
    run_nn_test("lsh", 100, "V", "squared_euclidean");
  }

  void test_lsh_squared_euclidean_2() {
    run_nn_test("lsh", 100, "d", "squared_euclidean");
  }

  void test_lsh_manhattan_1() {
    run_nn_test("lsh", 100, "V", "manhattan");
  }

  void test_lsh_manhattan_2() {
    run_nn_test("lsh", 100, "d", "manhattan");
  }

  void test_lsh_cosine_1() {
    run_nn_test("lsh", 100, "V", "cosine");
  }

  void test_lsh_cosine_2() {
    run_nn_test("lsh", 100, "d", "cosine");
  }

  void test_lsh_jaccard_1() {
    run_nn_test("lsh", 100, "D", "jaccard");
  }

  void test_lsh_jaccard_2() {
    run_nn_test("lsh", 1000, "D", "jaccard");
  }

  void test_lsh_dot_product_1() {
    run_nn_test("lsh", 100, "D", "dot_product");
  }

  void test_lsh_dot_product_2() {
    run_nn_test("lsh", 100, "V", "dot_product");
  }

  void test_lsh_transformed_dot_product_1() {
    run_nn_test("lsh", 100, "D", "transformed_dot_product");
  }

  void test_lsh_transformed_dot_product_2() {
    run_nn_test("lsh", 100, "V", "transformed_dot_product");
  }

  void test_ball_tree_n_1_large() {
    run_nn_test("ball_tree", 100, "n", "euclidean");
  }

  void test_ball_tree_n_2_large() {
    run_nn_test("ball_tree", 100, "n", "manhattan");
  }

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

  void test_brute_force_n_1() {
    run_nn_test("brute_force", 30, "n", "euclidean");
  }

  void test_brute_force_n_2() {
    run_nn_test("brute_force", 30, "n", "manhattan");
  }

  // void test_brute_force_n_3() {
  //   run_nn_test("brute_force", 30, "n", "jaccard");
  // }
  //
  // void test_brute_force_n_4() {
  //   run_nn_test("brute_force", 30, "n", "weighted_jaccard");
  // }
  //
  void test_brute_force_n_5() {
    run_nn_test("brute_force", 30, "n", "cosine");
  }

  // void test_brute_force_n_6() {
  //   run_nn_test("brute_force", 30, "n", "auto");
  // }

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

  void test_brute_force_d_1() {
    run_nn_test("brute_force", 30, "nd", "euclidean");
  }

  void test_brute_force_d_2() {
    run_nn_test("brute_force", 30, "nd", "manhattan");
  }

  // void test_brute_force_d_3() {
  //   run_nn_test("brute_force", 30, "nd", "jaccard");
  // }
  //
  // void test_brute_force_d_4() {
  //   run_nn_test("brute_force", 30, "nd", "weighted_jaccard");
  // }

  void test_brute_force_d_5() {
    run_nn_test("brute_force", 30, "nd", "cosine");
  }

  // void test_brute_force_d_6() {
  //   run_nn_test("brute_force", 30, "nd", "auto");
  // }

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

  void test_brute_force_nnnnnn_1() {
    run_nn_test("brute_force", 30, "nnnnnn", "euclidean");
  }

  void test_brute_force_nnnnnn_2() {
    run_nn_test("brute_force", 30, "nnnnnn", "manhattan");
  }

  // void test_brute_force_nnnnnn_3() {
  //   run_nn_test("brute_force", 30, "nnnnnn", "jaccard");
  // }
  //
  // void test_brute_force_nnnnnn_4() {
  //   run_nn_test("brute_force", 30, "nnnnnn", "weighted_jaccard");
  // }
  //
  void test_brute_force_nnnnnn_5() {
    run_nn_test("brute_force", 30, "nnnnnn", "cosine");
  }

  // void test_brute_force_nnnnnn_6() {
  //   run_nn_test("brute_force", 30, "nnnnnn", "auto");
  // }

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

  // void test_brute_force_ndvus_1() {
  //   run_nn_test("brute_force", 30, "ndvus", "euclidean");
  // }
  //
  // void test_brute_force_ndvus_2() {
  //   run_nn_test("brute_force", 30, "ndvus", "manhattan");
  // }
  //
  // void test_brute_force_ndvus_3() {
  //   run_nn_test("brute_force", 30, "ndvus", "jaccard");
  // }
  //
  // void test_brute_force_ndvus_4() {
  //   run_nn_test("brute_force", 30, "ndvus", "weighted_jaccard");
  // }
  //
  // void test_brute_force_ndvus_5() {
  //   run_nn_test("brute_force", 30, "ndvus", "cosine");
  // }
  //
  // void test_brute_force_ndvus_6() {
  //   run_nn_test("brute_force", 30, "ndvus", "auto");
  // }

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

  void test_brute_force_n_1_large() {
    run_nn_test("brute_force", 100, "n", "euclidean");
  }

  void test_brute_force_n_2_large() {
    run_nn_test("brute_force", 100, "n", "manhattan");
  }

  // void test_brute_force_n_3_large() {
  //   run_nn_test("brute_force", 100, "n", "jaccard");
  // }
  //
  // void test_brute_force_n_4_large() {
  //   run_nn_test("brute_force", 100, "n", "weighted_jaccard");
  // }
  //
  void test_brute_force_n_5_large() {
    run_nn_test("brute_force", 100, "n", "cosine");
  }

  // void test_brute_force_n_6_large() {
  //   run_nn_test("brute_force", 100, "n", "auto");
  // }

};

BOOST_FIXTURE_TEST_SUITE(_test_nearest_neighbors_utils, test_nearest_neighbors_utils)
BOOST_AUTO_TEST_CASE(test_upper_triangle_indices) {
  test_nearest_neighbors_utils::test_upper_triangle_indices();
}
BOOST_AUTO_TEST_CASE(test_distance_name_extraction) {
  test_nearest_neighbors_utils::test_distance_name_extraction();
}
BOOST_AUTO_TEST_CASE(test_block_number_calculation) {
  test_nearest_neighbors_utils::test_block_number_calculation();
}
BOOST_AUTO_TEST_CASE(test_all_pairs_squared_euclidean) {
  test_nearest_neighbors_utils::test_all_pairs_squared_euclidean();
}
BOOST_AUTO_TEST_SUITE_END()
BOOST_FIXTURE_TEST_SUITE(_test_similarity_graph, test_similarity_graph)
BOOST_AUTO_TEST_CASE(test_brute_force_dist1) {
  test_similarity_graph::test_brute_force_dist1();
}
BOOST_AUTO_TEST_CASE(test_brute_force_dist2) {
  test_similarity_graph::test_brute_force_dist2();
}
BOOST_AUTO_TEST_CASE(test_brute_force_dist3) {
  test_similarity_graph::test_brute_force_dist3();
}
BOOST_AUTO_TEST_CASE(test_brute_force_dist4) {
  test_similarity_graph::test_brute_force_dist4();
}
BOOST_AUTO_TEST_CASE(test_brute_force_data1) {
  test_similarity_graph::test_brute_force_data1();
}
BOOST_AUTO_TEST_CASE(test_brute_force_data2) {
  test_similarity_graph::test_brute_force_data2();
}
BOOST_AUTO_TEST_CASE(test_brute_force_data3) {
  test_similarity_graph::test_brute_force_data3();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_dist1) {
  test_similarity_graph::test_ball_tree_dist1();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_dist2) {
  test_similarity_graph::test_ball_tree_dist2();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_dist3) {
  test_similarity_graph::test_ball_tree_dist3();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_dist4) {
  test_similarity_graph::test_ball_tree_dist4();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_data1) {
  test_similarity_graph::test_ball_tree_data1();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_data2) {
  test_similarity_graph::test_ball_tree_data2();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_data3) {
  test_similarity_graph::test_ball_tree_data3();
}
BOOST_AUTO_TEST_CASE(test_lsh_dist1) {
  test_similarity_graph::test_lsh_dist1();
}
BOOST_AUTO_TEST_CASE(test_lsh_dist2) {
  test_similarity_graph::test_lsh_dist2();
}
BOOST_AUTO_TEST_CASE(test_lsh_dist3) {
  test_similarity_graph::test_lsh_dist3();
}
BOOST_AUTO_TEST_CASE(test_lsh_dist4) {
  test_similarity_graph::test_lsh_dist4();
}
BOOST_AUTO_TEST_CASE(test_lsh_data1) {
  test_similarity_graph::test_lsh_data1();
}
BOOST_AUTO_TEST_CASE(test_lsh_data2) {
  test_similarity_graph::test_lsh_data2();
}
BOOST_AUTO_TEST_CASE(test_lsh_data3) {
  test_similarity_graph::test_lsh_data3();
}
BOOST_AUTO_TEST_SUITE_END()
BOOST_FIXTURE_TEST_SUITE(_test_nn_consistency, test_nn_consistency)
BOOST_AUTO_TEST_CASE(test_ball_tree_n_1) {
  test_nn_consistency::test_ball_tree_n_1();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_n_2) {
  test_nn_consistency::test_ball_tree_n_2();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_nnnnnn_1) {
  test_nn_consistency::test_ball_tree_nnnnnn_1();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_nnnnnn_2) {
  test_nn_consistency::test_ball_tree_nnnnnn_2();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_nd_1) {
  test_nn_consistency::test_ball_tree_nd_1();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_nd_2) {
  test_nn_consistency::test_ball_tree_nd_2();
}
BOOST_AUTO_TEST_CASE(test_lsh_euclidean_1) {
  test_nn_consistency::test_lsh_euclidean_1();
}
BOOST_AUTO_TEST_CASE(test_lsh_euclidean_2) {
  test_nn_consistency::test_lsh_euclidean_2();
}
BOOST_AUTO_TEST_CASE(test_lsh_squared_euclidean_1) {
  test_nn_consistency::test_lsh_squared_euclidean_1();
}
BOOST_AUTO_TEST_CASE(test_lsh_squared_euclidean_2) {
  test_nn_consistency::test_lsh_squared_euclidean_2();
}
BOOST_AUTO_TEST_CASE(test_lsh_manhattan_1) {
  test_nn_consistency::test_lsh_manhattan_1();
}
BOOST_AUTO_TEST_CASE(test_lsh_manhattan_2) {
  test_nn_consistency::test_lsh_manhattan_2();
}
BOOST_AUTO_TEST_CASE(test_lsh_cosine_1) {
  test_nn_consistency::test_lsh_cosine_1();
}
BOOST_AUTO_TEST_CASE(test_lsh_cosine_2) {
  test_nn_consistency::test_lsh_cosine_2();
}
BOOST_AUTO_TEST_CASE(test_lsh_jaccard_1) {
  test_nn_consistency::test_lsh_jaccard_1();
}
BOOST_AUTO_TEST_CASE(test_lsh_jaccard_2) {
  test_nn_consistency::test_lsh_jaccard_2();
}
BOOST_AUTO_TEST_CASE(test_lsh_dot_product_1) {
  test_nn_consistency::test_lsh_dot_product_1();
}
BOOST_AUTO_TEST_CASE(test_lsh_dot_product_2) {
  test_nn_consistency::test_lsh_dot_product_2();
}
BOOST_AUTO_TEST_CASE(test_lsh_transformed_dot_product_1) {
  test_nn_consistency::test_lsh_transformed_dot_product_1();
}
BOOST_AUTO_TEST_CASE(test_lsh_transformed_dot_product_2) {
  test_nn_consistency::test_lsh_transformed_dot_product_2();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_n_1_large) {
  test_nn_consistency::test_ball_tree_n_1_large();
}
BOOST_AUTO_TEST_CASE(test_ball_tree_n_2_large) {
  test_nn_consistency::test_ball_tree_n_2_large();
}
BOOST_AUTO_TEST_CASE(test_brute_force_n_1) {
  test_nn_consistency::test_brute_force_n_1();
}
BOOST_AUTO_TEST_CASE(test_brute_force_n_2) {
  test_nn_consistency::test_brute_force_n_2();
}
BOOST_AUTO_TEST_CASE(test_brute_force_n_5) {
  test_nn_consistency::test_brute_force_n_5();
}
BOOST_AUTO_TEST_CASE(test_brute_force_d_1) {
  test_nn_consistency::test_brute_force_d_1();
}
BOOST_AUTO_TEST_CASE(test_brute_force_d_2) {
  test_nn_consistency::test_brute_force_d_2();
}
BOOST_AUTO_TEST_CASE(test_brute_force_d_5) {
  test_nn_consistency::test_brute_force_d_5();
}
BOOST_AUTO_TEST_CASE(test_brute_force_nnnnnn_1) {
  test_nn_consistency::test_brute_force_nnnnnn_1();
}
BOOST_AUTO_TEST_CASE(test_brute_force_nnnnnn_2) {
  test_nn_consistency::test_brute_force_nnnnnn_2();
}
BOOST_AUTO_TEST_CASE(test_brute_force_nnnnnn_5) {
  test_nn_consistency::test_brute_force_nnnnnn_5();
}
BOOST_AUTO_TEST_CASE(test_brute_force_n_1_large) {
  test_nn_consistency::test_brute_force_n_1_large();
}
BOOST_AUTO_TEST_CASE(test_brute_force_n_2_large) {
  test_nn_consistency::test_brute_force_n_2_large();
}
BOOST_AUTO_TEST_CASE(test_brute_force_n_5_large) {
  test_nn_consistency::test_brute_force_n_5_large();
}
BOOST_AUTO_TEST_SUITE_END()


#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#include "DatasetAR.h"
#include "DatasetARX.h"
#include "GaussSeq.h"
#include "ARSeq.h"
#include "utils.h"

using utils::my_float;

int main() {
    using boost::multiprecision::cpp_bin_float_50;
    using boost::random::uniform_real_distribution;

    // Dataset write
    DatasetARX dat_1("testing_one_background", 1);
    DatasetARX dat_2("testing_two_background", 2);
    DatasetARX dat_2_2("testing_two_background_2", 2);
    DatasetARX dat_3("testing_three_background", 3);
    DatasetARX dat_3_2("testing_three_background_2", 3);
    DatasetARX dat_4("testing_four_background", 4);
    DatasetARX dat_4_2("testing_four_background_2", 4);

    dat_1.write_csv();
    dat_2.write_csv();
    dat_2_2.write_csv();
    dat_3.write_csv();
    dat_3_2.write_csv();
    dat_4.write_csv();
    dat_4_2.write_csv();

    return 0;
}


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

#pragma once

#include "config.hpp"
#include "errors.hpp"
#include <boost/align/aligned_allocator.hpp>
#include <mkl.h>
// #include <sleef.h>
#include <torch/extension.h>
#include <immintrin.h>

#if defined(TCM_GCC)
#    pragma GCC diagnostic push
// #    pragma GCC diagnostic ignored "-Wsign-conversion"
// #    pragma GCC diagnostic ignored "-Wsign-promo"
// #    pragma GCC diagnostic ignored "-Wswitch-default"
// #    pragma GCC diagnostic ignored "-Wctor-dtor-privacy"
// #    pragma GCC diagnostic ignored "-Wstrict-overflow"
#    pragma GCC diagnostic ignored "-Wold-style-cast"
#    pragma GCC diagnostic ignored "-Wshadow"
#elif defined(TCM_CLANG)
#    pragma clang diagnostic push
#    pragma clang diagnostic ignored "-Wmissing-noreturn"
#    pragma clang diagnostic ignored "-Wsign-conversion"
#    pragma clang diagnostic ignored "-Wswitch-enum"
#    pragma clang diagnostic ignored "-Wundefined-func-template"
#endif
#include <pybind11/pybind11.h>
#if defined(TCM_GCC)
#    pragma GCC diagnostic pop
#elif defined(TCM_CLANG)
#    pragma clang diagnostic pop
#endif

TCM_NAMESPACE_BEGIN

// ----------------------------- [Activations] ----------------------------- {{{
/// ReLU activation function
struct ReLU {
    auto operator()(float const x) const noexcept -> float
    {
        return std::max(x, 0.0f);
    }

    auto operator()(__m128 const x) const noexcept -> __m128
    {
        return _mm_max_ps(x, _mm_set1_ps(0.0f));
    }

    auto operator()(__m256 const x) const noexcept -> __m256
    {
        return _mm256_max_ps(x, _mm256_set1_ps(0.0f));
    }

    auto operator()(torch::Tensor const& input) const -> torch::Tensor
    {
        return torch::relu(input);
    }
};

/// Softplus activation function
struct Softplus {
    auto operator()(float const x) const noexcept -> float
    {
        return std::log(1.0f + std::exp(x));
    }

    auto operator()(torch::Tensor const& input) const -> torch::Tensor
    {
        return torch::softplus(input);
    }
};

/// Tanh activation function
struct Tanh {
    auto operator()(float const x) const noexcept -> float
    {
        // return Sleef_tanhf_u10(x);
        return std::tanh(x);
    }

#if 0
    auto operator()(__m128 const x) const noexcept -> __m128
    {
        return Sleef_tanhf4_u10sse4(x);
    }

    auto operator()(__m256 const x) const noexcept -> __m256
    {
        return Sleef_tanhf8_u10avx(x);
    }
#endif

    auto operator()(torch::Tensor const& input) const -> torch::Tensor
    {
        return torch::tanh(input);
    }
};

/// Identity activation function
struct Identity {
    template <class T> constexpr decltype(auto) operator()(T&& x) const noexcept
    {
        return std::forward<T>(x);
    }
};
// ----------------------------- [Activations] ----------------------------- }}}

namespace detail {
struct Gemm {
    sgemm_jit_kernel_t func;
    void*              jitter;

    auto operator()(float const* a, float const* b, float* c) const noexcept
        -> void
    {
        (*func)(jitter, const_cast<float*>(a), const_cast<float*>(b), c);
    }
};

template <class Activation>
auto bias_and_activation(size_t batch_size, size_t out_features, float* out,
                         size_t ldim, float const* bias, Activation&& func)
    -> void;
} // namespace detail

// ------------------------- [DisableConversions] -------------------------- {{{
class DisableConversions : public torch::nn::Module {
  public:
    using torch::nn::Module::Module;

    DisableConversions(DisableConversions const&)     = default;
    DisableConversions(DisableConversions&&) noexcept = default;
    DisableConversions& operator=(DisableConversions const&) = default;
    DisableConversions& operator=(DisableConversions&&) noexcept = default;

    // to() functions just throw. We override them to "fail fast".
    virtual void to(torch::Device, torch::Dtype,
                    bool non_blocking = false) override;
    virtual void to(torch::Dtype, bool non_blocking = false) override;
    virtual void to(torch::Device, bool non_blocking = false) override;
};
// ------------------------- [DisableConversions] -------------------------- }}}

// --------------------------- [DenseLayerImpl] ---------------------------- {{{
class DenseLayerImpl : public DisableConversions {
  public:
    using buffer_type =
        std::vector<float, boost::alignment::aligned_allocator<float, 64>>;

  protected:
    buffer_type                   _weight;
    buffer_type                   _bias;
    buffer_type                   _dst;
    int64_t                       _in_features;
    int64_t                       _out_features;
    int64_t                       _batch_size;
    bool                          _with_bias;
    std::shared_ptr<detail::Gemm> _gemm;
    torch::Tensor                 _weight_tensor;
    torch::Tensor                 _bias_tensor;
    torch::Tensor                 _dst_tensor;

  private:
    auto init_buffers() -> void;
    auto init_parameters() -> void;
    auto init_gemm() -> void;

  public:
    DenseLayerImpl(size_t in_features, size_t out_features, size_t batch_size,
                   bool with_bias = true);

    DenseLayerImpl(DenseLayerImpl const& other);
    DenseLayerImpl(DenseLayerImpl&&) = delete;
    DenseLayerImpl& operator=(DenseLayerImpl const&) = delete;
    DenseLayerImpl& operator=(DenseLayerImpl&&) = delete;

    auto           reset() -> void;
    auto           check_input(torch::Tensor const& input) const -> void;
    auto           batch_size(size_t batch_size) -> void;
    constexpr auto batch_size() const noexcept -> size_t;
    constexpr auto weight_tensor() const noexcept -> torch::Tensor const&;
    constexpr auto bias_tensor() const noexcept -> torch::Tensor const&;
};

constexpr auto DenseLayerImpl::weight_tensor() const noexcept
    -> torch::Tensor const&
{
    return _weight_tensor;
}

constexpr auto DenseLayerImpl::bias_tensor() const noexcept
    -> torch::Tensor const&
{
    return _bias_tensor;
}

constexpr auto DenseLayerImpl::batch_size() const noexcept -> size_t
{
    return static_cast<size_t>(_batch_size);
}
// --------------------------- [DenseLayerImpl] ---------------------------- }}}

// ----------------------------- [DenseLayer] ------------------------------ {{{
template <class Activation>
class DenseLayer
    : private Activation
    , public DenseLayerImpl {
  public:
    using DenseLayerImpl::DenseLayerImpl;

  private:
    inline auto apply_bias_and_activation() -> void;
    inline auto forward_fast(torch::Tensor const& input) -> torch::Tensor;
    inline auto forward_slow(torch::Tensor const& input) -> torch::Tensor;

  public:
    inline auto forward(torch::Tensor const& input) -> torch::Tensor;
    inline auto operator()(torch::Tensor const& input) -> torch::Tensor;
};

template <class Activation>
auto DenseLayer<Activation>::apply_bias_and_activation() -> void
{
    TCM_ASSERT(_with_bias || _bias.data() == nullptr, "");
    detail::bias_and_activation(
        static_cast<size_t>(_batch_size), static_cast<size_t>(_out_features),
        _dst.data(), static_cast<size_t>(_out_features), _bias.data(),
        static_cast<Activation const&>(*this));
}

template <class Activation>
auto DenseLayer<Activation>::forward_slow(torch::Tensor const& input)
    -> torch::Tensor
{
    TCM_ASSERT(!_with_bias || _bias_tensor.defined(), "");
    return static_cast<Activation const&>(*this)(
        torch::linear(input, _weight_tensor, _bias_tensor));
}

template <class Activation>
auto DenseLayer<Activation>::forward_fast(torch::Tensor const& input)
    -> torch::Tensor
{
    if (input.dim() == 2) {
        // cblas_sgemm(
        //     /*layout=*/CblasRowMajor, /*transa=*/CblasNoTrans,
        //     /*transb=*/CblasTrans, /*m=*/_batch_size, /*n=*/_out_features,
        //     /*k=*/_in_features,
        //     /*alpha=*/1.0f, /*a=*/input.data<float>(), /*lda=*/_in_features,
        //     /*b=*/_weight.data(), /*ldb=*/_in_features, /*beta=*/0.0f,
        //     /*c=*/_dst.data(), /*ldc=*/_out_features);
        (*_gemm)(static_cast<float const*>(input.data_ptr()), _weight.data(),
                 _dst.data());
        apply_bias_and_activation();
        return _dst_tensor;
    }
    else {
        TCM_ASSERT(input.dim() == 1, "");
        cblas_sgemv(/*layout=*/CblasRowMajor, /*trans=*/CblasNoTrans,
                    /*m=*/_out_features, /*n=*/_in_features,
                    /*alpha=*/1.0f, /*a=*/_weight.data(), /*lda=*/_in_features,
                    /*x=*/input.data<float>(), /*incx=*/1, /*beta*/ 0.0f,
                    /*y=*/_dst.data(), /*incy=*/1);
        detail::bias_and_activation(
            1, static_cast<size_t>(_out_features), _dst.data(),
            static_cast<size_t>(_out_features), _bias.data(),
            static_cast<Activation const&>(*this));
        return torch::from_blob(_dst.data(), {_out_features},
                                torch::TensorOptions{torch::kFloat32});
    }
}

template <class Activation>
auto DenseLayer<Activation>::forward(torch::Tensor const& input)
    -> torch::Tensor
{
    return torch::nn::Module::is_training() ? forward_slow(input)
                                            : forward_fast(input);
}

template <class Activation>
auto DenseLayer<Activation>::operator()(torch::Tensor const& input)
    -> torch::Tensor
{
    return forward(input);
}
// ----------------------------- [DenseLayer] ------------------------------ }}}

// ---------------------------- [AmplitudeNet] ----------------------------- {{{
class AmplitudeNet : public DisableConversions {
    std::shared_ptr<DenseLayer<ReLU>>     _layer_1;
    std::shared_ptr<DenseLayer<ReLU>>     _layer_2;
    std::shared_ptr<DenseLayer<Softplus>> _layer_3;

  public:
    AmplitudeNet(std::array<size_t, 3> sizes, size_t batch_size);
    AmplitudeNet(AmplitudeNet const&);
    AmplitudeNet(AmplitudeNet&&) noexcept = default;
    AmplitudeNet& operator=(AmplitudeNet const&) = delete;
    AmplitudeNet& operator=(AmplitudeNet&&) = delete;

    inline auto batch_size() const noexcept -> size_t;
    auto        operator()(torch::Tensor const& input) const -> torch::Tensor;
};

auto AmplitudeNet::batch_size() const noexcept -> size_t
{
    return _layer_1->batch_size();
}
// ---------------------------- [AmplitudeNet] ----------------------------- }}}

// ------------------------------ [PhaseNet] ------------------------------- {{{
class PhaseNet : public DisableConversions {
    std::shared_ptr<DenseLayer<Tanh>>     _layer_1;
    std::shared_ptr<DenseLayer<Identity>> _layer_2;

  public:
    PhaseNet(std::array<size_t, 2> sizes, size_t batch_size);
    PhaseNet(PhaseNet const& other);
    PhaseNet(PhaseNet&&) noexcept = default;
    PhaseNet& operator=(PhaseNet const& other) = delete;
    PhaseNet& operator=(PhaseNet&&) noexcept = delete;

    inline auto batch_size() const noexcept -> size_t;
    auto        operator()(torch::Tensor input) const -> torch::Tensor;
};

inline auto PhaseNet::batch_size() const noexcept -> size_t
{
    return _layer_1->batch_size();
}
// ------------------------------ [PhaseNet] ------------------------------- }}}

// --------------------------- [CombiningState] ---------------------------- {{{
inline auto combine_amplitude_and_phase(torch::Tensor&       amplitude,
                                        torch::Tensor const& phase)
{
    if (amplitude.dim() == 1) {
        auto amplitude_accessor = amplitude.accessor<float, 1>();
        auto phase_accessor     = phase.accessor<float, 1>();
        TCM_CHECK_SHAPE(amplitude_accessor.size(0), 1);
        TCM_CHECK_SHAPE(phase_accessor.size(0), 2);
        auto const flag = phase_accessor[0] < phase_accessor[1];
        if (flag) { amplitude_accessor[0] *= -1.0f; }
        return;
    }
    auto amplitude_accessor = amplitude.accessor<float, 2>();
    auto phase_accessor     = phase.accessor<float, 2>();
    TCM_CHECK_SHAPE(amplitude_accessor.size(0), phase_accessor.size(0));
    TCM_CHECK_SHAPE(phase_accessor.size(1), 2);
    TCM_CHECK_SHAPE(amplitude_accessor.size(1), 1);
    for (auto i = int64_t{0}; i < amplitude_accessor.size(0); ++i) {
        auto const flag = phase_accessor[i][0] < phase_accessor[i][1];
        if (flag) { amplitude_accessor[i][0] *= -1.0f; }
    }
}

class CombiningState {
    std::shared_ptr<AmplitudeNet> _amplitude;
    std::shared_ptr<PhaseNet>     _phase;

  public:
    CombiningState(std::shared_ptr<AmplitudeNet> amplitude,
                   std::shared_ptr<PhaseNet>     phase)
        : _amplitude{std::move(amplitude)}, _phase{std::move(phase)}
    {}

    CombiningState(CombiningState const& other)
        : _amplitude{std::make_shared<AmplitudeNet>(*other._amplitude)}
        , _phase{std::make_shared<PhaseNet>(*other._phase)}
    {}
    CombiningState(CombiningState&&) noexcept = default;
    CombiningState& operator=(CombiningState const&) = delete;
    CombiningState& operator=(CombiningState&&) noexcept = default;

    auto amplitude() const noexcept -> std::shared_ptr<AmplitudeNet>
    {
        return _amplitude;
    }

    auto phase() const noexcept -> std::shared_ptr<PhaseNet> { return _phase; }

    auto operator()(torch::Tensor const& input) const -> torch::Tensor
    {
        auto       amplitude = (*_amplitude)(input);
        auto const phase     = (*_phase)(input);
        combine_amplitude_and_phase(amplitude, phase);
        return amplitude;
    }
};
// --------------------------- [CombiningState] ---------------------------- }}}

auto bind_networks(py::module m) -> void;

TCM_NAMESPACE_END

namespace pybind11 {
namespace detail {
    template <class Key, class Value>
    struct type_caster<::torch::OrderedDict<Key, Value>> {
      public:
        using type       = ::torch::OrderedDict<Key, Value>;
        using key_conv   = make_caster<Key>;
        using value_conv = make_caster<Value>;

        PYBIND11_TYPE_CASTER(type, _("Dict[") + key_conv::name + _(", ")
                                       + value_conv::name + _("]"));

        auto load(handle src, bool convert) -> bool
        {
            if (!isinstance<dict>(src)) { return false; }
            auto d = reinterpret_borrow<dict>(src);
            value.clear();
            for (auto it : d) {
                key_conv   kconv;
                value_conv vconv;
                if (!kconv.load(it.first.ptr(), convert)
                    || !vconv.load(it.second.ptr(), convert)) {
                    return false;
                }
                value.insert(cast_op<Key&&>(std::move(kconv)),
                             cast_op<Value&&>(std::move(vconv)));
            }
            return true;
        }

        template <typename T>
        static handle cast(T&& src, return_value_policy policy, handle parent)
        {
            dict d;
            auto policy_key   = policy;
            auto policy_value = policy;
            if (!std::is_lvalue_reference<T>::value) {
                policy_key =
                    return_value_policy_override<Key>::policy(policy_key);
                policy_value =
                    return_value_policy_override<Value>::policy(policy_value);
            }
            for (auto&& kv : src) {
                auto key   = reinterpret_steal<object>(key_conv::cast(
                    forward_like<T>(kv.key()), policy_key, parent));
                auto value = reinterpret_steal<object>(value_conv::cast(
                    forward_like<T>(kv.value()), policy_value, parent));
                if (!key || !value) { return handle(); }
                d[key] = value;
            }
            return d.release();
        }
    };
} // namespace detail
} // namespace pybind11


#ifndef MAP_DATA_HPP_DEF__
#define MAP_DATA_HPP_DEF__
#include<rexio/tk/toolkit.h++>
#include <string>
#include <boost/shared_ptr.hpp>
#include <boost/multi_array.hpp>
#include "keymap.h++"
#include "pathfinder.h++"
#include "entity.h++"
class MapData;
class PlayerAssets;

//! minimum distance between two positions on hex grid
Scr::Uint Distance(Scr::Position s, Scr::Position f);

//! execute F1 within range r around p, limited by board size pmax
template < typename F >
void for_each_position (Scr::Position p, Scr::Size pmax, Scr::Uint r, F F1)
{
	for (int i = (int)p.row-(int)r ; i <= (int)p.row+(int)r ; i++)
		for (int j = (int)p.col-(int)r ; j <= (int)p.col+(int)r ; j++)
			if (i>=0 and j>=0 and i<(int)pmax.height and j<(int)pmax.width)
			{
				Scr::Position p2(i,j);
				if (Distance(p,p2)<=r)
					F1(p2);
			}// to be optimized
}

//! board, tiles, entities and so on
class MapData
{
public:
	static const char Oil;
	static const char Road;
private:
	//!
	class MapField
	{
	private:
		char resources; //' ' - nothing
		// 'o' oil resources here!
		// 'w' oasis!
		SPE pE1; // something is here!

		char terrain_type; // '-' - road!
		// ' ' - plain wasteland
		//

		PlayerAssets * ZoI1; // Zone of Influence
	public:

		//! default ctor
		MapField();

		//! removes entity from specific field
		void RemoveEntity();
		//! insert entity
		void InsertEntity(SPE pE) {pE1=pE;}
		//! get SPE to entity (SPE is shared pointer)
		const SPE GetEntity() const
			{return pE1;}
		//! set influence
		void SetZoI (PlayerAssets * ZoI);
		//! who influences this field?
		PlayerAssets * GetZoI() {return ZoI1;}
		const std::string GetStatus() const; //description
		char GetResources() const {return resources;}
		void SetResources(char r) {resources=r;}
		char GetTerrainType() const {return terrain_type;}
		void SetTerrainType(char t) {terrain_type=t;}
		HP Attack(int damage);
	};
	boost::multi_array<MapField,2> Fields;
public:
	typedef std::list<std::tr1::shared_ptr<PlayerAssets> > PlayerList;
private:
	PlayerList PL1;
	PlayerList::iterator active; // who plays??
	int turn;
public:
	MapData();
	void Randomize(PlayerList& PL,int h, int w); // generate random  map

	//! Size of map (number of fields vertical/horizontal)
	Scr::Size GetSize();
	//! height of map
	Scr::Uint GetHeight();
	//! width of map
	Scr::Uint GetWidth();

	//! resources on specific field ('o' = oil, ' ' = nothing)
	char GetResources(int y, int x) const {return Fields[y][x].GetResources();}
	//! Statusbar description for field
	const std::string GetStatus(int y, int x) const {return Fields[y][x].GetStatus();}

	char GetTerrainType(int y, int x) const {return Fields[y][x].GetTerrainType();}
	void SetTerrainType(int y, int x,char t)
		{return Fields[y][x].SetTerrainType(t);}
	//! Cast attack on specific field
	//! \param y
	//! \param x coordinates of target
	//! \param damage - power of attack
	HP Attack(int y, int x,int damage);

	//! return entity on specific field
	const SPE GetEntity(int y, int x) const
		{
			return Fields[y][x].GetEntity();
		}
private:

	template <typename F>
	void ForEachEntityInZoI(Scr::Position p, F F1);
	// recalculate ZoI for entity of cordinates p
	void AcquireZoI (Scr::Position p);
	void VerifyZoI (Scr::Position p);
public:
	//! Insert entity to map
	void InsertEntity(int y, int x, SPE e,PlayerAssets * a)
		throw(Entity::IncorrectPosition);

	//! Remove entity from map
	void RemoveEntity(int y, int x);

	//! to != end() !!!!
	void MoveEntity(PathFinder::Path::iterator from,
					PathFinder::Path::iterator to);

	void RecalculateZoI(int y, int x, PlayerAssets * ZoI);

	void SetZoI(int y, int x, PlayerAssets * ZoI);

	PlayerAssets *  GetZoI(int y, int x)
		{
			return Fields[y][x].GetZoI();
		}

	//! end turn (pass control to next player)
	void SwitchTurn();

	//! Name of active player
	const std::string GetPlayerName() const ;

	//! full record of active player
	PlayerAssets * GetActive()
		{
			if (active==PL1.end())
				return 0;
			else
				return active->get();
		}
	//! turn number
	int GetTurn() {return turn;}
};

#endif


/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    This program is free software: you can redistribute it and/or modify
|    it under the terms of the GNU General Public License as published by
|    the Free Software Foundation, either version 3 of the License, or
|    (at your option) any later version.
|
|    This program is distributed in the hope that it will be useful,
|    but WITHOUT ANY WARRANTY; without even the implied warranty of
|    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
|    GNU General Public License for more details.
|
|    You should have received a copy of the GNU General Public License
|    along with this program.  If not, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#include <vector>
#include <string>
#include <map>

#include <boost/lexical_cast.hpp>

#include <hashclash/saveload_bz2.hpp>
#include <hashclash/sha1detail.hpp>
#include <hashclash/sha1differentialpath.hpp>
#include <hashclash/booleanfunction.hpp>

#include "main.hpp"

using namespace hashclash;
using namespace std;

void sha1_backward_differential_step(const sha1differentialpath& path, path_container_autobalance& outpaths)
{
	const int t = int(outpaths.t);
	const unsigned& maxcond = outpaths.maxcond;
	const unsigned& maxsdrs = outpaths.maxsdrs;
	const unsigned& maxweight = outpaths.maxweight;
	const unsigned& minweight = outpaths.minweight;

	booleanfunction* F = 0;
	if (t < 20) F = &SHA1_F1_data; // this implementation only works properly for sha1_f1
	else if (t < 40) F = &SHA1_F2_data;
	else if (t < 60) F = &SHA1_F3_data;
	else if (t < 80) F = &SHA1_F4_data;

	static sha1differentialpath newpath;
	static vector<sdr> sdrs;
	static bitcondition Qtm1b[32], Qtm2b[32], Qtm3b[32];
	static vector<unsigned> bval;
	static uint32 fdiv[32];
	static bf_outcome foutcomes[32];
//	static std::vector<std::pair<uint32,double> > rotateddiff;
	static vector<sdr> deltam;
	static vector<unsigned> Qtm1prev, Qtm1prevn;

	newpath = path;
	newpath[t-4].clear();

	unsigned totprecond = 0;
	unsigned totcond = 0;
	for (int k = t; k <= outpaths.tend && k <= 80; ++k)
		if (k < newpath.tend())
			totprecond += newpath[k].hw();
	totcond = totprecond + newpath[t-1].hw() + newpath[t-2].hw();

	unsigned minextracond = 0;
	Qtm1prev.clear();
	Qtm1prevn.clear();
	bf_outcome outconstant, outplus, outminus;
	for (unsigned b = 0; b < 32; ++b)
	{
		Qtm1b[b] = newpath(t-1,b);
		if (Qtm1b[b] == bc_prev) {
			Qtm1prev.push_back(b);
			Qtm1b[b] = bc_constant;
			newpath[t-1].set(b, bc_constant);
		} else if (Qtm1b[b] == bc_prevn) {
			Qtm1prevn.push_back(b);
			Qtm1b[b] = bc_constant;
			newpath.setbitcondition(t-1,b, bc_constant);
		}
		Qtm2b[b] = newpath(t-2,(b+2)&31);
		if (b < 31) {
			outconstant = F->outcome( Qtm1b[b], Qtm2b[b], bc_constant );
			outplus = F->outcome( Qtm1b[b], Qtm2b[b], bc_plus );
			outminus = F->outcome( Qtm1b[b], Qtm2b[b], bc_minus );
		} else {
			outconstant = msb_bf_outcome(*F, Qtm1b[b], Qtm2b[b], bc_constant );
			outplus = msb_bf_outcome(*F, Qtm1b[b], Qtm2b[b], bc_plus );
			outminus = msb_bf_outcome(*F, Qtm1b[b], Qtm2b[b], bc_minus );
		}
		if (outconstant.size() > 1 && outplus.size() > 1 && outminus.size() > 1)
			++minextracond;
	}
	sdr sdrQtm3 = newpath[t-3].getsdr();
	uint32 Qtm3_hwnaf = hwnaf(sdrQtm3.adddiff());
	if (totcond + Qtm3_hwnaf + minextracond > maxcond) return;

	/* table deltam */
	sdr mmask = outpaths.m_mask[t];
	deltam.clear();
	deltam.reserve(1<<mmask.hw());
	uint32 addmask = (~mmask.mask)+1;
	uint32 andmask = mmask.mask & 0x7FFFFFFF;
	mmask.sign = 0;
	do {
		mmask.sign += addmask; mmask.sign &= andmask;
		deltam.push_back(mmask);
	} while (mmask.sign != 0 && !outpaths.onemessagediff);
#if 1
        unsigned minhw = 32;
        for (unsigned i = 0; i < deltam.size(); ++i)
                if (hwnaf(deltam[i].adddiff()) < minhw)
                        minhw = hwnaf(deltam[i].adddiff());
        unsigned ii = 0;
        while (ii < deltam.size()) {
                if (hwnaf(deltam[ii].adddiff()) > minhw) {
                        swap(deltam[ii], deltam.back());
                        deltam.pop_back();
                } else
                        ++ii;
        }
#endif

	/* table sdrs for delta Qtm1 */
	unsigned w = Qtm3_hwnaf+1;
	if (w < minweight) w = minweight;
        unsigned mincount = 0;
        if (minweight > 0)
                mincount = count_sdrs(sdrQtm3,minweight-1,30);
	while (
			(w < 32)
			&& (w+1 <= maxweight)
			&& (totcond + w + 1 + minextracond <= maxcond)
			&& (count_sdrs(sdrQtm3, w+1,30)-mincount <= maxsdrs)
		)
		++w;
	table_sdrs(sdrs, sdrQtm3, w, 30);

	wordconditions pathQtm1 = newpath[t-1];
	wordconditions& newpathQtm1 = newpath[t-1];
	wordconditions& newpathQtm2 = newpath[t-2];
	wordconditions& newpathQtm3 = newpath[t-3];
	wordconditions& newpathQtm4 = newpath[t-4];

	uint32 dQtm4pc = newpath[t+1].diff() - newpath[t].getsdr().rotate_left(5).adddiff();

	vector<sdr>::const_iterator cit = sdrs.begin(), citend = sdrs.end();
	for (; cit != citend; ++cit)
	{
		sdrQtm3 = *cit;
		unsigned hwQtm3 = sdrQtm3.hw();
		if (hwQtm3 < minweight) continue;
		if (totcond + hwQtm3 + minextracond > maxcond)
			continue;
		newpathQtm3 = sdrQtm3;

		uint32 cnt = 1;
		uint32 dF_fixed = 0;
		unsigned maxextracond = 0;
		bval.clear();
		for (unsigned b = 0; b < 32; ++b)
		{
			Qtm3b[b] = newpathQtm3.get((b+2)&31);
			if (b < 31)
				foutcomes[b] = F->outcome( Qtm1b[b], Qtm2b[b], Qtm3b[b] );
			else
				foutcomes[b] = msb_bf_outcome(*F, Qtm1b[b], Qtm2b[b], Qtm3b[b] );
			unsigned fsize = foutcomes[b].size();
			if (fsize > 1)
			{
				fdiv[b] = cnt;
				if (fsize == 2) cnt <<= 1;
				else if (fsize == 3) { cnt += cnt<<1; ++ maxextracond; }
				bval.push_back(b);
			} else
				dF_fixed += foutcomes[b](0,b);
		}
		if (totcond + hwQtm3 + bval.size() > maxcond)
			continue;
		if (outpaths.estimatefactor != 0)
		{
			outpaths.estimate(totcond + hwQtm3 + bval.size() + ((maxextracond+1)>>1), cnt*deltam.size());
			continue;
		}

		newpathQtm2 = path[t-2];

		std::reverse(bval.begin(), bval.end());
		bf_conditions newconditions;
		for (uint32 k = 0; k < cnt; ++k)
		{
			uint32 m = k;
			uint32 dF = dF_fixed;
			newpathQtm1 = pathQtm1;
			for (unsigned j = 0; j < bval.size(); ++j)
			{
				const unsigned b = bval[j];
				unsigned i = 0;
				while (m >= fdiv[b])
				{
					m -= fdiv[b];
					++i;
				}
				if (b < 31)
					newconditions = F->backwardconditions( Qtm1b[b], Qtm2b[b], Qtm3b[b], foutcomes[b][i] );
				else {
					newconditions = msb_bf_backwardconditions(*F, Qtm1b[b], Qtm2b[b], Qtm3b[b], foutcomes[b][i] );
					if (msb_bf_outcome(*F, newconditions).size() > 1 || msb_bf_outcome(*F, newconditions)[0] != foutcomes[b][i]) {
						cout << endl << "[" << Qtm1b[b] << Qtm2b[b] << Qtm3b[b] <<"](" << foutcomes[b][i] <<")=>[" << newconditions.first << newconditions.second << newconditions.third << "]" << endl;
						bf_outcome tmp = msb_bf_outcome(*F, Qtm1b[b], Qtm2b[b], Qtm3b[b]);
						for (unsigned i = 0; i < tmp.size(); ++i)
							cout << tmp[i] << flush;
						cout << endl;
						tmp = F->outcome(Qtm1b[b], Qtm2b[b], Qtm3b[b]);
						for (unsigned i = 0; i < tmp.size(); ++i)
							cout << tmp[i] << flush;
						cout << endl;
						newconditions = F->backwardconditions( Qtm1b[b], Qtm2b[b], Qtm3b[b], bc_constant );
						cout << "(.)=>[" << newconditions.first << newconditions.second << newconditions.third << "]" << endl;
						throw;
					}
				}
				newpathQtm1.set(b, newconditions.first);
				newpathQtm2.set((b+2)&31, newconditions.second);
				newpathQtm3.set((b+2)&31, newconditions.third);

				dF += foutcomes[b](i,b);
			}

			bool contradiction = false;
			for (unsigned i = 0; i < Qtm1prev.size(); ++i) {
				const unsigned b = Qtm1prev[i];
				const bitcondition bcQtm1b = newpathQtm1[b];
				switch (bcQtm1b) {
				case bc_constant:
					newpathQtm1.set(b, bc_prev);
					break;
				case bc_one:
				case bc_zero:
					if (newpathQtm2[b] == bc_prev) {
						newpathQtm3.set(b, bcQtm1b);
					} else if (newpathQtm2[b] == bc_prevn) {
						if (bcQtm1b == bc_one)
							newpathQtm3.set(b, bc_zero);
						else
							newpathQtm3.set(b, bc_one);
					} else if (newpathQtm2[b] != bc_constant && newpathQtm2[b] != bcQtm1b) {
						//cerr << "[^" << newpathQtm1[b] << newpathQtm2[b] << newpathQtm3[b] << "]" << flush;
						contradiction = true;
					}
					newpathQtm2.set(b, bcQtm1b);
					break;
				default:
					cerr << "[^" << newpathQtm1[b] << newpathQtm2[b] << newpathQtm3[b] << "]" << flush;
					contradiction = true;
				}
			}
			for (unsigned i = 0; i < Qtm1prevn.size(); ++i) {
				const unsigned b = Qtm1prevn[i];
				const bitcondition bcQtm1b = newpathQtm1[b];
				switch (bcQtm1b) {
				case bc_constant:
					newpathQtm1.set(b, bc_prevn);
					break;
				case bc_one:
				case bc_zero:
					if (newpathQtm2[b] == bc_prev) {
						if (bcQtm1b == bc_one)
							newpathQtm3.set(b, bc_zero);
						else
							newpathQtm3.set(b, bc_one);
					} else if (newpathQtm2[b] == bc_prevn) {
						newpathQtm3.set(b, bcQtm1b);
					} else if (newpathQtm2[b] != bc_constant && newpathQtm2[b] == bcQtm1b) {
						//cerr << "[^" << newpathQtm1[b] << newpathQtm2[b] << newpathQtm3[b] << "]" << flush;
						contradiction = true;
					}
					if (bcQtm1b == bc_one)
						newpathQtm2.set(b, bc_zero);
					else
						newpathQtm2.set(b, bc_one);
					break;
				default:
					cerr << "[^" << newpathQtm1[b] << newpathQtm2[b] << newpathQtm3[b] << "]" << flush;
					contradiction = true;
				}
			}
			if (contradiction)
				continue;

			unsigned ncond = totprecond + newpathQtm1.hw() + newpathQtm2.hw() + newpathQtm3.hw();
			if (ncond > maxcond) return;
			for (unsigned i = 0; i < deltam.size(); ++i)
			{
				newpath.getme(t) = deltam[i];
				uint32 dQtm4 = dQtm4pc - dF - deltam[i].adddiff();
				newpathQtm4 = naf(dQtm4).rotate_right(30);
				unsigned ncond2 = ncond;
				if (outpaths.includenaf) {
					if (outpaths.halfnafweight)
						ncond2 += (newpathQtm4.hw()>>1);
					else
						ncond2 += newpathQtm4.hw();
				}
				if (ncond2 <= maxcond)
					outpaths.push_back(newpath, ncond2);
			}
		} // for cnt
	} // for sdrs
}