#include <boost/mpl/assert.hpp>
#include <nt2/sdk/meta/as.hpp>
#include <nt2/sdk/meta/behave_as.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/type_traits/is_same.hpp>

using boost::mpl::_;
using boost::is_same;

int main()
{
  BOOST_MPL_ASSERT(( nt2::meta::behave_as< is_same<_,float> , float >               ));
  BOOST_MPL_ASSERT(( nt2::meta::behave_as< is_same<_,int>   , nt2::meta::as_<int> > ));
}


#define BOOST_TEST_MODULE scfixed
#define BOOST_TEST_DYN_LINK

#ifndef DONT_USE_BOOST
#include <boost/test/included/unit_test.hpp>
#else
#define BOOST_CHECK_EQUAL(a,b) ((a) == (b));
#define BOOST_AUTO_TEST_CASE(x) int sc_main(int argc, char* argv[])
#endif
//
#define CHECK_EQ(a,b) BOOST_CHECK_EQUAL(a,b)

#ifdef CHECK_TYPES
#define LOOP_ITERS 1
#define SHIFT_ITERS 1
#define CHECK_DOUBLE(a,b) TYPE_NAME(a); TYPE_NAME(b);
#elif CHECK_VALUES
#define LOOP_ITERS 32
#define SHIFT_ITERS 5
#define CHECK_DOUBLE(a,b) TYPE_SHORT(b); std::cout << (b).to_double() << "\n";
#else
#define LOOP_ITERS 32
#define SHIFT_ITERS 5
#define CHECK_DOUBLE(a,b) BOOST_CHECK_EQUAL((a).to_double(), (b).to_double());
#endif

#include "testData.h"
#include "get_type_info.h"
int status;

#ifndef DONT_USE_BOOST
// Keep SystemC compile happy!
int sc_main(int argc, char* argv[]) { return 0;}
#endif

using namespace std;

BOOST_AUTO_TEST_CASE(tb_units){
    testData Data;
    for(int i=0; i<LOOP_ITERS; i++) {
	  Data.inc();
	  Data.Reset1();

	  for(int j=0; j<LOOP_ITERS; j++) {
		Data.inc2();

		CHECK_DOUBLE((Data.ref_0 & Data.ref_s5_3), (Data.new_5_3 & Data.new_s5_3));
		CHECK_DOUBLE((Data.ref_0 & Data.ref_s5_2), (Data.new_5_3 & Data.new_s5_2));

		CHECK_DOUBLE((Data.ref_0 | Data.ref_s5_3), (Data.new_5_3 | Data.new_s5_3));
		CHECK_DOUBLE((Data.ref_0 | Data.ref_s5_2), (Data.new_5_3 | Data.new_s5_2));

		CHECK_DOUBLE((~Data.ref_s5_3), (~Data.new_s5_3));
		CHECK_DOUBLE((~Data.ref_s5_2), (~Data.new_s5_2));
		CHECK_DOUBLE((~Data.ref_u5_3), (~Data.new_u5_3));
		CHECK_DOUBLE((~Data.ref_u5_2), (~Data.new_u5_2));

		CHECK_DOUBLE((Data.ref_0 ^ Data.ref_s5_3), (Data.new_5_3 ^ Data.new_s5_3));
		CHECK_DOUBLE((Data.ref_0 ^ Data.ref_s5_2), (Data.new_5_3 ^ Data.new_s5_2));

		CHECK_DOUBLE((Data.ref_0*Data.ref_s5_3), (Data.new_5_3*Data.new_s5_3));
		CHECK_DOUBLE((Data.ref_0*Data.ref_s5_2), (Data.new_5_3*Data.new_s5_2));
		CHECK_DOUBLE((Data.ref_0*Data.ref_u5_3), (Data.new_5_3*Data.new_u5_3));
		CHECK_DOUBLE((Data.ref_0*Data.ref_u5_2), (Data.new_5_3*Data.new_u5_2));

		CHECK_DOUBLE((Data.ref_0+Data.ref_s5_3), (Data.new_5_3+Data.new_s5_3));
		CHECK_DOUBLE((Data.ref_0+Data.ref_s5_2), (Data.new_5_3+Data.new_s5_2));
		CHECK_DOUBLE((Data.ref_0+Data.ref_u5_3), (Data.new_5_3+Data.new_u5_3));
		CHECK_DOUBLE((Data.ref_0+Data.ref_u5_2), (Data.new_5_3+Data.new_u5_2));

		CHECK_DOUBLE((Data.ref_0-Data.ref_s5_3), (Data.new_5_3-Data.new_s5_3));
		CHECK_DOUBLE((Data.ref_0-Data.ref_s5_2), (Data.new_5_3-Data.new_s5_2));
		CHECK_DOUBLE((Data.ref_0-Data.ref_u5_3), (Data.new_5_3-Data.new_u5_3));
		CHECK_DOUBLE((Data.ref_0-Data.ref_u5_2), (Data.new_5_3-Data.new_u5_2));

		CHECK_DOUBLE((-Data.ref_s5_3), (-Data.new_s5_3));
		CHECK_DOUBLE((-Data.ref_s5_2), (-Data.new_s5_2));
		CHECK_DOUBLE((-Data.ref_u5_3), (-Data.new_u5_3));
		CHECK_DOUBLE((-Data.ref_u5_2), (-Data.new_u5_2));

		CHECK_EQ(!Data.ref_s5_3, !Data.new_s5_3);
		CHECK_EQ(!Data.ref_s5_2, !Data.new_s5_2);
		CHECK_EQ(!Data.ref_u5_3, !Data.new_u5_3);
		CHECK_EQ(!Data.ref_u5_2, !Data.new_u5_2);

		CHECK_DOUBLE( Data.ref_0 &= Data.ref_s5_3,  Data.new_5_3 &= Data.new_s5_3);
		CHECK_DOUBLE( Data.ref_0 &= Data.ref_s5_2,  Data.new_5_3 &= Data.new_s5_2);
		CHECK_DOUBLE( Data.ref_0 |= Data.ref_s5_3,  Data.new_5_3 |= Data.new_s5_3);
		CHECK_DOUBLE( Data.ref_0 |= Data.ref_s5_2,  Data.new_5_3 |= Data.new_s5_2);
		CHECK_DOUBLE( Data.ref_0 ^= Data.ref_s5_3,  Data.new_5_3 ^= Data.new_s5_3);
		CHECK_DOUBLE( Data.ref_0 ^= Data.ref_s5_2,  Data.new_5_3 ^= Data.new_s5_2);

	  }

	  Data.inc();
	  Data.Reset1();

	  for(int j=0; j<LOOP_ITERS; j++) {
		s_0 ref_local;
		t_0 new_local;
		ref_local = Data.ref_0;
		new_local = Data.new_5_3;

		Data.inc2();
		CHECK_DOUBLE( ref_local *= Data.ref_s5_3,  new_local *= Data.new_s5_3);
		CHECK_DOUBLE( ref_local *= Data.ref_s5_2,  new_local *= Data.new_s5_2);
		CHECK_DOUBLE( ref_local *= Data.ref_u5_3,  new_local *= Data.new_u5_3);
		CHECK_DOUBLE( ref_local *= Data.ref_u5_3,  new_local *= Data.new_u5_3);

		ref_local = Data.ref_0;
		new_local = Data.new_5_3;

		CHECK_DOUBLE( ref_local += Data.ref_s5_3,  new_local += Data.new_s5_3);
		CHECK_DOUBLE( ref_local += Data.ref_s5_2,  new_local += Data.new_s5_2);
		CHECK_DOUBLE( ref_local += Data.ref_u5_3,  new_local += Data.new_u5_3);
		CHECK_DOUBLE( ref_local += Data.ref_u5_3,  new_local += Data.new_u5_3);

		ref_local = Data.ref_0;
		new_local = Data.new_5_3;

		CHECK_DOUBLE( ref_local -= Data.ref_s5_3,  new_local -= Data.new_s5_3);
		CHECK_DOUBLE( ref_local -= Data.ref_s5_2,  new_local -= Data.new_s5_2);
		CHECK_DOUBLE( ref_local -= Data.ref_u5_3,  new_local -= Data.new_u5_3);
		CHECK_DOUBLE( ref_local -= Data.ref_u5_3,  new_local -= Data.new_u5_3);

	  }

    }

    for(int i=0; i<LOOP_ITERS; i++) {
	  Data.inc();
	  Data.Reset1();

	  for(int j=0; j<LOOP_ITERS; j++) {
		Data.inc2();
		CHECK_EQ( (Data.ref_0 == Data.ref_s5_3), (Data.new_5_3 == Data.new_s5_3));
		CHECK_EQ( (Data.ref_0 == Data.ref_s5_2), (Data.new_5_3 == Data.new_s5_2));
		CHECK_EQ( (Data.ref_0 != Data.ref_s5_3), (Data.new_5_3 != Data.new_s5_3));
		CHECK_EQ( (Data.ref_0 != Data.ref_s5_2), (Data.new_5_3 != Data.new_s5_2));
		CHECK_EQ( (Data.ref_0 > Data.ref_s5_3),  (Data.new_5_3 > Data.new_s5_3));
		CHECK_EQ( (Data.ref_0 > Data.ref_s5_2),  (Data.new_5_3 > Data.new_s5_2));
		CHECK_EQ( (Data.ref_0 >= Data.ref_s5_3), (Data.new_5_3 >= Data.new_s5_3));
		CHECK_EQ( (Data.ref_0 >= Data.ref_s5_2), (Data.new_5_3 >= Data.new_s5_2));
		CHECK_EQ( (Data.ref_0 < Data.ref_s5_3),  (Data.new_5_3 < Data.new_s5_3));
		CHECK_EQ( (Data.ref_0 < Data.ref_s5_2),  (Data.new_5_3 < Data.new_s5_2));
		CHECK_EQ( (Data.ref_0 <= Data.ref_s5_3), (Data.new_5_3 <= Data.new_s5_3));
		CHECK_EQ( (Data.ref_0 <= Data.ref_s5_2), (Data.new_5_3 <= Data.new_s5_2));

		CHECK_EQ( (Data.ref_0 == Data.ref_u5_3), (Data.new_5_3 == Data.new_u5_3));
		CHECK_EQ( (Data.ref_0 == Data.ref_u5_2), (Data.new_5_3 == Data.new_u5_2));
		CHECK_EQ( (Data.ref_0 != Data.ref_u5_3), (Data.new_5_3 != Data.new_u5_3));
		CHECK_EQ( (Data.ref_0 != Data.ref_u5_2), (Data.new_5_3 != Data.new_u5_2));
		CHECK_EQ( (Data.ref_0 > Data.ref_u5_3), (Data.new_5_3 > Data.new_u5_3));
		CHECK_EQ( (Data.ref_0 > Data.ref_u5_2), (Data.new_5_3 > Data.new_u5_2));
		CHECK_EQ( (Data.ref_0 < Data.ref_u5_3), (Data.new_5_3 < Data.new_u5_3));
		CHECK_EQ( (Data.ref_0 < Data.ref_u5_2), (Data.new_5_3 < Data.new_u5_2));
		CHECK_EQ( (Data.ref_0 >= Data.ref_u5_3), (Data.new_5_3 >= Data.new_u5_3));
		CHECK_EQ( (Data.ref_0 >= Data.ref_u5_2), (Data.new_5_3 >= Data.new_u5_2));
		CHECK_EQ( (Data.ref_0 <= Data.ref_u5_3), (Data.new_5_3 <= Data.new_u5_3));
		CHECK_EQ( (Data.ref_0 <= Data.ref_u5_2), (Data.new_5_3 <= Data.new_u5_2));
	  }
    }

    s_1 ref_s5_3;
    s_2 ref_s5_2;
    u_1 ref_u5_3;
    u_2 ref_u5_2;

    t_1 new_s5_3;
    t_2 new_s5_2;
    v_1 new_u5_3;
    v_2 new_u5_2;

    for(int i=0; i<SHIFT_ITERS; i++) {
	  for(int j=0; j<LOOP_ITERS; j++) {
		Data.inc2();
		ref_s5_3 = Data.ref_s5_3;
		ref_s5_2 = Data.ref_s5_2;
		ref_u5_3 = Data.ref_u5_3;
		ref_u5_2 = Data.ref_u5_2;

		new_s5_3 = Data.new_s5_3;
		new_s5_2 = Data.new_s5_2;
		new_u5_3 = Data.new_u5_3;
		new_u5_2 = Data.new_u5_2;

		CHECK_DOUBLE(ref_s5_3 <<= i, new_s5_3 <<= i);
		CHECK_DOUBLE(ref_s5_2 <<= i, new_s5_2 <<= i);
		CHECK_DOUBLE(ref_u5_3 <<= i, new_u5_3 <<= i);
		CHECK_DOUBLE(ref_u5_2 <<= i, new_u5_2 <<= i);

		ref_s5_3 = Data.ref_s5_3;
		ref_s5_2 = Data.ref_s5_2;
		ref_u5_3 = Data.ref_u5_3;
		ref_u5_2 = Data.ref_u5_2;

		new_s5_3 = Data.new_s5_3;
		new_s5_2 = Data.new_s5_2;
		new_u5_3 = Data.new_u5_3;
		new_u5_2 = Data.new_u5_2;

		CHECK_DOUBLE(ref_s5_3 >>= i, new_s5_3 >>= i);
		CHECK_DOUBLE(ref_s5_2 >>= i, new_s5_2 >>= i);
		CHECK_DOUBLE(ref_u5_3 >>= i, new_u5_3 >>= i);
		CHECK_DOUBLE(ref_u5_2 >>= i, new_u5_2 >>= i);

		ref_s5_3 = Data.ref_s5_3;
		ref_s5_2 = Data.ref_s5_2;
		ref_u5_3 = Data.ref_u5_3;
		ref_u5_2 = Data.ref_u5_2;

		new_s5_3 = Data.new_s5_3;
		new_s5_2 = Data.new_s5_2;
		new_u5_3 = Data.new_u5_3;
		new_u5_2 = Data.new_u5_2;

		ref_s5_3++;
		ref_s5_2++;
		ref_u5_3++;
		ref_u5_2++;

		new_s5_3++;
		new_s5_2++;
		new_u5_3++;
		new_u5_2++;

		CHECK_DOUBLE(ref_s5_3, new_s5_3);
		CHECK_DOUBLE(ref_s5_2, new_s5_2);
		CHECK_DOUBLE(ref_u5_3, new_u5_3);
		CHECK_DOUBLE(ref_u5_2, new_u5_2);

		ref_s5_3 = Data.ref_s5_3;
		ref_s5_2 = Data.ref_s5_2;
		ref_u5_3 = Data.ref_u5_3;
		ref_u5_2 = Data.ref_u5_2;

		new_s5_3 = Data.new_s5_3;
		new_s5_2 = Data.new_s5_2;
		new_u5_3 = Data.new_u5_3;
		new_u5_2 = Data.new_u5_2;

		//std::cout << "ref_u5_3 = " << ref_u5_3  << " vs " << new_u5_3 << "\n";

		ref_s5_3--;
		ref_s5_2--;
		ref_u5_3--;
		ref_u5_2--;

		new_s5_3--;
		new_s5_2--;
		new_u5_3--;
		new_u5_2--;

		//std::cout << "ref_u5_3 = " << ref_u5_3	  << " vs " << new_u5_3 << "\n";

		CHECK_DOUBLE(ref_s5_3, new_s5_3);
		CHECK_DOUBLE(ref_s5_2, new_s5_2);
		CHECK_DOUBLE(ref_u5_3, new_u5_3);
		CHECK_DOUBLE(ref_u5_2, new_u5_2);
	  }
    }
#ifdef DONT_USE_BOOST
	return 0;
#endif
}


/*
 * RobotMotionMapUpdater.hpp
 *
 *  Created on: Feb 5, 2014
 *      Author: Péter Fankhauser
 *	 Institute: ETH Zurich, Autonomous Systems Lab
 */

#pragma once

// Elevation Mapping
#include "elevation_mapping/ElevationMap.hpp"

// Eigen
#include <Eigen/Core>

// Kindr
#include <kindr/poses/eigen/HomogeneousTransformation.hpp>

// ROS (time)
#include <ros/ros.h>

namespace elevation_mapping {

/*!
 * Computes the map variance update from the pose covariance of the robot.
 */
class RobotMotionMapUpdater
{
 public:

  /*!
   * Constructor.
   */
  RobotMotionMapUpdater(ros::NodeHandle& nodeHandle);

  /*!
   * Destructor.
   */
  virtual ~RobotMotionMapUpdater();

  /*!
   * Reads and verifies the ROS parameters.
   * @return true if successful.
   */
  bool readParameters();

  /*!
   * Computes the model update for the elevation map based on the pose covariance and
   * adds the update to the map.
   * @param[in] map the elevation map to be updated.
   * @param[in] robotPose the latest pose.
   * @param[in] robotPoseCovariance the latest pose covariance matrix.
   * @param[in] time the time of the update.
   * @return true if successful.
   */
  bool update(ElevationMap& map,
              const kindr::poses::eigen_impl::HomogeneousTransformationPosition3RotationQuaternionD& robotPose,
              const Eigen::Matrix<double, 6, 6>& robotPoseCovariance,
              const ros::Time& time);

 private:

  //! ROS nodehandle.
  ros::NodeHandle& nodeHandle_;

  //! Robot pose covariance from the previous update.
  Eigen::Matrix<double, 6, 6> previousRobotPoseCovariance_;

  //! Scaling factor for the covariance matrix (default all ones).
  Eigen::Array<double, 6, 6> covarianceScale_;
};

} /* namespace */


/**
 * Copyright (c) 2011-2017 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero 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 Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <bitcoin/bitcoin/wallet/ec_public.hpp>

#include <algorithm>
#include <iostream>
#include <boost/program_options.hpp>
#include <bitcoin/bitcoin/formats/base_16.hpp>
#include <bitcoin/bitcoin/math/elliptic_curve.hpp>
#include <bitcoin/bitcoin/math/hash.hpp>
#include <bitcoin/bitcoin/utility/data.hpp>
#include <bitcoin/bitcoin/wallet/ec_private.hpp>
#include <bitcoin/bitcoin/wallet/payment_address.hpp>

namespace libbitcoin {
namespace wallet {

const uint8_t ec_public::compressed_even = 0x02;
const uint8_t ec_public::compressed_odd = 0x03;
const uint8_t ec_public::uncompressed = 0x04;
const uint8_t ec_public::mainnet_p2kh = 0x00;

ec_public::ec_public()
 : valid_(false), compress_(true), point_(null_compressed_point)
{
}

ec_public::ec_public(const ec_public& other)
  : valid_(other.valid_), compress_(other.compress_), point_(other.point_)
{
}

ec_public::ec_public(const ec_private& secret)
  : ec_public(from_private(secret))
{
}

ec_public::ec_public(const data_chunk& decoded)
  : ec_public(from_data(decoded))
{
}

ec_public::ec_public(const std::string& base16)
  : ec_public(from_string(base16))
{
}

ec_public::ec_public(const ec_uncompressed& point, bool compress)
  : ec_public(from_point(point, compress))
{
}

ec_public::ec_public(const ec_compressed& point, bool compress)
  : valid_(true), compress_(compress), point_(point)
{
}

// Validators.
// ----------------------------------------------------------------------------

bool ec_public::is_point(data_slice decoded)
{
    return bc::is_public_key(decoded);
}

// Factories.
// ----------------------------------------------------------------------------

ec_public ec_public::from_private(const ec_private& secret)
{
    if (!secret)
        return ec_public();

    return ec_public(secret.to_public());
}

ec_public ec_public::from_string(const std::string& base16)
{
    data_chunk decoded;
    if (!decode_base16(decoded, base16))
        return ec_public();

    return ec_public(decoded);
}

ec_public ec_public::from_data(const data_chunk& decoded)
{
    if (!is_point(decoded))
        return ec_public();

    if (decoded.size() == ec_compressed_size)
        return ec_public(to_array<ec_compressed_size>(decoded), true);

    ec_compressed compressed;
    return bc::compress(compressed, to_array<ec_uncompressed_size>(decoded)) ?
        ec_public(compressed, false) : ec_public();
}

ec_public ec_public::from_point(const ec_uncompressed& point, bool compress)
{
    if (!is_point(point))
        return ec_public();

    ec_compressed compressed;
    return bc::compress(compressed, point) ? ec_public(compressed, compress) :
        ec_public();
}

// Cast operators.
// ----------------------------------------------------------------------------

ec_public::operator const bool() const
{
    return valid_;
}

ec_public::operator const ec_compressed&() const
{
    return point_;
}

// Serializer.
// ----------------------------------------------------------------------------

std::string ec_public::encoded() const
{
    if (compressed())
        return encode_base16(point_);

    // If the point is valid it should always decompress, but if not, is null.
    ec_uncompressed uncompressed(null_uncompressed_point);
    to_uncompressed(uncompressed);
    return encode_base16(uncompressed);
}

// Accessors.
// ----------------------------------------------------------------------------

const ec_compressed& ec_public::point() const
{
    return point_;
}

const bool ec_public::compressed() const
{
    return compress_;
}

// Methods.
// ----------------------------------------------------------------------------

bool ec_public::to_data(data_chunk& out) const
{
    if (!valid_)
        return false;

    if (compressed())
    {
        out.resize(ec_compressed_size);
        std::copy_n(point_.begin(), ec_compressed_size, out.begin());
        return true;
    }

    ec_uncompressed uncompressed;
    if (to_uncompressed(uncompressed))
    {
        out.resize(ec_uncompressed_size);
        std::copy_n(uncompressed.begin(), ec_uncompressed_size, out.begin());
        return true;
    }

    return false;
}

bool ec_public::to_uncompressed(ec_uncompressed& out) const
{
    if (!valid_)
        return false;

    return bc::decompress(out, to_array<ec_compressed_size>(point_));
}

payment_address ec_public::to_payment_address(uint8_t version) const
{
    return payment_address(*this, version);
}

// Operators.
// ----------------------------------------------------------------------------

ec_public& ec_public::operator=(const ec_public& other)
{
    valid_ = other.valid_;
    compress_ = other.compress_;
    point_ = other.point_;
    return *this;
}

bool ec_public::operator<(const ec_public& other) const
{
    return encoded() < other.encoded();
}

bool ec_public::operator==(const ec_public& other) const
{
    return valid_ == other.valid_ && compress_ == other.compress_ &&
        point_ == other.point_;
}

bool ec_public::operator!=(const ec_public& other) const
{
    return !(*this == other);
}

std::istream& operator>>(std::istream& in, ec_public& to)
{
    std::string value;
    in >> value;
    to = ec_public(value);

    if (!to)
    {
        using namespace boost::program_options;
        BOOST_THROW_EXCEPTION(invalid_option_value(value));
    }

    return in;
}

std::ostream& operator<<(std::ostream& out, const ec_public& of)
{
    out << of.encoded();
    return out;
}

} // namespace wallet
} // namespace libbitcoin


//
// Copyright (C) 2013 Mateusz Loskot <mateusz@loskot.net>
//
// 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)
//
#include "spatial_index_benchmark.hpp"
#ifdef SIBENCH_RTREE_LOAD_BLK
#define BOOST_GEOMETRY_INDEX_DETAIL_EXPERIMENTAL 1
#endif
#include <boost/geometry/index/rtree.hpp>
// enable internal debugging utilities
#include <boost/geometry/index/detail/rtree/utilities/statistics.hpp>
#include <boost/geometry/index/detail/rtree/utilities/print.hpp>
#include <boost/tuple/tuple.hpp>
using namespace std;
namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

template <typename T>
void print_statistics(std::ostream& os, std::string const& lib, T const& i)
{
    using bgi::detail::rtree::utilities::statistics;
    auto const s = statistics(i);
    os << sibench::get_banner(lib)
       << " stats: levels=" << boost::get<0>(s)
       << ", nodes=" << boost::get<1>(s)
       << ", leaves=" << boost::get<2>(s)
       << ", values=" << boost::get<3>(s)
       << ", values_min=" << boost::get<4>(s)
       << ", values_max=" << boost::get<5>(s)
       << std::endl;
}

int main()
{
    try
    {
#ifdef SIBENCH_BGI_RTREE_PARAMS_CT
        std::string const lib("bgi_ct");
#elif SIBENCH_BGI_RTREE_PARAMS_RT
        std::string const lib("bgi");
#else
#error Boost.Geometry rtree parameters variant unknown
#endif

        sibench::print_result_header(std::cout, lib);

        // Generate random objects for indexing
        auto const boxes = sibench::generate_boxes(sibench::max_insertions);

#ifdef SIBENCH_BGI_RTREE_PARAMS_RT
        for (std::size_t next_capacity = 2; next_capacity <= sibench::max_capacities; ++next_capacity)
        {
            double const fill_factor = 0.5;
            std::size_t const min_capacity = next_capacity;
            std::size_t const max_capacity = std::size_t(std::floor(min_capacity / fill_factor));
#elif SIBENCH_BGI_RTREE_PARAMS_CT
        {
            std::size_t const max_capacity = 1024;
            std::size_t const min_capacity =  340;
#endif

            typedef bg::model::point<double, 2, bg::cs::cartesian> point_t;
            typedef bg::model::box<point_t> box_t;
#ifdef SIBENCH_BGI_RTREE_PARAMS_CT
#ifdef SIBENCH_RTREE_SPLIT_LINEAR
            typedef bgi::rtree<box_t, bgi::linear<max_capacity, min_capacity>> rtree_t;
#elif SIBENCH_RTREE_SPLIT_QUADRATIC
            typedef bgi::rtree<box_t, bgi::quadratic<max_capacity, min_capacity>> rtree_t;
#else
            typedef bgi::rtree<box_t, bgi::rstar<max_capacity, min_capacity>> rtree_t;
#endif

            rtree_t rtree;

#elif SIBENCH_BGI_RTREE_PARAMS_RT
#ifdef SIBENCH_RTREE_SPLIT_LINEAR
            typedef bgi::dynamic_linear rtree_parameters_t;
#elif SIBENCH_RTREE_SPLIT_QUADRATIC
            typedef bgi::dynamic_quadratic rtree_parameters_t;
#else
            typedef bgi::dynamic_rstar rtree_parameters_t;
#endif
            typedef bgi::rtree<box_t, rtree_parameters_t> rtree_t;

            rtree_parameters_t rtree_parameters(max_capacity, min_capacity);
            rtree_t rtree(rtree_parameters);

#endif // SIBENCH_BGI_RTREE_PARAMS_RT

            // accumulated results store (load, query)
            sibench::result_info load_r;
            sibench::result_info query_r;

            load_r.min_capacity = query_r.min_capacity = min_capacity;
            load_r.max_capacity = query_r.max_capacity = max_capacity;

            // Benchmark: insert
            {
#ifdef SIBENCH_RTREE_LOAD_BLK
                typedef std::vector<box_t> box_values_t;
                box_values_t vboxes;
                vboxes.reserve(boxes.size());
                for(auto const& box : boxes)
                {
                    point_t p1(std::get<0>(box), std::get<1>(box));
                    point_t p2(std::get<2>(box), std::get<3>(box));
                    vboxes.emplace_back(p1, p2);
                }

                auto const marks = sibench::benchmark("load", 1, vboxes,
#if SIBENCH_BGI_RTREE_PARAMS_CT
                    [&rtree] (box_values_t const& boxes, std::size_t iterations)
                {
                    rtree = rtree_t(boxes.cbegin(), boxes.cend());
#else // !SIBENCH_BGI_RTREE_PARAMS_CT
                    [&rtree, &rtree_parameters] (box_values_t const& boxes, std::size_t iterations)
                {
                    rtree = rtree_t(boxes.cbegin(), boxes.cend(), rtree_parameters);
#endif
                    ::boost::ignore_unused_variable_warning(iterations);
                });
#else // !SIBENCH_RTREE_LOAD_BLK
                auto const marks = sibench::benchmark("load", boxes.size(), boxes,
                    [&rtree] (sibench::boxes2d_t const& boxes, std::size_t iterations)
                {
                    auto const s = iterations < boxes.size() ? iterations : boxes.size();
                    for (size_t i = 0; i < s; ++i)
                    {
                        auto const& box = boxes[i];
                        point_t p1(std::get<0>(box), std::get<1>(box));
                        point_t p2(std::get<2>(box), std::get<3>(box));
                        box_t region(p1, p2);
                        rtree.insert(region);
                    }
                });
#endif

                load_r.accumulate(marks);

                // debugging
                //sibench::print_result(std::cout, lib, marks);
                //std::cout <<  rtree;
                //print_statistics(std::cout, lib, rtree);
            }

            // Benchmark: query
            {
                std::size_t query_found = 0;

                auto const marks = sibench::benchmark("query", sibench::max_queries, boxes,
                    [&rtree, &query_found] (sibench::boxes2d_t const& boxes, std::size_t iterations)
                {
                    std::vector<box_t> result;
                    result.reserve(iterations);

                    for (std::size_t i = 0; i < iterations; ++i)
                    {
                        result.clear();
                        auto const& box = boxes[i];
                        point_t p1(std::get<0>(box) - 10, std::get<1>(box) - 10);
                        point_t p2(std::get<2>(box) + 10, std::get<3>(box) + 10);
                        box_t region(p1, p2);
                        rtree.query(bgi::intersects(region), std::back_inserter(result));
                        query_found += result.size();
                    }
                });

                query_r.accumulate(marks);

                // debugging
                //sibench::print_result(std::cout, lib, marks);
                //sibench::print_query_count(std::cout, lib, query_found);
            }

            // single line per run
            sibench::print_result(std::cout, lib, load_r, query_r);

        } // for capacity

        return EXIT_SUCCESS;
    }
    catch (std::exception const& e)
    {
        std::cerr << e.what() << std::endl;
    }
    catch (...)
    {
        std::cerr << "unknown error" << std::endl;
    }
    return EXIT_FAILURE;
}


#ifndef DAMPER_HH
#define DAMPER_HH

/*!
 * \file Damper.hh
 * \brief Definition of the Damper class
 */

#include <Eigen/Core>

/*!
 * \brief Class for damping an iteration
 *
 * Class Damper can be used to damp a matrix in an iterative process, such as
 * the Fock matrix in an SCF calculation, to prevent oscillations. Instead of
 * directly using the \f$n\$'th matrix \f$F_n\f$ in the iteration,
 * \f$cF_{n-1} + (1-c)F_{n}\f$ is used, where the damping parameter \f$c\f$ is
 * a constant between 0 and 1 that determineshow strongly the iteration is
 * damped.
 * \sa Class HFConverger
 */
class Damper
{
public:
	//! Constructor
	Damper(): _last(), _started(false) {}

	//! Return the error in the last step of the iteration
	double error() const { return _error; }

	/*!
	 * \brief Create a new matrix
	 *
	 * Create a new matrix from the current and previous iterations of the
	 * matrix. On the first call, this simply returns \a F unchanged, with
	 * an error of \a Etot. On subsequent calls, \f$cF_{-1} + (1-c)F\f$ is
	 * returned, where \f$F_{-1}\f$ is the result of the previous iteration.
	 * \param F      The current matrix matrix
	 * \param factor The damping factor \f$c\f$
	 * \param Etot   Scaling for the error
	 */
	void step(Eigen::MatrixXd& F, double factor, double Etot);
	void step(Eigen::MatrixXd& Fa, Eigen::MatrixXd& Fb, double factor, double Etot);

private:
	//! The previous matrix in the iteration
	Eigen::MatrixXd _last;
	//! The error in the iteration, \f$||F_{n} - F_{n-1}||_\infty\f$.
	double _error;
	//! Whether the iteration has been started
	bool _started;
};

#endif // DAMPER_HH


#include <iomanip>
#include <cassert>
#include "boost/format.hpp"

#include "amilab_messages.h"
#include "DefineClass.hpp"
#include "Variable.hpp"
#include <math.h>

#define NEW_SMARTPTR(type, var, value) \
  boost::shared_ptr<type> var(new type(value));

#define RETURN_VARPTR(type,  value) \
  boost::shared_ptr<type> newval(new type(value)); \
  return Variable<type>::ptr( new Variable<type>(newval));

#include <boost/numeric/conversion/cast.hpp>

// initialize conversions
std::vector<std::string> AMILabType<double>::conversion_types =
        { "bool", "float", "long", "int", "unsigned char", "short" };

//------------------------------------------------------
//------- Variable<double>
//------------------------------------------------------

/// Copy contents to new variable
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::NewCopy() const
{
  boost::shared_ptr<double> newval( new double(Value()));
  Variable<double>::ptr newvar(new Variable<double>(newval));
  return newvar;
}

// Arithmetic operators

/// +a
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator +()
{  RETURN_VARPTR(double,Value());}

/// prefix ++ operator ++a
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator ++()
{
  //std::cout << "**" << std::endl;
  RETURN_VARPTR(double,++RefValue());
}

/// postfix ++ operator a++
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator ++(int)
{
  //std::cout << "**" << std::endl;
  RETURN_VARPTR(double,RefValue()++);
}

/// -a
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator -()
{   RETURN_VARPTR(double,-Value());}

/// prefix -- operator --a
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator --()
{  RETURN_VARPTR(double,--RefValue()); }

/// postfix -- operator a--
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator --(int)
{  RETURN_VARPTR(double,RefValue()--);  }

/// a+b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator +(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(double,Value()+b->GetValueAsDouble());
  }
  else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a+=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator +=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RefValue() += b->GetValueAsDouble();
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a-b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator -(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(double,Value()-b->GetValueAsDouble());
  }
  else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a-=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator -=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RefValue() -= b->GetValueAsDouble();
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a*b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator *(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(double,Value()*b->GetValueAsDouble());
  }
  else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a*=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator *=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RefValue() *= b->GetValueAsDouble();
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a/b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator /(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(double,Value()/b->GetValueAsDouble());
  }
  else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a/=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator /=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RefValue() /= b->GetValueAsDouble();
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a%b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator %(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(double, ((int) round(Value())) % ((int) round(b->GetValueAsDouble())));
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a%=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator %=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RefValue() =  ((int) round(Value())) % ((int) round(b->GetValueAsDouble()));
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

//  Comparison Operators

/// a<b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator <(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value()<b->GetValueAsDouble());
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a<=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator <=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value()<=b->GetValueAsDouble());
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a>b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator >(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value()>b->GetValueAsDouble());
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a>=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator >=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value()>=b->GetValueAsDouble());
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a!=b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator !=(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value()!=b->GetValueAsDouble());
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

/// a==b
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator ==(const BasicVariable::ptr& b)
{
  //std::cout << __func__ << std::endl;
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value()==b->GetValueAsDouble());
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

// Logical operators

template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator !()
{
  RETURN_VARPTR(bool,!(Value()>0.5));
}

template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator &&(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value()&& (bool) (b->GetValueAsDouble()>0.5));
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::operator ||(const BasicVariable::ptr& b)
{
  if (b->IsNumeric()) {
    RETURN_VARPTR(bool,Value() || (bool) (b->GetValueAsDouble()>0.5));
  } else
    CLASS_ERROR("operation not defined");
  return this->NewReference();
}

// Mathematical functions
#define VAR_IMPL_FUNC(type,fname,func) \
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<type>::m_##fname() \
{ \
    RETURN_VARPTR(double, func(Value())); \
}

VAR_IMPL_FUNC(double,  sin,  sin)
VAR_IMPL_FUNC(double,  cos,  cos)
VAR_IMPL_FUNC(double,  tan,  tan)
VAR_IMPL_FUNC(double,  asin, asin)
VAR_IMPL_FUNC(double,  acos, acos)
VAR_IMPL_FUNC(double,  atan, atan)
VAR_IMPL_FUNC(double,  fabs, fabs)
VAR_IMPL_FUNC(double,  round,round)
VAR_IMPL_FUNC(double,  floor,floor)
VAR_IMPL_FUNC(double,  exp,  exp)
//VAR_IMPL_FUNC(double,  log,  1.0/log(10.0)*log)
VAR_IMPL_FUNC(double,  log,  log10)
VAR_IMPL_FUNC(double,  ln,   log)
VAR_IMPL_FUNC(double,  norm, fabs)
VAR_IMPL_FUNC(double,  sqrt, sqrt)
//VAR_IMPL_FUNC(double,  pow,  pow)

//---------------------------------------------------
template<> AMI_DLLEXPORT
BasicVariable::ptr Variable<double>::TryCast(
    const std::string& type_string) const
{
  try
  {
    // cast to float
    if (type_string==AMILabType<float>::name_as_string()) {
      RETURN_VARPTR(float, boost::numeric_cast<float>(Value()));
    } else
    // cast to int
    if (type_string==AMILabType<int>::name_as_string()) {
      RETURN_VARPTR(int, boost::numeric_cast<int>(Value()));
    } else
    // cast to long
    if (type_string==AMILabType<long>::name_as_string()) {
      RETURN_VARPTR(long, boost::numeric_cast<long>(Value()));
    } else
    // cast to unsigned long
    if (type_string==AMILabType<unsigned long>::name_as_string()) {
      RETURN_VARPTR(unsigned long, boost::numeric_cast<unsigned long>(Value()));
    } else
    // cast to unsigned char
    if (type_string==AMILabType<unsigned char>::name_as_string()) {
      RETURN_VARPTR(unsigned char, boost::numeric_cast<unsigned char>(Value()));
    } else
    // cast to bool
    if (type_string==AMILabType<bool>::name_as_string()) {
      RETURN_VARPTR(bool, boost::numeric_cast<bool>(Value()));
    } else
    {
      // make default conversion to double??
      CLASS_ERROR((boost::format("No conversion available for variable %1% from double to %2%") % _name % type_string).str().c_str());
    }
  } catch (std::bad_cast &e)
  {
    CLASS_ERROR((boost::format("%1%, for variable %2% from double to %3%") % e.what() % _name % type_string).str().c_str());
    return BasicVariable::ptr();
  }
  return BasicVariable::ptr();
}

//
template<> AMI_DLLEXPORT BasicVariable::ptr Variable<double>::BasicCast(const int& type)
{
  double res = Value();

  switch((WORDTYPE)type) {
    case WT_UNSIGNED_CHAR:  { RETURN_VARPTR(unsigned char,  (unsigned char)  res); }
    case WT_SIGNED_SHORT:   { RETURN_VARPTR(short,          (short)          res); }
    case WT_UNSIGNED_SHORT: { RETURN_VARPTR(unsigned short, (unsigned short) res); }
    case WT_SIGNED_INT:     { RETURN_VARPTR(int,            (int)            res); }
    case WT_UNSIGNED_INT:   res=(unsigned int) res;  break;
    case WT_FLOAT:          { RETURN_VARPTR(float,          (float)          res); }
    case WT_DOUBLE:         { RETURN_VARPTR(double,         (double)         res); }
    default:
      CLASS_ERROR(( boost::format("Conversion to type %1% not available")%((WORDTYPE)type)).str().c_str());
  }
  RETURN_VARPTR(double, res);
}

//
template<> AMI_DLLEXPORT
BasicVariable::ptr Variable<double>::TernaryCondition(const BasicVariable::ptr& v1, const BasicVariable::ptr&v2)
{
  if (Value()>0.5) {
    return v1->NewReference();
  } else {
    return v2->NewReference();
  }
  return NewReference();
}

template<> AMI_DLLEXPORT
  BasicVariable::ptr Variable<double>::operator[](const BasicVariable::ptr& v)
{
  if (v->IsNumeric()) {
    double* pointer = this->Pointer().get();
    double res = pointer[(int)(v->GetValueAsDouble()+0.5)];
    return AMILabType<double>::CreateVar(res);
  } else
    CLASS_ERROR("operator[] only takes a numerical parameter");
  return NewReference();
}

template<> AMI_DLLEXPORT
BasicVariable::ptr Variable<double>::operator =(const BasicVariable::ptr& b)
{
  if (IsNumeric()) {
    RefValue() = b->GetValueAsDouble();
  } else
    CLASS_ERROR("operation not defined");
  return NewReference();
}


#include <boost/dynamic_bitset.hpp>
#include <assert.h>
#include <stdlib.h>
#include <ctime>
#include <iomanip>
#include <chrono>
#include <string>
#include "random.h"
#include "IBSSimulations.h"

namespace PopGen{

    #ifndef random_h
        rnd::set_seed();
    #endif

    enum type {type_A,type_a};

    template <class _ploidytype>
    class Population{
        public:
        Population(const int n0[2], const Parameters* pop_pars, const Parameters* ind_pars) : Individual_pars(ind_pars), Population_pars(pop_pars) {
            N = n0[type_A]+n0[type_a];
            pop.reserve(N);
            for(int i = 0; i < n0[type_A]; ++i)
            pop.push_back(new _ploidytype(type_A, ind_pars));
            for(int i = 0; i < n0[type_a]; ++i)
            pop.push_back(new _ploidytype(type_a, ind_pars));
        }
        ~Population();
        /*
        Population Population::operator+ (const Population& pop2) const
        {
            Population out;
            out.genome = this->genome + pop2.genome;
            return out;
        }
        */
        bool Iterate(const int &Selection_locus, const double prob_death[2]){
            // Create offspring
            const int Nparents = pop.size();
            const int N = rnd::poisson(E_r(Nparents));
            offspring.clear();
            offspring.resize(N);

            // Random mating
            for(_ploidytype* ind : offspring){ ind = new _ploidytype(pop[rnd::integer(Nparents)],pop[rnd::integer(Nparents)]); }
            for(_ploidytype* ind : pop) delete ind;
            pop.clear();

            // Viability selection
            bool is_rescue = false;
            for(_ploidytype* ind : offspring){
                if(ind->GetRescue(Selection_locus)) {
                    is_rescue = true;
                    if(rnd::uniform() < prob_death[type_A]) pop.push_back(ind) ;
                }
                else{
                    if(rnd::uniform() < prob_death[type_a]) pop.push_back(ind) ;
                }
            }

            ++generation;
            return is_rescue;
        }

        private:
        typename std::vector<_ploidytype*>::iterator it;
        std::vector<_ploidytype*> offspring;
        int N;
        int generation = 0;
        inline double E_r(const int &Nparents){
            return (double)Nparents*(1.0 + Population_pars->r*(1.0 - ((double)Nparents / Population_pars->K)));
        }

        protected:
        Parameters* Individual_pars;
        Parameters* Population_pars;
        std::vector<_ploidytype*> pop;
    };

    class Individual{
    public:
        Individual(const Parameters* Simpars) : pars(Simpars){};
        virtual bool GetRescue(const int &locus);
        virtual int GenotypeCount();

        protected:
        const Parameters* pars;
        void Make_localbit(boost::dynamic_bitset<> &local, const boost::dynamic_bitset<> &global)
        {
            const int global_size = global.size();

            assert(local.size() == pars->NLOCI);
            assert(global_size > NLOCI);
            int start = rnd::integer(global_size - pars->NLOCI);

            for (int i = 0; i < pars->NLOCI; ++i)
            {
                local[i] = global[start + i];
            }
        };
    };

    class Haploid : public Individual{
        public:
        Haploid(const type &maketype, const Parameters* Simpars) : Individual(Simpars) {
            maketype == type_A ? haplotype.set(true) : haplotype.set(false);
        }
        Haploid(const Haploid* parent1, const Haploid* parent2, const Parameters*Simpars) : Individual(Simpars){
            boost::dynamic_bitset<> temp1, temp2, temp3, r_localbit(pars->NLOCI), m_localbit(pars->NLOCI);

            // Recombination
            Make_localbit(r_localbit, pars->r_global);
            temp1 = parent1->haplotype & r_localbit;
            temp2 = parent2->haplotype & r_localbit.flip();
            temp3 = temp1 | temp2;

            // Mutation
            Make_localbit(m_localbit, pars->m_global);
            temp1 = temp3 & m_localbit;
            temp2 = temp3 | m_localbit;
            haplotype = temp2 & temp1.flip();
        }
        int GenotypeCount(){
            return haplotype.count();
        }
        bool GetRescue(const int &locus){
            return haplotype[locus];
        }

        private:
        boost::dynamic_bitset<> haplotype;
    };

    /*
    class Diploid : public Individual{

    };
    */
};

int main(){
    using namespace PopGen;
    Parameters* pars;
    const int n0[2] = {5,5};

    Population<Haploid> pop(n0, pars, pars);
    return 0;
}

/*
    class Diploid : public Individual{
        public:
        Polyploid(const Parameters* Simpars, const int &NPLOIDY) : public Individual(Simpars) {}

        private:
        boost::dynamic_bitset<> genome;
    };
*/

/// Code graveyard:

/*
static void RescueSimulation(Population* pop, const int Ngen,  const Parameters* Simpars, SimData* SimulationData){
    using namespace PopGen;
    do{
        for (int i = 0; i < Ngen; ++i){
            if(pop->Iterate(Simpars)==false){
                delete pops;
                ++SimulationData.nofixcounter;
                break;
        }}
    }
    while(i < Ngen);

    SimulationData.push_back_protect(pop);
}

        void SaveCurrentState(DataBlock* data, const int Nloci, const int &Selection_locus){
            data->Nind.push_back(N);
            std::vector<int> count[2];
            int F_A = 0, F_a = 0, N_A = 0, N_a = 0;
            for(_ploidytype* ind : pop){
                if(ind->GetAllele(Selection_locus)){
                    ++N_A;
                    F_A += ind->GenotypeCount() - 1;
                }
                else{
                    ++N_a;
                    F_a += ind->GenotypeCount();
                }
            }
            data->NA.push_back(N_A);
            data->Na.push_back(N_a);
            data->FA.push_back((double)F_A/(double)((Nloci-1)*N_A));
            data->Fa.push_back((double)F_a/(double)((Nloci-1)*N_a));
        }
*/


#include <Columns/ColumnNullable.h>
#include <Columns/ColumnString.h>
#include <Columns/ColumnTuple.h>
#include <Columns/ColumnsNumber.h>
#include <Columns/IColumn.h>
#include <DataTypes/DataTypeTuple.h>
#include <DataTypes/DataTypesNumber.h>
#include <Functions/FunctionFactory.h>
#include <Functions/FunctionHelpers.h>
#include <Functions/IFunction.h>
#include <Functions/castTypeToEither.h>
#include <Interpreters/castColumn.h>
#include <boost/math/distributions/normal.hpp>
#include <Common/typeid_cast.h>

namespace DB
{

namespace ErrorCodes
{
    extern const int ILLEGAL_TYPE_OF_ARGUMENT;
    extern const int BAD_ARGUMENTS;
}

class FunctionTwoSampleProportionsZTest : public IFunction
{
public:
    static constexpr auto POOLED = "pooled";
    static constexpr auto UNPOOLED = "unpooled";

    static constexpr auto name = "proportionsZTest";

    static FunctionPtr create(ContextPtr) { return std::make_shared<FunctionTwoSampleProportionsZTest>(); }

    String getName() const override { return name; }

    size_t getNumberOfArguments() const override { return 6; }
    ColumnNumbers getArgumentsThatAreAlwaysConstant() const override { return {5}; }

    bool useDefaultImplementationForNulls() const override { return false; }
    bool useDefaultImplementationForConstants() const override { return true; }
    bool isSuitableForShortCircuitArgumentsExecution(const DataTypesWithConstInfo & /*arguments*/) const override { return false; }

    static DataTypePtr getReturnType()
    {
        auto float_data_type = std::make_shared<DataTypeNumber<Float64>>();
        DataTypes types(4, float_data_type);

        Strings names{"z_statistic", "p_value", "confidence_interval_low", "confidence_interval_high"};

        return std::make_shared<DataTypeTuple>(std::move(types), std::move(names));
    }

    DataTypePtr getReturnTypeImpl(const ColumnsWithTypeAndName & arguments) const override
    {
        for (size_t i = 0; i < 4; ++i)
        {
            if (!isUnsignedInteger(arguments[i].type))
            {
                throw Exception(
                    ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT,
                    "The {}th Argument of function {} must be an unsigned integer.",
                    i + 1,
                    getName());
            }
        }

        if (!isFloat(arguments[4].type))
        {
            throw Exception{ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT,
                "The fifth argument {} of function {} should be a float,",
                arguments[4].type->getName(),
                getName()};
        }

        /// There is an additional check for constancy in ExecuteImpl
        if (!isString(arguments[5].type) || !arguments[5].column)
        {
            throw Exception{ErrorCodes::ILLEGAL_TYPE_OF_ARGUMENT,
                "The sixth argument {} of function {} should be a constant string",
                arguments[5].type->getName(),
                getName()};
        }

        return getReturnType();
    }

    ColumnPtr executeImpl(const ColumnsWithTypeAndName & const_arguments, const DataTypePtr &, size_t input_rows_count) const override
    {
        auto arguments = const_arguments;
        /// Only last argument have to be constant
        for (size_t i = 0; i < 5; ++i)
            arguments[i].column = arguments[i].column->convertToFullColumnIfConst();

        static const auto uint64_data_type = std::make_shared<DataTypeNumber<UInt64>>();

        auto column_successes_x = castColumnAccurate(arguments[0], uint64_data_type);
        const auto & data_successes_x = checkAndGetColumn<ColumnVector<UInt64>>(column_successes_x.get())->getData();

        auto column_successes_y = castColumnAccurate(arguments[1], uint64_data_type);
        const auto & data_successes_y = checkAndGetColumn<ColumnVector<UInt64>>(column_successes_y.get())->getData();

        auto column_trials_x = castColumnAccurate(arguments[2], uint64_data_type);
        const auto & data_trials_x = checkAndGetColumn<ColumnVector<UInt64>>(column_trials_x.get())->getData();

        auto column_trials_y = castColumnAccurate(arguments[3], uint64_data_type);
        const auto & data_trials_y = checkAndGetColumn<ColumnVector<UInt64>>(column_trials_y.get())->getData();

        static const auto float64_data_type = std::make_shared<DataTypeNumber<Float64>>();

        auto column_confidence_level = castColumnAccurate(arguments[4], float64_data_type);
        const auto & data_confidence_level = checkAndGetColumn<ColumnVector<Float64>>(column_confidence_level.get())->getData();

        String usevar = checkAndGetColumnConst<ColumnString>(arguments[5].column.get())->getValue<String>();

        if (usevar != UNPOOLED && usevar != POOLED)
            throw Exception{ErrorCodes::BAD_ARGUMENTS,
                "The sixth argument {} of function {} must be equal to `pooled` or `unpooled`",
                arguments[5].type->getName(),
                getName()};

        const bool is_unpooled = (usevar == UNPOOLED);

        auto res_z_statistic = ColumnFloat64::create();
        auto & data_z_statistic = res_z_statistic->getData();
        data_z_statistic.reserve(input_rows_count);

        auto res_p_value = ColumnFloat64::create();
        auto & data_p_value = res_p_value->getData();
        data_p_value.reserve(input_rows_count);

        auto res_ci_lower = ColumnFloat64::create();
        auto & data_ci_lower = res_ci_lower->getData();
        data_ci_lower.reserve(input_rows_count);

        auto res_ci_upper = ColumnFloat64::create();
        auto & data_ci_upper = res_ci_upper->getData();
        data_ci_upper.reserve(input_rows_count);

        auto insert_values_into_result = [&data_z_statistic, &data_p_value, &data_ci_lower, &data_ci_upper](
                                             Float64 z_stat, Float64 p_value, Float64 lower, Float64 upper)
        {
            data_z_statistic.emplace_back(z_stat);
            data_p_value.emplace_back(p_value);
            data_ci_lower.emplace_back(lower);
            data_ci_upper.emplace_back(upper);
        };

        static constexpr Float64 nan = std::numeric_limits<Float64>::quiet_NaN();

        boost::math::normal_distribution<> nd(0.0, 1.0);

        for (size_t row_num = 0; row_num < input_rows_count; ++row_num)
        {
            const UInt64 successes_x = data_successes_x[row_num];
            const UInt64 successes_y = data_successes_y[row_num];
            const UInt64 trials_x = data_trials_x[row_num];
            const UInt64 trials_y = data_trials_y[row_num];
            const Float64 confidence_level = data_confidence_level[row_num];

            const Float64 props_x = static_cast<Float64>(successes_x) / trials_x;
            const Float64 props_y = static_cast<Float64>(successes_y) / trials_y;
            const Float64 diff = props_x - props_y;
            const UInt64 trials_total = trials_x + trials_y;

            if (successes_x == 0 || successes_y == 0 || successes_x > trials_x || successes_y > trials_y || trials_total == 0
                || !std::isfinite(confidence_level) || confidence_level < 0.0 || confidence_level > 1.0)
            {
                insert_values_into_result(nan, nan, nan, nan);
                continue;
            }

            Float64 se = std::sqrt(props_x * (1.0 - props_x) / trials_x + props_y * (1.0 - props_y) / trials_y);

            /// z-statistics
            /// z = \frac{ \bar{p_{1}} - \bar{p_{2}} }{ \sqrt{ \frac{ \bar{p_{1}} \left ( 1 - \bar{p_{1}} \right ) }{ n_{1} } \frac{ \bar{p_{2}} \left ( 1 - \bar{p_{2}} \right ) }{ n_{2} } } }
            Float64 zstat;
            if (is_unpooled)
            {
                zstat = (props_x - props_y) / se;
            }
            else
            {
                UInt64 successes_total = successes_x + successes_y;
                Float64 p_pooled = static_cast<Float64>(successes_total) / trials_total;
                Float64 trials_fact = 1.0 / trials_x + 1.0 / trials_y;
                zstat = diff / std::sqrt(p_pooled * (1.0 - p_pooled) * trials_fact);
            }

            if (!std::isfinite(zstat))
            {
                insert_values_into_result(nan, nan, nan, nan);
                continue;
            }

            // pvalue
            Float64 pvalue = 0;
            Float64 one_side = 1 - boost::math::cdf(nd, std::abs(zstat));
            pvalue = one_side * 2;

            // Confidence intervals
            Float64 d = props_x - props_y;
            Float64 z = -boost::math::quantile(nd, (1.0 - confidence_level) / 2.0);
            Float64 dist = z * se;
            Float64 ci_low = d - dist;
            Float64 ci_high = d + dist;

            insert_values_into_result(zstat, pvalue, ci_low, ci_high);
        }

        return ColumnTuple::create(
            Columns{std::move(res_z_statistic), std::move(res_p_value), std::move(res_ci_lower), std::move(res_ci_upper)});
    }
};

void registerFunctionZTest(FunctionFactory & factory)
{
    factory.registerFunction<FunctionTwoSampleProportionsZTest>();
}

}


/*
 * File:   HmcSampler.cpp
 * Author: aripakman
 *
 * Created on July 4, 2012, 10:44 AM
 */

#define _USE_MATH_DEFINES   // for the constant M_PI
//#include <cstdlib>
#include <cmath>
#include <tr1/random>
#include <Eigen/Dense>
#include <magnet/math/quartic.hpp>

#include "HmcSampler.h"

using namespace std;
using namespace std::tr1;
using namespace magnet::math;

const double HmcSampler::min_t = 0.00001;

HmcSampler::HmcSampler(const int & d, const int & seed ) {
dim=d;
//eng1.seed(static_cast<unsigned int >(time(NULL)));
eng1.seed(seed);
ud= uniform_real<>(0,M_PI);
}

void HmcSampler::setInitialValue(const VectorXd & initial_value){
   // double check =_verifyConstraints( initial_value );
  /*  if (check <0) {
        cout << "Initial condition out of constraint!" << endl;
        exit(1);
    } else */
            lastSample = initial_value;
    }

MatrixXd HmcSampler::sampleNext(bool returnTrace ) {

    MatrixXd tracePoints = MatrixXd(dim,0);   //this matrix will only be filled if(returnTrace)

    double T = ud(eng1);        // sample how much time T to move
    VectorXd b = lastSample;
    VectorXd a = VectorXd(dim);   // initial velocity

    while(2){

        double velsign =0;
        for (int i =0; i<dim; i++)     // Sample new initial velocity
        {  a(i)=  nd(eng1);            }

        double tt=T;   // tt is the time left to move
        double t1;
        double t2;
        int cn1,cn2; //constraint number

        bool first_bounce = true;    // for the first move, we do not fear that a small t1 or t2 is due to being in the boundary from the previous bounce.
        while (1){

            t1=0;
            if (!linearConstraints.empty())
                    _getNextLinearHitTime(a,b,t1,cn1);

            t2=0;
            if (!quadraticConstraints.empty() )
                    {   _getNextQuadraticHitTime(a,b,t2,cn2, first_bounce);
                        first_bounce=false;}

            double t =t1;     // how much time to move. if t==0, move tt
            bool linear_hit = true;
            if (t2>0 && (t1==0 || t2<t1 )){
                t=t2;
                linear_hit = false;
            }

            if (t==0 || tt < t){                 // if no wall to be hit (t==0) or not enough time left to hit the wall (tt<t2)
                break;
            }
            else{
                if (returnTrace){
                    _updateTrace(a,b,t,tracePoints);
                }

                tt = tt-t;
                VectorXd new_b   = sin(t)*a + cos(t)*b;   // hit location
                VectorXd hit_vel = cos(t)*a - sin(t)*b;   // hit velocity
                b = new_b;

                // reflect the velocity and verify that it points in the right direction

                if (!linear_hit){
                    QuadraticConstraint qc = quadraticConstraints[cn2];
                    VectorXd nabla = 2* ((qc.A)*b) + qc.B;
                    double alpha = (nabla.dot(hit_vel))/(nabla.dot(nabla));
                    a = hit_vel - 2*alpha*nabla;                  // reflected velocity
                    velsign = a.dot(nabla);
                }
                else {
                    LinearConstraint ql = linearConstraints[cn1];
                    double f2 = ((ql.f).dot((ql.f)));
                    double alpha = ((ql.f).dot(hit_vel))/f2;
                    a = hit_vel - 2*alpha*(ql.f);                  // reflected velocity
                    velsign = a.dot((ql.f));
                }
                if (velsign <0 ) break ;    //get out of while(1). resample the velocity and start again. this occurs rarely, due to numerical instabilities
            }

        } //while(1)

        if (velsign<0) {/* cout << "wrong velocity " << endl; */  continue;}    // go to beginning of while(2)

        VectorXd bb =  sin(tt)*a + cos(tt)*b;             //make last move of time tt without hitting walls

        double check = _verifyConstraints( bb);
        if (check >= 0){     //verify that we don't violate the constraints due to a numerical instability
            lastSample = bb;
            if (returnTrace){
                _updateTrace(a,b,tt,tracePoints);
                return tracePoints.transpose();

            } else return lastSample.transpose();
        }
       // at this point we have check<0, so we violated constraints: resample.

    } // while(2)
}

void HmcSampler::addLinearConstraint(const VectorXd & f, const double & g){
    LinearConstraint newConstraint;
    newConstraint.f =f;
    newConstraint.g =g;
    linearConstraints.push_back(newConstraint);
}
void HmcSampler::addQuadraticConstraint(const MatrixXd & A, const VectorXd & B, const double & C){
    QuadraticConstraint newConstraint;
    newConstraint.A =A;
    newConstraint.B =B;
    newConstraint.C =C;
    quadraticConstraints.push_back(newConstraint);

}

void HmcSampler::_getNextLinearHitTime(const VectorXd & a, const VectorXd & b, double & hit_time, int & cn ){
    hit_time=0;

    for (int i=0; i != linearConstraints.size(); i++ ){
        LinearConstraint lc = linearConstraints[i];
        double fa = (lc.f).dot(a);
        double fb = (lc.f).dot(b);
        double u = sqrt(fa*fa + fb*fb);
        if (u>lc.g && u>-lc.g){
                double phi =atan2(-fa,fb);      //     -pi < phi < pi
                double t1 = acos(-lc.g/u)-phi;  //     -pi < t1 < 2*pi

                if (t1<0) t1 += 2*M_PI;                //  0 < t1 < 2*pi
                if (abs(t1) < min_t ) t1=0;
                else if (abs(t1-2*M_PI) < min_t ) t1=0;

                double t2 = -t1-2*phi;             //  -4*pi < t2 < 3*pi
                if (t2<0) t2 += 2*M_PI;                 //-2*pi < t2 < 2*pi
                if (t2<0) t2 += 2*M_PI;                 //0 < t2 < 2*pi

                if (abs(t2) < min_t ) t2=0;
                else if (abs(t2-2*M_PI) < min_t ) t2=0;

                double t=t1;
                if (t1==0) t = t2;
                else if (t2==0) t = t1;
                else t=(t1<t2?t1:t2);

                if  (t> min_t  && (hit_time == 0 || t < hit_time)){
                    hit_time=t;
                    cn =i;
                }
        }
    }
}

void HmcSampler::_getNextQuadraticHitTime(const VectorXd & a, const VectorXd & b, double & hit_time, int & cn , const bool first_bounce ){
        hit_time=0;

        double mint;
        if (first_bounce) {mint=0;}
        else {mint=min_t;}

    for (int i=0; i != quadraticConstraints.size(); i++ ){

        QuadraticConstraint qc = quadraticConstraints[i];
        double q1= - ((a.transpose())*(qc.A))*a;
        q1 = q1 + ((b.transpose())*(qc.A))*b;
        double q2= (qc.B).dot(b);
        double q3= qc.C + a.transpose()*(qc.A)*a;
        double q4= 2*b.transpose()*(qc.A)*a;
        double q5= (qc.B).dot(a);

        double r4 = q1*q1 + q4*q4;
        double r3 = 2*q1*q2 + 2*q4*q5;
        double r2 = q2*q2 + 2*q1*q3 + q5*q5 -q4*q4;
        double r1 = 2*q2*q3 - 2*q4*q5;
        double r0=  q3*q3 - q5*q5;

        double roots[]={0,0,0,0};
        double aa = r3/r4;
        double bb = r2/r4;
        double cc = r1/r4;
        double dd = r0/r4;

        //Solve quartics of the form x^4 + aa x^3 + bb x^2 + cc x + dd ==0
        int sols = quarticSolve(aa, bb, cc, dd, roots[0], roots[1],  roots[2],  roots[3]);
        for (int j=0; j<sols; j++){
            double r = roots[j];
            if (abs(r) <=1 ){
                double l1 = q1*r*r + q2*r + q3;
                double l2 = -sqrt(1-r*r)*(q4*r + q5);
                if (l1/l2 > 0){
                    double t = acos(r);
                    if (   t> mint      && (hit_time == 0 || t < hit_time)){
                       hit_time=t;
                       cn=i;
                    }
                }
            }
        }
    }

}

double HmcSampler::_verifyConstraints(const VectorXd & b){
    double r =0;

    for (int i=0; i != quadraticConstraints.size(); i++ ){
        QuadraticConstraint qc = quadraticConstraints[i];
        double check = ((b.transpose())*(qc.A))*b + (qc.B).dot(b) + qc.C;
        if (i==0 || check < r) {
            r = check;
        }
    }

    for (int i=0; i != linearConstraints.size(); i++ ){
    LinearConstraint lc = linearConstraints[i];
    double check = (lc.f).dot(b) + lc.g;
    if (i==0 || check < r) {
        r = check;
    }
    }

    return r;
}

void HmcSampler::_updateTrace( VectorXd const & a,  VectorXd const & b, double const & t, MatrixXd & tracePoints){
    double const stepsize = .01;
    int steps = t/stepsize;

    int c = tracePoints.cols();
    tracePoints.conservativeResize(NoChange, c+steps+1);
    for (int i=0; i<steps; i++){
        VectorXd bb= sin(i*stepsize)*a + cos(i*stepsize)*b;
//      cout << bb.transpose() << endl;
        tracePoints.col(c+i) = bb;
    }
        VectorXd bb= sin(t)*a + cos(t)*b;
        tracePoints.col(c+steps) = bb;
}


#ifndef KANOOTH_NUMBERS_BOOST_INTEGER_HPP
#define KANOOTH_NUMBERS_BOOST_INTEGER_HPP

#include <boost/multiprecision/number.hpp>
#include <boost/multiprecision/detail/integer_ops.hpp>
#include <boost/multiprecision/detail/digits.hpp>
#include <boost/mpl/contains.hpp>
#include <boost/cstdint.hpp>
#include <limits>

#include <kanooth/numbers/integer_base.hpp>
#include <kanooth/numbers/natural_number.hpp>

namespace boost {
namespace multiprecision {
namespace backends {

template <typename N>
struct kanooth_integer;

} // namespace backends

template <typename N>
struct number_category<backends::kanooth_integer<N> > : public mpl::int_<number_kind_integer> {};

namespace backends {

template <typename N>
class kanooth_integer : public kanooth::numbers::integer_base<N>
{
public:
    // needed by front-end
    typedef mpl::list<long>          signed_types;
    typedef mpl::list<unsigned long> unsigned_types;
    typedef mpl::list<>              float_types;

    template <typename T, typename R>
    struct if_supported_int
        : public enable_if<mpl::or_<mpl::contains<unsigned_types, T>,
                                    mpl::contains<signed_types, T> >, R> {};

    kanooth_integer() : base_type() {}
    kanooth_integer(long v) : base_type(v) {}
    kanooth_integer(unsigned long v) : base_type(v) {}
    kanooth_integer(const char* s) : base_type(s) {}
private:
    typedef kanooth::numbers::integer_base<N> base_type;
};

template <typename N>
inline bool eval_is_zero(const kanooth_integer<N>& val)
{
    return val.is_zero();
}

template <typename N>
inline void eval_add(kanooth_integer<N>& r, const kanooth_integer<N>& a)
{
    r.add(r, a);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_add(kanooth_integer<N>& r, T a)
{
    r.add(r, a);
}

template <typename N>
inline void eval_add(kanooth_integer<N>& r, const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    r.add(a, b);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_add(kanooth_integer<N>& r, const kanooth_integer<N>& a, T b)
{
    r.add(a, b);
}

template <typename N>
inline void eval_subtract(kanooth_integer<N>& r, const kanooth_integer<N>& a)
{
    r.subtract(r, a);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_subtract(kanooth_integer<N>& r, T a)
{
    r.subtract(r, a);
}

template <typename N>
inline void eval_subtract(kanooth_integer<N>& r, const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    r.subtract(a, b);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_subtract(kanooth_integer<N>& r, const kanooth_integer<N>& a, T b)
{
    r.subtract(a, b);
}

template <typename N>
inline void eval_multiply(kanooth_integer<N>& r, const kanooth_integer<N>& a)
{
    r.multiply(r, a);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_multiply(kanooth_integer<N>& r, T a)
{
    r.multiply(r, a);
}

template <typename N>
inline void eval_multiply(kanooth_integer<N>& r, const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    r.multiply(a, b);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_multiply(kanooth_integer<N>& r, const kanooth_integer<N>& a, T b)
{
    r.multiply(a, b);
}

template <typename N>
inline void eval_divide(kanooth_integer<N>& r, const kanooth_integer<N>& a)
{
    r.divide_truncate(r, a);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_divide(kanooth_integer<N>& r, T a)
{
    r.divide_truncate(r, a);
}

template <typename N>
inline void eval_divide(kanooth_integer<N>& r, const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    r.divide_truncate(a, b);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_divide(kanooth_integer<N>& r, const kanooth_integer<N>& a, T b)
{
    r.divide_truncate(a, b);
}

template <typename N>
inline void eval_modulus(kanooth_integer<N>& r, const kanooth_integer<N>& a)
{
    r.modulus_truncate(r, a);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_modulus(kanooth_integer<N>& r, T a)
{
    r.modulus_truncate(r, a);
}

template <typename N>
inline void eval_modulus(kanooth_integer<N>& r, const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    r.modulus_truncate(a, b);
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, void>::type
eval_modulus(kanooth_integer<N>& r, const kanooth_integer<N>& a, T b)
{
    r.modulus_truncate(a, b);
}

template <typename N>
inline void eval_qr(const kanooth_integer<N>& x, const kanooth_integer<N>& y, kanooth_integer<N>& q, kanooth_integer<N>& r)
{
    kanooth_integer<N>::quotrem(q, r, x, y);
}

template <typename N>
inline void eval_gcd(kanooth_integer<N>& result, const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    result.gcd(a, b);
}

template <typename N, class Integer>
inline typename enable_if<is_unsigned<Integer>, Integer>::type eval_integer_modulus(const kanooth_integer<N>& x, Integer val)
{
    if ((sizeof(Integer) <= sizeof(long)) || (val <= (std::numeric_limits<unsigned long>::max)())) {
        return kanooth_integer<N>::integer_modulus(x, static_cast<unsigned long>(val));
    } else {
        return default_ops::eval_integer_modulus(x, val);
    }
}

template <typename N, class Integer>
inline typename enable_if<is_signed<Integer>, Integer>::type eval_integer_modulus(const kanooth_integer<N>& x, Integer val)
{
   typedef typename make_unsigned<Integer>::type unsigned_type;
   return eval_integer_modulus(x, static_cast<unsigned_type>(std::abs(val)));
}

template <typename N>
inline void eval_bitwise_and(kanooth_integer<N>& result, const kanooth_integer<N>& v)
{
   result.bitwise_and(result, v);
}

template <typename N>
inline void eval_bitwise_or(kanooth_integer<N>& result, const kanooth_integer<N>& v)
{
   result.bitwise_or(result, v);
}

template <typename N>
inline void eval_bitwise_xor(kanooth_integer<N>& result, const kanooth_integer<N>& v)
{
   result.bitwise_xor(result, v);
}

template <typename N>
inline void eval_bitwise_and(kanooth_integer<N>& result, const kanooth_integer<N>& u, const kanooth_integer<N>& v)
{
   result.bitwise_and(u, v);
}

template <typename N>
inline void eval_bitwise_or(kanooth_integer<N>& result, const kanooth_integer<N>& u, const kanooth_integer<N>& v)
{
   result.bitwise_or(u, v);
}

template <typename N>
inline void eval_bitwise_xor(kanooth_integer<N>& result, const kanooth_integer<N>& u, const kanooth_integer<N>& v)
{
   result.bitwise_xor(u, v);
}

template <typename N>
inline void eval_left_shift(kanooth_integer<N>& r, unsigned long v)
{
   r.left_shift(r, v);
}

template <typename N>
inline void eval_left_shift(kanooth_integer<N>& r, kanooth_integer<N>& u, unsigned long v)
{
   r.left_shift(u, v);
}

template <typename N>
inline void eval_right_shift(kanooth_integer<N>& r, unsigned long v)
{
   r.right_shift(r, v);
}

template <typename N>
inline void eval_right_shift(kanooth_integer<N>& r, kanooth_integer<N>& u, unsigned long v)
{
   r.right_shift(u, v);
}

template <typename N>
inline bool eval_bit_test(const kanooth_integer<N>& u, unsigned pos)
{
    return u.bit_test(pos);
}

template <typename N>
inline void eval_bit_set(kanooth_integer<N>& u, unsigned pos)
{
    u.bit_set(pos);
}

template <typename N>
inline void eval_bit_unset(kanooth_integer<N>& u, unsigned pos)
{
    u.bit_unset(pos);
}

template <typename N>
inline void eval_bit_flip(kanooth_integer<N>& u, unsigned pos)
{
    u.bit_flip(pos);
}

template <typename N>
inline bool eval_eq(const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    return a.compare(b) == 0;
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, bool>::type
eval_eq(const kanooth_integer<N>& a, T b)
{
    return a.compare(b) == 0;
}

template <typename N>
inline bool eval_lt(const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    return a.compare(b) < 0;
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, bool>::type
eval_lt(const kanooth_integer<N>& a, T b)
{
    return a.compare(b) < 0;
}

template <typename N>
inline bool eval_gt(const kanooth_integer<N>& a, const kanooth_integer<N>& b)
{
    return a.compare(b) > 0;
}

template <typename N, typename T>
inline typename kanooth_integer<N>::template if_supported_int<T, bool>::type
eval_gt(const kanooth_integer<N>& a, T b)
{
    return a.compare(b) > 0;
}

} // namespace backends
} // namespace multiprecision
} // namespace boost

namespace kanooth {
namespace numbers {

typedef boost::multiprecision::number<boost::multiprecision::backends::kanooth_integer<natural_number> > boost_integer;

} // namespace numbers
} // namespace kanooth

namespace std {

template <typename N, boost::multiprecision::expression_template_option ExpressionTemplates>
class numeric_limits<boost::multiprecision::number<boost::multiprecision::backends::kanooth_integer<N>, ExpressionTemplates> >
{
   typedef boost::multiprecision::number<boost::multiprecision::backends::kanooth_integer<N>, ExpressionTemplates> number_type;
public:
   BOOST_STATIC_CONSTEXPR bool is_specialized = true;
   //
   // Largest and smallest numbers are bounded only by available memory, set
   // to zero:
   //
   static number_type (min)() BOOST_NOEXCEPT
   {
      return number_type();
   }
   static number_type (max)() BOOST_NOEXCEPT
   {
      return number_type();
   }
   static number_type lowest() BOOST_NOEXCEPT { return (min)(); }
   BOOST_STATIC_CONSTEXPR int digits = INT_MAX;
   BOOST_STATIC_CONSTEXPR int digits10 = (INT_MAX / 1000) * 301L;
   BOOST_STATIC_CONSTEXPR int max_digits10 = digits10 + 2;
   BOOST_STATIC_CONSTEXPR bool is_signed = true;
   BOOST_STATIC_CONSTEXPR bool is_integer = true;
   BOOST_STATIC_CONSTEXPR bool is_exact = true;
   BOOST_STATIC_CONSTEXPR int radix = 2;
   static number_type epsilon() BOOST_NOEXCEPT { return number_type(); }
   static number_type round_error() BOOST_NOEXCEPT { return number_type(); }
   BOOST_STATIC_CONSTEXPR int min_exponent = 0;
   BOOST_STATIC_CONSTEXPR int min_exponent10 = 0;
   BOOST_STATIC_CONSTEXPR int max_exponent = 0;
   BOOST_STATIC_CONSTEXPR int max_exponent10 = 0;
   BOOST_STATIC_CONSTEXPR bool has_infinity = false;
   BOOST_STATIC_CONSTEXPR bool has_quiet_NaN = false;
   BOOST_STATIC_CONSTEXPR bool has_signaling_NaN = false;
   BOOST_STATIC_CONSTEXPR float_denorm_style has_denorm = denorm_absent;
   BOOST_STATIC_CONSTEXPR bool has_denorm_loss = false;
   static number_type infinity() BOOST_NOEXCEPT { return number_type(); }
   static number_type quiet_NaN() BOOST_NOEXCEPT { return number_type(); }
   static number_type signaling_NaN() BOOST_NOEXCEPT { return number_type(); }
   static number_type denorm_min() BOOST_NOEXCEPT { return number_type(); }
   BOOST_STATIC_CONSTEXPR bool is_iec559 = false;
   BOOST_STATIC_CONSTEXPR bool is_bounded = false;
   BOOST_STATIC_CONSTEXPR bool is_modulo = false;
   BOOST_STATIC_CONSTEXPR bool traps = false;
   BOOST_STATIC_CONSTEXPR bool tinyness_before = false;
   BOOST_STATIC_CONSTEXPR float_round_style round_style = round_toward_zero;
};

} // namespace std

#endif // KANOOTH_NUMBERS_BOOST_INTEGER_HPP


//==================================================================================================
/*!
  @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_INEARBYINT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_INEARBYINT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-arithmetic
    This function object computes the integer conversion of the @ref nearbyint of its parameter.

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

    @par Note:
    This operation is NOT properly saturated.
    To ensure proper saturation use the decorartor pedantic_

    @par Decorators:

   - pedantic_ if you need a full range correctness
     use this decorator that computes  saturated_(toint)(nearbyint(x));

    @see nearbyint, ifloor, iround, ifix, iceil

    @par Example:

      @snippet inearbyint.cpp inearbyint

    @par Possible output:

      @snippet inearbyint.txt inearbyint

  **/
  as_integer_t<Value> inearbyint(Value const& x);
} }
#endif

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

#endif


#include <boost/random.hpp>
#include <openssl/rand.h>
#include "irods/irods_random.hpp"
#include <ctime>

void irods::getRandomBytes( void * buf, int bytes ) {
    if ( RAND_bytes( ( unsigned char * )buf, bytes ) != 1 ) {
        static boost::mt19937 generator( std::time( 0 ) ^ ( getpid() << 16 ) );
        static boost::uniform_int<unsigned char> byte_range( 0, 0xff );
        static boost::variate_generator<boost::mt19937, boost::uniform_int<unsigned char> > random_byte( generator, byte_range );
        for ( int i = 0; i < bytes; i++ ) {
            ( ( unsigned char * ) buf )[i] = random_byte();
        }
    }
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// 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 "graph.hh"
#include "graph_filtering.hh"
#include "graph_properties.hh"
#include "graph_selectors.hh"
#include "graph_util.hh"

#include "random.hh"

#include <boost/python.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct do_random_matching
{
    template <class Graph, class VertexIndex, class WeightMap, class MatchMap,
              class RNG>
    void operator()(const Graph& g, VertexIndex vertex_index, WeightMap weight,
                    MatchMap match, bool minimize, RNG& rng) const
    {
        typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
        typedef typename graph_traits<Graph>::edge_descriptor edge_t;
        typedef typename property_traits<WeightMap>::value_type wval_t;

        vector<vertex_t> vlist;
        typename graph_traits<Graph>::vertex_iterator v, v_end;
        for (tie(v, v_end) = vertices(g); v != v_end; ++v)
            vlist.push_back(*v);

        unchecked_vector_property_map<uint8_t, VertexIndex>
            matched(vertex_index, num_vertices(g));

        typedef random_permutation_iterator<typename vector<vertex_t>::iterator,
                                            RNG>
            random_vertex_iter;
        random_vertex_iter vr(vlist.begin(), vlist.end(), rng),
            vr_end(vlist.end(), vlist.end(), rng);
        for (; vr != vr_end; ++vr)
        {
            vertex_t v = *vr;
            if (matched[v])
                continue;
            wval_t min_w = minimize ? numeric_limits<wval_t>::max() :
                numeric_limits<wval_t>::min() ;
            vector<edge_t> candidates;
            typename graph_traits<Graph>::out_edge_iterator e, e_end;
            for(tie(e, e_end) = out_edges(v, g); e != e_end; ++e)
            {
                vertex_t w = target(*e, g);
                if (matched[w])
                    continue;
                if ((minimize && (weight[*e] < min_w)) ||
                    (!minimize && (weight[*e] > min_w)))
                {
                    min_w = weight[*e];
                    candidates.clear();
                }
                if (weight[*e] == min_w)
                    candidates.push_back(*e);
            }

            if (!candidates.empty())
            {
                uniform_int_distribution<> sample(0, candidates.size() - 1);
                size_t j = sample(rng);
                match[candidates[j]] = true;
                matched[v] = true;
                matched[target(candidates[j], g)] = true;
            }
        }
    }
};

void random_matching(GraphInterface& gi, boost::any weight, boost::any match,
                     bool minimize, rng_t& rng)
{
    typedef ConstantPropertyMap<int32_t,GraphInterface::edge_t> weight_map_t;
    typedef mpl::push_back<edge_scalar_properties, weight_map_t>::type
        edge_props_t;

    if(weight.empty())
        weight = weight_map_t(1);

    run_action<>()
        (gi, std::bind(do_random_matching(), placeholders::_1, gi.GetVertexIndex(),
                       placeholders::_2, placeholders::_3, minimize, std::ref(rng)),
         edge_props_t(), writable_edge_scalar_properties())(weight, match);
}

void export_random_matching()
{
    python::def("random_matching", &random_matching);
}


#include <iostream>
#include <stdlib.h>
#include <caffe/caffe.hpp>
#include <Eigen/Eigen/Dense>

// opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/eigen.hpp>
#include <algorithm>
#include <iosfwd>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <boost/lexical_cast.hpp>

using namespace caffe;
using namespace std;
using std::string;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::RowVectorXd;

/*************************************************/
class Classifier {
 public:
  Classifier(const string& model_file,
             const string& trained_file,
             const string& limit_file,
             const int output_rows);
  std::vector<float> Predict(const cv::Mat& img);
  private:
  void WrapInputLayer(std::vector<cv::Mat>* input_channels);
  void Preprocess(const cv::Mat& img, std::vector<cv::Mat>* input_channels);
  std::vector<float> Postprocess(std::vector<float> raw_prediction);
 public:
 	std::vector<float> encoded_data_;
 	int num_outputs_;
  cv::Size output_geometry_;
 private:
  shared_ptr<Net<float> > net_;
  cv::Size input_geometry_;
  int num_channels_;
  std::vector<string> labels_;
  std::vector<float> min_;
  std::vector<float> max_;
  };
/*************************************************/
Classifier::Classifier(const string& model_file,
                       const string& trained_file,
                       const string& limit_file,
                       const int output_rows)
{
	Caffe::set_mode(Caffe::CPU);
	/* Load the network. */
  net_.reset(new Net<float>(model_file, TEST));
  net_->CopyTrainedLayersFrom(trained_file);

  num_outputs_ = net_ ->num_outputs();
	//cout << "net inputs x outputs dimension: " << net_ ->num_inputs() << "x" << num_outputs_ << endl;

  Blob<float>* input_layer = net_->input_blobs()[0];
  num_channels_ = input_layer->channels();

  //cout << "net input layer (channels x width x height): " << num_channels_ <<"x"<<input_layer->width()<<"x"<<input_layer->height() << endl;

  CHECK(num_channels_ == 3 || num_channels_ == 1)
    << "Input layer should have 1 or 3 channels.";
  input_geometry_ = cv::Size(input_layer->width(), input_layer->height());

  Blob<float>* output_layer = net_->output_blobs()[0];
  //cout << "net output layer (channels x width x height): " << output_layer->channels()<< "x" << output_layer->width() << "x" << output_layer->height() << endl;
  output_geometry_ = cv::Size(output_layer->channels()/output_rows,output_rows);
  //cout << "output geometry: " <<  output_geometry_.height << " x " << output_geometry_.width << endl;

  ifstream file;
  file.open(limit_file.c_str());
  string line;
  if (file.is_open())
  {
    while(std::getline(file, line))
    {
        min_.push_back(boost::lexical_cast<float>(line));
        std::getline(file, line);
        max_.push_back(boost::lexical_cast<float>(line));
    }
  }
  else
  {
    std::cout << "Error - could not open limit_file" << endl;
  }
  file.close();
}

std::vector<float> Classifier::Predict(const cv::Mat& img) {
  Blob<float>* input_layer = net_->input_blobs()[0];
  input_layer->Reshape(1, num_channels_,
                       input_geometry_.height, input_geometry_.width);
  /* Forward dimension change to all layers. */
  net_->Reshape();

  std::vector<cv::Mat> input_channels;
  WrapInputLayer(&input_channels);

  Preprocess(img, &input_channels);

  net_->Forward();

  /* Copy the encoded output layer to a std::vector */
  if (num_outputs_ > 1)
  {
  	Blob<float>* output_layer2 = net_->output_blobs()[1];
  	const float* begin2 = output_layer2->cpu_data();
  	const float* end2 = begin2 + output_layer2->channels();
		encoded_data_ = std::vector<float>(begin2, end2);
	}
  /* Copy the output layer to a std::vector */
  Blob<float>* output_layer = net_->output_blobs()[0];
  const float* begin = output_layer->cpu_data();
  const float* end = begin + output_layer->channels();

  std::vector<float> predictions = Postprocess(std::vector<float>(begin, end));
  return predictions;
}

/* Wrap the input layer of the network in separate cv::Mat objects
 * (one per channel). This way we save one memcpy operation and we
 * don't need to rely on cudaMemcpy2D. The last preprocessing
 * operation will write the separate channels directly to the input
 * layer. */
void Classifier::WrapInputLayer(std::vector<cv::Mat>* input_channels) {
  Blob<float>* input_layer = net_->input_blobs()[0];

  int width = input_layer->width();
  int height = input_layer->height();
  float* input_data = input_layer->mutable_cpu_data();
  for (int i = 0; i < input_layer->channels(); ++i) {
    cv::Mat channel(height, width, CV_32FC1, input_data);
    input_channels->push_back(channel);
    input_data += width * height;
  }
}

void Classifier::Preprocess(const cv::Mat& img,
                            std::vector<cv::Mat>* input_channels) {
  /* Convert the input image to the input image format of the network. */
  //cout << "image channels: " << img.channels() << endl;
  cv::Mat sample;
  if (img.channels() == 3 && num_channels_ == 1)
    cv::cvtColor(img, sample, cv::COLOR_BGR2GRAY);
  else if (img.channels() == 4 && num_channels_ == 1)
    cv::cvtColor(img, sample, cv::COLOR_BGRA2GRAY);
  else if (img.channels() == 4 && num_channels_ == 3)
    cv::cvtColor(img, sample, cv::COLOR_BGRA2BGR);
  else if (img.channels() == 1 && num_channels_ == 3)
    cv::cvtColor(img, sample, cv::COLOR_GRAY2BGR);
  else
    sample = img;

	//cout << "sample channels: " << sample.channels() << endl;
  cv::Mat sample_resized;
  if (sample.size() != input_geometry_)
  {
    cout << "input does not match the network geometry" << endl;
    cv::resize(sample, sample_resized, input_geometry_);
  }
  else
    sample_resized = sample;

  cv::Mat sample_float;
  if (num_channels_ == 3)
    sample_resized.convertTo(sample_float, CV_32FC3);
  else
    sample_resized.convertTo(sample_float, CV_32FC1);

  cv::Mat sample_normalized;
  //sample_normalized = (1.0 / 256.0) * sample_float;
  sample_normalized = (1.0) * sample_float;

  //cout << sample_normalized << endl;

  //cv::subtract(sample_float, mean_, sample_normalized);

  /* This operation will write the separate BGR planes directly to the
   * input layer of the network because it is wrapped by the cv::Mat
   * objects in input_channels. */

  cv::split(sample_normalized, *input_channels);

  CHECK(reinterpret_cast<float*>(input_channels->at(0).data)
        == net_->input_blobs()[0]->cpu_data())
    << "Input channels are not wrapping the input layer of the network.";

}

std::vector<float> Classifier::Postprocess(std::vector<float> raw_prediction)
{
  std::vector<float> predictions;
  int k;
  for (int j = 0; j < output_geometry_.height; j++)
  {
    for(int i = 0; i < output_geometry_.width; i++)
    {
      k = i + output_geometry_.width;
      float scaled_data;
      scaled_data = raw_prediction[i + output_geometry_.width*j] * (max_[k] - min_[k]) + min_[k];
      predictions.push_back(scaled_data);
    }
  }
  return predictions;
}

/*************************************************/
void Encode(const string& filename_root, Classifier& classifier, int n_files );
vector<vector<int> > GetIndecisFromFile(const string& filename);
MatrixXd Reconstruct(cv::Mat img, Classifier& classifier);
void WriteMatrixToFile(string fileName, MatrixXd data);
MatrixXd ReadMatrixFromFile(string fileName);
void ReconstructActionGroups(Classifier& classifier);
void MakeDescriminatorDatabase(Classifier& classifier);
void Encode2ImageFile(Classifier& classifier, const string& input_image_filename, const string & output_image_filename);
void Random2ImageFile(int dim, const string & output_image_filename);
void CreateTextFileName(const string & output_filename,const string & data_root_file, const int n_neg_samples, const int n_pos_samples);
void calcReconstructError(Classifier& classifier, string data_file, int n_data);
void Decoder(Classifier& classifier, MatrixXd data);

int main(int argc, char** argv)
{
	std::cout << "Caffe action_autoencoder is started" << std::endl;
  srand (time(NULL));
	::google::InitGoogleLogging(argv[0]);
  const int action_rows = 20;

  const bool calc_reconstruct_err = false;
  const bool decode_input_data = true;
  const bool encode_image_data = false;

  string model_file;

  if (calc_reconstruct_err || encode_image_data)
    model_file =  "models/action_vae/train.prototxt";
  else if (decode_input_data)
    model_file =  "models/action_vae/decoder.prototxt";

  const string trained_file = "models/action_vae/snapshot_reach/vae_iter_44534.caffemodel";
  const string limit_file = "data/reach_action/data_limits.txt";
  const string actimg_file = "data/reach_action/img";
  const string data_file = "data/reach_action";
  /*
  const string trained_file = "models/action_vae/snapshot_throw/_iter_32329.caffemodel";
  const string limit_file = "data/throw_action/data_limits.txt";
  const string actimg_file = "data/throw_action/img";
  const string data_file = "data/throw_action";
  */

  Classifier classifier(model_file, trained_file, limit_file, action_rows);
  if (calc_reconstruct_err)
    calcReconstructError(classifier, data_file, 5501);
  else if(decode_input_data)
  {
    MatrixXd enc_samples = ReadMatrixFromFile("actions.txt");
    cout << enc_samples << endl;
    Decoder(classifier, enc_samples);
  }
  else if(encode_image_data)
    Encode(actimg_file, classifier, 5501 );

	return 0;
}

// Decode the encoded 'data'
// each row of data corresponds to one encoded sample
void Decoder(Classifier& classifier, MatrixXd data)
{
  int n_samples = data.rows();
  cv::Mat m;
  MatrixXd rowMat;
  char save_filename[100];
  for(int i = 0; i < n_samples; i++)
  {
    rowMat = data.block(i,0,1,data.cols());
    eigen2cv(rowMat, m);
    //cout << m << endl;
    MatrixXd cmd = Reconstruct(m, classifier);
    //cout << cmd << endl << endl << endl;
    sprintf(save_filename, "commands/%04d.txt", i);
    WriteMatrixToFile(save_filename, cmd);
  }
}

void ReconstructActionGroups(Classifier& classifier)
{
  const string ind_filename = "results/reconst_actions/ind.txt";
  const string action_rootfile = "data/toss_action/image0/";
  const string result_rootfile = "results/reconst_actions/";
  char img_filename[100];
  char res_filename[100];
  vector< vector < int > > indecis =  GetIndecisFromFile(ind_filename);

  for(int i = 0; i < indecis.size(); i++)
  {
    for(int j = 0; j < indecis[i].size(); j++)
    {
      sprintf(img_filename, "%sact_img%04d.jpg", action_rootfile.c_str(), (indecis[i][j] - 1));
      cv::Mat img = cv::imread(img_filename, -1);
      CHECK(!img.empty()) << "Unable to decode image " << img_filename;
      MatrixXd reconst_action = Reconstruct(img, classifier);
      sprintf(res_filename, "%s%d/%04d.txt", result_rootfile.c_str(), i+1, j);
      WriteMatrixToFile(res_filename, reconst_action);
    }
  }
}

// This function calculates reconstruction error for all data in a folder
void calcReconstructError(Classifier& classifier, string data_file, int n_data)
{
  int cols = classifier.output_geometry_.width;
  int rows = classifier.output_geometry_.height;
  char file[100];
  MatrixXd rec_act, org_act;
  RowVectorXd err = RowVectorXd::Zero(cols);
  RowVectorXd max_err = RowVectorXd::Zero(cols);
  for(int n = 0; n < n_data; n++)
  {
    // finding the original action
    sprintf(file, "%s/raw/act_log%04d.txt", data_file.c_str(), n);
    org_act = ReadMatrixFromFile(file);
    org_act = org_act.block(0,7,20,7);
    // reconstructing the action
    sprintf(file, "%s/img/act_img%04d.png", data_file.c_str(), n);
    cv::Mat img = cv::imread(file, -1);
    CHECK(!img.empty()) << "Unable to decode image " << file;
    rec_act = Reconstruct(img, classifier);
    double col_err;
    for(int j = 0; j < cols; j++)
    {
      VectorXd diff = org_act.col(j) - rec_act.col(j);
      col_err = diff.cwiseAbs().sum()/rows;
      err(j) += col_err;
      if ( col_err > max_err(j) )
        max_err(j) = col_err;
    }
  }
  err = err / n_data;
  cout << err <<endl;
  cout << max_err <<endl;
}

MatrixXd Reconstruct(cv::Mat img, Classifier& classifier)
{
  int cols = classifier.output_geometry_.width;
  int rows = classifier.output_geometry_.height;

  MatrixXd reconst_out(rows, cols);
  std::vector<float> pred = classifier.Predict(img);

  for(int i = 0; i < rows; i++)
    for(int j = 0; j < cols; j++)
      reconst_out(i,j) = pred[j + i*cols];

  return reconst_out;
}

// This function opens act_imgs in a directory and save the encoded space
// into a file
void Encode(const string& filename_root, Classifier& classifier, int n_files )
{
  char filename[100];
  ofstream file_writer;
  file_writer.open("enc_data.txt");
  if(file_writer.is_open())
  {
    file_writer << n_files << endl;
    for(int i = 0; i < n_files; i++)
    {
      sprintf(filename, "%s/act_img%04d.png",filename_root.c_str(), i);
      cv::Mat img = cv::imread(filename, -1);
      CHECK(!img.empty()) << "Unable to decode image " << filename;
      classifier.Predict(img);
      // displaying the encoded data
      cout << "i = " << i << " enc_data = " ;
      for (int j = 0; j < classifier.encoded_data_.size(); j++)
        cout << classifier.encoded_data_[j]<< "\t\t\t";
      cout << endl;
      // saving the data
      for (int j = 0; j < classifier.encoded_data_.size(); j++)
        file_writer << classifier.encoded_data_[j] << endl;
    }
    file_writer.close();
  }
  else
    cout << "Error - the file_writer could not be opened. " << endl;
}

void MakeDescriminatorDatabase(Classifier& classifier)
{
  char in_filename[100];
  char out_filename[100];
  const string act_img_root_filename = "data/toss_action/image0";
  const string enc_img_root_filename = "data/enc_toss_action/images";
  const string labels_filename = "data/enc_toss_action/train.txt";
  const int n_pos_samples = 2200;      // randomly distributed numbers
  const int n_neg_samples = 2200;      // encoded data
  const int dim_enc_space = 3;

  // generating the encoded data
  for(int i = 0; i < n_neg_samples; i++)
  {
    sprintf(in_filename, "%s/act_img%04d.jpg",act_img_root_filename.c_str(), i);
    sprintf(out_filename, "%s/enc%04d.PNG",enc_img_root_filename.c_str(), i);
    Encode2ImageFile(classifier, in_filename, out_filename);
  }
  // generating random data
  for(int i = 0; i < n_pos_samples; i++)
  {
    sprintf(out_filename, "%s/enc%04d.PNG",enc_img_root_filename.c_str(), i+n_neg_samples);
    Random2ImageFile(dim_enc_space, out_filename);
  }
  // saving the text file indicating filenames and labels
  CreateTextFileName(labels_filename, enc_img_root_filename, n_neg_samples, n_pos_samples);
}

void Encode2ImageFile(Classifier& classifier, const string& input_image_filename, const string & output_image_filename)
{
  cv::Mat input_image = cv::imread(input_image_filename, -1);
  CHECK(!input_image.empty()) << "Unable to decode image " << input_image_filename;
  classifier.Predict(input_image);
  cv::Mat enc_image = cv::Mat(classifier.encoded_data_);
  enc_image.convertTo(enc_image, CV_8UC1, 255.0);
	imwrite(output_image_filename, enc_image);
}
void Random2ImageFile(int dim, const string & output_image_filename)
{
  std::vector<float> v;
  float r;
  for (int i = 0; i < dim; i++)
  {
    r = (rand()%1000+1)/1000.0;
    v.push_back(r);
  }
  cv::Mat rand_image = cv::Mat(v);
  rand_image.convertTo(rand_image, CV_8UC1, 255.0);
	imwrite(output_image_filename, rand_image);
}
void CreateTextFileName(const string & output_filename,const string & data_root_file, const int n_neg_samples, const int n_pos_samples)
{
  ofstream file_writer;
  file_writer.open(output_filename.c_str());
  char line[100];
  for(int i = 0; i < n_neg_samples; i++)
  {
    sprintf(line, "%s/enc%04d.PNG 0\n",data_root_file.c_str(), i);
    file_writer << line;
  }
  for(int i = 0; i < n_pos_samples; i++)
  {
    sprintf(line, "%s/enc%04d.PNG 1\n",data_root_file.c_str(), i + n_neg_samples);
    file_writer << line;
  }
  file_writer.close();
}

vector<vector<int> > GetIndecisFromFile(const string& filename)
{
  vector<vector<int> > indecis;
  ifstream fileReader;
  fileReader.open(filename.c_str());

  string line;
  int n_data;
  int data;
  if (fileReader.is_open())
  {
    while(getline(fileReader, line))
    {
      vector<int> row;
      n_data = boost::lexical_cast<int>(line);
      for(int i=0; i < n_data; i++)
      {
        getline(fileReader, line);
        data = boost::lexical_cast<int>(line);
        row.push_back(data);
      }
      indecis.push_back(row);
    }
  }
  else
    cout << "Error - could not open the file " << filename << endl;
  return indecis;
}

void WriteMatrixToFile(string fileName, MatrixXd data)
{
    int nRow = data.rows();
    int nCol = data.cols();
    ofstream fileWriter;
    fileWriter.open(fileName.c_str());

    fileWriter << nRow << endl;
    fileWriter << nCol << endl;

    for(int i = 0; i < nRow; i++)
        {
            for(int j = 0; j < nCol; j++)
                {
                    fileWriter << data(i,j) << endl;
                }
        }

    fileWriter.close();
}
MatrixXd ReadMatrixFromFile(string fileName)
{
    MatrixXd data;
    ifstream fileReader;
    fileReader.open(fileName.c_str());
    int nRow, nCol;
    string line;

    if (fileReader.is_open())
        {

            getline(fileReader, line);
            nRow = boost::lexical_cast<int>(line);
            getline(fileReader, line);
            nCol = boost::lexical_cast<int>(line);
            data.resize(nRow, nCol);
            for(int i = 0; i < nRow; i++)
                {
                    for(int j = 0; j < nCol; j++)
                        {
                            getline(fileReader, line);
                            data(i,j) = boost::lexical_cast<double>(line);
                        }
                }
        }

    fileReader.close();
    return data;
}