/********************************************************************************
 *
 * This file is part of the Geneva library collection. The following license
 * applies to this file:
 *
 * ------------------------------------------------------------------------------
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 * ------------------------------------------------------------------------------
 *
 * Note that other files in the Geneva library collection may use a different
 * license. Please see the licensing information in each file.
 *
 ********************************************************************************
 *
 * Geneva was started by Dr. Rüdiger Berlich and was later maintained together
 * with Dr. Ariel Garcia under the auspices of Gemfony scientific. For further
 * information on Gemfony scientific, see http://www.gemfomy.eu .
 *
 * The majority of files in Geneva was released under the Apache license v2.0
 * in February 2020.
 *
 * See the NOTICE file in the top-level directory of the Geneva library
 * collection for a list of contributors and copyright information.
 *
 ********************************************************************************/

#pragma once

// Global checks, defines and includes needed for all of Geneva
#include "common/GGlobalDefines.hpp"

// Standard header files go here
#include <iostream>
#include <cmath>
#include <cstdlib>
#include <sstream>
#include <vector>
#include <list>
#include <algorithm> // for std::sort
#include <tuple>

// Boost header files go here
#include <boost/filesystem/operations.hpp>
#include <boost/algorithm/string/trim.hpp>
#include <boost/cast.hpp>

// Geneva header files go here
#include "geneva/GParameterSet.hpp"
#include "geneva/GDoubleCollection.hpp"
#include "geneva/GDoubleObject.hpp"
#include "geneva/GDoubleGaussAdaptor.hpp"
#include "geneva/GConstrainedDoubleCollection.hpp"
#include "geneva/GConstrainedDoubleObjectCollection.hpp"
#include "geneva/GConstrainedDoubleObject.hpp"
#include "geneva/GOptimizationEnums.hpp"
#include "geneva/G_OptimizationAlgorithm_GradientDescent_PersonalityTraits.hpp"
#include "geneva/G_OptimizationAlgorithm_SwarmAlgorithm_PersonalityTraits.hpp"
#include "common/GExceptions.hpp"
#include "common/GCommonEnums.hpp"
#include "common/GFactoryT.hpp"

namespace Gem {
namespace Geneva {

/******************************************************************************/
/**
 * This individual takes a vector of 2D double-tuples and calculates the
 * root-square deviation from the line defined by its two parameters
 */
class GLineFitIndividual
	: public Gem::Geneva::GParameterSet {
	 ///////////////////////////////////////////////////////////////////////
	 friend class boost::serialization::access;

	 template<typename Archive>
	 void serialize(Archive &ar, const unsigned int) {
		 using boost::serialization::make_nvp;

		 ar
		 & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GParameterSet)
		 & BOOST_SERIALIZATION_NVP(dataPoints_);
	 }
	 ///////////////////////////////////////////////////////////////////////

public:
	 /** @brief The default constructor */
	 G_API_INDIVIDUALS GLineFitIndividual(const std::vector<std::tuple<double, double>> &);
	 /** @brief The copy constructor */
	 G_API_INDIVIDUALS GLineFitIndividual(const GLineFitIndividual &);

	 /** @brief The standard destructor */
	 virtual G_API_INDIVIDUALS ~GLineFitIndividual();

	 /** @brief Retrieves the tuple (a,b) of the line represented by this object */
	 G_API_INDIVIDUALS std::tuple<double, double> getLine() const;

protected:
	 /** @brief Loads the data of another GLineFitIndividual */
	 virtual G_API_INDIVIDUALS void load_(const GObject *) final;

	/** @brief Allow access to this classes compare_ function */
	friend void Gem::Common::compare_base_t<GLineFitIndividual>(
		GLineFitIndividual const &
		, GLineFitIndividual const &
		, Gem::Common::GToken &
	);

	/** @brief Searches for compliance with expectations with respect to another object of the same type */
	virtual G_API_INDIVIDUALS void compare_(
		const GObject & // the other object
		, const Gem::Common::expectation & // the expectation for this object, e.g. equality
		, const double & // the limit for allowed deviations of floating point types
	) const final;

	 /** @brief The actual fitness calculation takes place here. */
	 virtual G_API_INDIVIDUALS double fitnessCalculation() final;

	 /** @brief Applies modifications to this object. */
	 virtual G_API_INDIVIDUALS bool modify_GUnitTests_();
	 /** @brief Performs self tests that are expected to succeed. */
	 virtual G_API_INDIVIDUALS void specificTestsNoFailureExpected_GUnitTests_();
	 /** @brief Performs self tests that are expected to fail. */
	 virtual G_API_INDIVIDUALS void specificTestsFailuresExpected_GUnitTests_();

private:
	 /** @brief Creates a deep clone of this object */
	 virtual G_API_INDIVIDUALS GObject *clone_() const final;

	 /** @brief The default constructor -- private, as it is only needed for (de-)serialization purposes */
	 G_API_INDIVIDUALS GLineFitIndividual();

	 std::vector<std::tuple<double, double>> dataPoints_; ///< Holds the data points used for the fit procedure
};

/******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
/******************************************************************************/
/**
 * A factory for GLineFitIndividual objects
 */
class GLineFitIndividualFactory
	: public Gem::Common::GFactoryT<GParameterSet> {
public:
	 /** @brief The standard constructor */
	 G_API_INDIVIDUALS GLineFitIndividualFactory(
		 const std::vector<std::tuple<double, double>> &
		 , boost::filesystem::path const &
	 );

	 /** @brief The destructor */
	 virtual G_API_INDIVIDUALS ~GLineFitIndividualFactory();

protected:
	 /** @brief Allows to describe local configuration options in derived classes */
	 virtual G_API_INDIVIDUALS void describeLocalOptions_(Gem::Common::GParserBuilder &);

	 /** @brief Allows to act on the configuration options received from the configuration file */
	 virtual G_API_INDIVIDUALS void postProcess_(std::shared_ptr <GParameterSet> &);

private:
	 /** @brief The default constructor. Only needed for (de-)serialization purposes */
	 GLineFitIndividualFactory() = default;

	 /** @brief Creates individuals of this type */
	 virtual G_API_INDIVIDUALS std::shared_ptr <GParameterSet> getObject_(Gem::Common::GParserBuilder &,
																		 const std::size_t &);

	 std::vector<std::tuple<double, double>> dataPoints_; ///< Holds data points for the fit
};

/******************************************************************************/
////////////////////////////////////////////////////////////////////////////////
/******************************************************************************/

} /* namespace Geneva */
} /* namespace Gem */

BOOST_CLASS_EXPORT_KEY(Gem::Geneva::GLineFitIndividual)


#include "CustomerAssignment.h"
#include <boost/graph/connected_components.hpp>
#include <iostream>

CustomerAssignment::CustomerAssignment(std::size_t num_customers)
    : g_(num_customers),
      tables_(num_customers, 0),
      n_tables_(num_customers)
{
    for (auto vit = boost::vertices(g_); vit.first != vit.second; ++vit.first)
        boost::add_edge(*vit.first, *vit.first, g_);
    tables_ = std::vector<std::size_t>( boost::num_vertices(g_) );
    n_tables_ = boost::connected_components(g_, &tables_[0]);
}

void CustomerAssignment::print_tables(std::ostream& os) const
{
    for (std::size_t t = 0; t < n_tables_; ++t)
    {
        auto vit = vertices(g_);
        do
        {
            if (is_in_table(*vit.first, t))
            {
                os << *vit.first;
                ++vit.first;
                if ( vit.first != vit.second)
                    os << ", ";
            }
            else
            {
                ++vit.first;
            }
        }
        while ( vit.first != vit.second);

        os << std::endl;
    }
}

void CustomerAssignment::unlink(std::size_t source)
{
    boost::remove_edge_if([source,this](Edge e){ return boost::source(e, g_) == source; }, g_);

    const std::size_t c_table = tables_[source];
    ComponentGraph f(g_,
                     [source, c_table, this](Edge e){ return tables_[boost::source(e, g_)] == c_table; },
                     [c_table, this](Vertex v){ return tables_[v] == c_table; });

    std::vector<std::size_t> newtables( boost::num_vertices(g_), 0 );
    std::size_t n = boost::connected_components(f, &newtables[0] );
    if (n > 1)
    {
        // newtables is filled from 0 to how many vertices left in filtered graph,
        // while we must assign to table according to the original graphs' vertex indices.
        for (auto fit = vertices(f); fit.first != fit.second; ++fit.first)
        {
            if ( newtables[*fit.first] == 1 )
                tables_[*fit.first] = n_tables_;
        }
        ++n_tables_;
    }
}

void CustomerAssignment::link(std::size_t source, std::size_t target)
{
    boost::add_edge(source, target, g_);
    std::size_t k(0), l(0);
    if (joins_tables(source, target, k, l))
    {
        // assign to minimal table, deduct others to keep numbering consistent
        const std::size_t tnew = std::min(k, l);
        const std::size_t tmax = std::max(k, l);
        for (std::size_t i = 0; i < tables_.size(); ++i)
        {
            if ((tables_[i] == k) || (tables_[i] == l))
            {
                tables_[i] = tnew;
            }
            else if ( tables_[i] > tmax )
            {
                --tables_[i];
            }
        }
        --n_tables_;
    }
}

bool CustomerAssignment::joins_tables(std::size_t source,
                                      std::size_t target,
                                      std::size_t& k,
                                      std::size_t& l) const
{
    k = tables_[source];
    l = tables_[target];
    return (k != l);
}

bool CustomerAssignment::is_in_table(std::size_t customer, std::size_t table) const
{
    return (tables_[customer] == table);
}

std::size_t CustomerAssignment::num_customers() const
{
    return boost::num_vertices(g_);
}

std::set<std::size_t> CustomerAssignment::get_table_members(std::size_t table) const
{
    std::set<std::size_t> v;
    for (std::size_t c = 0; c < num_customers(); ++c)
    {
        if (is_in_table(c, table))
            v.insert(c);
    }
    return v;
}

std::size_t CustomerAssignment::num_tables() const
{
    return n_tables_;
}

std::size_t CustomerAssignment::get_table(std::size_t customer) const
{
    return tables_[customer];
}


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

#pragma once

#include <array>
#include <utility>

#include <Eigen/Core>
#include <rclcpp/duration.hpp>
/**
 * An example showing how to generate a joint pose motion to a goal position. Adapted from:
 * Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots
 * (Kogan Page Science Paper edition).
 */
class MotionGenerator {
 public:
  using Vector7d = Eigen::Matrix<double, 7, 1>;
  /**
   * Creates a new MotionGenerator instance for a target q.
   *
   * @param[in] speed_factor General speed factor in range (0, 1].
   * @param[in] q_start Start joint positions.
   * @param[in] q_goal Target joint positions.
   */
  MotionGenerator(double speed_factor, const Vector7d& q_start, const Vector7d& q_goal);

  /**
   * Sends joint position calculations
   *
   * @param[in] robot_state Current state of the robot.
   * @param[in] trajectory_time Amount of time, that has passed since the start of the trajectory.
   *
   * @return Joint positions to use inside a control loop and a boolean indicating whether the
   * motion is finished.
   */
  std::pair<Vector7d, bool> getDesiredJointPositions(const rclcpp::Duration& trajectory_time);

 private:
  using Vector7i = Eigen::Matrix<int, 7, 1>;

  bool calculateDesiredValues(double t, Vector7d* delta_q_d) const;
  void calculateSynchronizedValues();

  static constexpr double kDeltaQMotionFinished = 1e-6;
  static const int kJoints = 7;

  Vector7d q_start_;
  Vector7d delta_q_;

  Vector7d dq_max_sync_;
  Vector7d t_1_sync_;
  Vector7d t_2_sync_;
  Vector7d t_f_sync_;
  Vector7d q_1_;

  double time_ = 0.0;

  Vector7d dq_max_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished();  // in m/s
  Vector7d ddq_max_start_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();         // in m/s^2
  Vector7d ddq_max_goal_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();          // in m/s^2
};


#pragma once

#include <functional>
#include <string>
#include <utility>
#include <variant>
#include <memory>

#include "dynamicMatrix.hpp"

#include <Eigen/Dense>

namespace op
{

class Affine;

namespace internal
{

struct AffineTerm;
struct AffineSum;

class ParameterSource
{
public:
    ParameterSource() = default;
    explicit ParameterSource(const double const_value);
    explicit ParameterSource(double *value_ptr);
    explicit ParameterSource(const std::function<double()> &callback);
    double get_value() const;
    bool is_constant() const;
    bool is_pointer() const;
    bool is_callback() const;
    bool is_zero() const;
    bool is_one() const;

    ParameterSource operator+(const ParameterSource &other) const;
    ParameterSource operator-(const ParameterSource &other) const;
    ParameterSource operator*(const ParameterSource &other) const;
    ParameterSource operator/(const ParameterSource &other) const;
    operator AffineTerm() const;
    operator AffineSum() const;

private:
    using source_variant_t = std::variant<double,
                                          const double *,
                                          std::function<double()>>;
    source_variant_t source;
};

} // namespace internal

class Parameter : public DynamicMatrix<internal::ParameterSource, Parameter>
{
public:
    using DynamicMatrix<internal::ParameterSource, Parameter>::DynamicMatrix;
    explicit Parameter(const double const_value);
    explicit Parameter(double *value_ptr);
    explicit Parameter(const std::function<double()> &callback);

    template <typename Derived>
    explicit Parameter(const Eigen::DenseBase<Derived> &matrix);
    template <typename Derived>
    explicit Parameter(Eigen::DenseBase<Derived> *matrix);

    Parameter operator+(const Parameter &other) const;
    Parameter operator-() const;
    Parameter operator-(const Parameter &other) const;
    Parameter operator*(const Parameter &other) const;
    Parameter operator/(const Parameter &other) const;
    Affine cwiseProduct(const Affine &affine) const;
    double get_value(const size_t row = 0,
                     const size_t col = 0) const;
    Eigen::MatrixXd get_values() const;

    operator Affine() const;
};

template <typename Derived>
Parameter::Parameter(const Eigen::DenseBase<Derived> &matrix)
{
    resize(matrix.rows(), matrix.cols());
    for (size_t row = 0; row < size_t(matrix.rows()); row++)
    {
        for (size_t col = 0; col < size_t(matrix.cols()); col++)
        {
            coeffRef(row, col) = internal::ParameterSource(matrix(row, col));
        }
    }
}

template <typename Derived>
Parameter::Parameter(Eigen::DenseBase<Derived> *matrix)
{
    resize(matrix->rows(), matrix->cols());
    for (size_t row = 0; row < size_t(matrix->rows()); row++)
    {
        for (size_t col = 0; col < size_t(matrix->cols()); col++)
        {
            coeffRef(row, col) = internal::ParameterSource(&matrix->coeffRef(row, col));
        }
    }
}

} // namespace op


/*
*   open .obj file and read get the graph
*   in .obj files, edges are noted "l" (for line)
*                  vertices are noted "v"
*
*   this file is scavenged from the readOBJ file of libIGL
*   the modifications are:
*    - remove the faces and add edge loading
*    - remove dependencies to exterior files (libIGL)
*    - change the #define tag
*
*   by R. Falque
*   10/05/2019
*/

#ifndef READ_GRAPH_OBJ_HPP
#define READ_GRAPH_OBJ_HPP

#include <Eigen/Core>
#include <vector>

#include <string>
#include <cstdio>

#include <iostream>
#include <fstream>
#include <sstream>
#include <iterator>

namespace libgraphcpp
{
    inline bool readGraphOBJ(const std::string obj_file_name, Eigen::MatrixXd & V_out, Eigen::MatrixXi & E_out){
        bool verbose = false;

        std::vector<std::vector<double > > V;
        std::vector<std::vector<int > > E;

        // Open file, and check for error
        FILE * obj_file = fopen(obj_file_name.c_str(),"r");
        if(NULL==obj_file)
        {
        fprintf(stderr,"IOError: %s could not be opened...\n",
                obj_file_name.c_str());
        return false;
        }

        V.clear();
        E.clear();

        // variables and constants to assist parsing the .obj file
        // Constant strings to compare against
        std::string v("v");
        std::string e("l");
        std::string tic_tac_toe("#");

        #ifndef IGL_LINE_MAX
        #  define IGL_LINE_MAX 2048
        #endif

        char line[IGL_LINE_MAX];
        int line_no = 1;
        while (fgets(line, IGL_LINE_MAX, obj_file) != NULL)
        {
        char type[IGL_LINE_MAX];
        // Read first word containing type
        if(sscanf(line, "%s",type) == 1)
        {
            // Get pointer to rest of line right after type
            char * l = &line[strlen(type)];
            if(type == v)
            {
            std::istringstream ls(&line[1]);
            std::vector<double > vertex{ std::istream_iterator<double >(ls), std::istream_iterator<double >() };

            if (vertex.size() < 3)
            {
                fprintf(stderr,
                        "Error: readOBJ() vertex on line %d should have at least 3 coordinates",
                        line_no);
                fclose(obj_file);
                return false;
            }

            V.push_back(vertex);
            }else if(type == e)
            {
            std::istringstream ls(&line[1]);
            std::vector<int > edge{ std::istream_iterator<int >(ls), std::istream_iterator<int >() };

            if (edge.size() != 2)
            {
                fprintf(stderr,
                        "Error: readOBJ() vertex on line %d should have 2 coordinates",
                        line_no);
                fclose(obj_file);
                return false;
            }

            E.push_back(edge);
            }else if(strlen(type) >= 1 && (type[0] == '#' ||
                type[0] == 'g'  ||
                type[0] == 's'  ||
                strcmp("usemtl",type)==0 ||
                strcmp("mtllib",type)==0))
            {
            //ignore comments or other shit
            }else
            {
            //ignore any other lines
            if (verbose)
            fprintf(stderr,
                    "Warning: readOBJ() ignored non-comment line %d:\n  %s",
                    line_no,
                    line);
            }
        }else
        {
            // ignore empty line
        }
        line_no++;
        }
        fclose(obj_file);

        int num_vertices = V.size();
        int num_edges = E.size();
        V_out.resize(num_vertices, 3);
        E_out.resize(num_edges, 2);

        for(int i = 0;i<num_vertices;i++)
        {
        if (V[i].size() != 3) {
            std::cout << "Error in readGraphOBJ.hpp: wrong input dimension\n";
            exit(0);
        }

        for(int j = 0;j<3;j++)
            V_out(i,j) = V[i][j];
        }

        for(int i = 0;i<num_edges;i++)
        {
        if (E[i].size() != 2) {
            std::cout << "Error in readGraphOBJ.hpp: wrong input dimension\n";
            exit(0);
        }

        for(int j = 0;j<2;j++)
            E_out(i,j) = E[i][j];
        }

        // remove one for 0 index references
        E_out = E_out - Eigen::MatrixXi::Constant(E_out.rows(),2,1);

        return true;
    };
}

#endif


#define BOOST_TEST_DYN_LINK
//#define BOOST_TEST_MODULE balance
#include <boost/test/unit_test.hpp>

#include <system.hh>

#include "balance.h"

using namespace ledger;

struct balance_fixture {
  balance_fixture() {
  times_initialize();
  amount_t::initialize();

  // Cause the display precision for dollars to be initialized to 2.
  amount_t x1("$1.00");
  BOOST_CHECK(x1);

  amount_t::stream_fullstrings = true; // make reports from UnitTests accurate
  }

  ~balance_fixture()
  {
  amount_t::stream_fullstrings = false;
  amount_t::shutdown();
  times_shutdown();
  }
};

BOOST_FIXTURE_TEST_SUITE(balance, balance_fixture)

BOOST_AUTO_TEST_CASE(testConstructors)
{
  balance_t b0;
  balance_t b1(1.00);
  balance_t b2(123456UL);
  balance_t b3(12345L);
  balance_t b4(string ("EUR 123"));
  balance_t b5("$ 456");
  balance_t b6;
  balance_t b7(amount_t("$ 1.00"));
  balance_t b8(b7);

  BOOST_CHECK_EQUAL(balance_t(), b0);
  BOOST_CHECK_NE(balance_t("0"), b0);
  BOOST_CHECK_NE(balance_t("0.0"), b0);
  BOOST_CHECK_EQUAL(b2, 123456UL);
  BOOST_CHECK_EQUAL(b3, 12345L);
  BOOST_CHECK_EQUAL(b4, "EUR 123");
  BOOST_CHECK_EQUAL(b5, string("$ 456"));
  BOOST_CHECK_EQUAL(b7, b8);
  BOOST_CHECK_EQUAL(b8, amount_t("$ 1.00"));

  b5 = "euro 2345";
  b6 = string("DM -34532");
  b7 = amount_t("$ 1.00");

  b8 = b5;
  BOOST_CHECK_EQUAL(b5, b8);

  BOOST_CHECK(b0.valid());
  BOOST_CHECK(b1.valid());
  BOOST_CHECK(b2.valid());
  BOOST_CHECK(b3.valid());
  BOOST_CHECK(b4.valid());
  BOOST_CHECK(b5.valid());
  BOOST_CHECK(b6.valid());
  BOOST_CHECK(b7.valid());
  BOOST_CHECK(b8.valid());
}

BOOST_AUTO_TEST_CASE(testAddition)
{
  amount_t a0;
  amount_t a1("$1");
  amount_t a2("2 EUR");
  amount_t a3("0.00 CAD");
  amount_t a4("$2");

  balance_t b0;
  balance_t b1(1.00);
  balance_t b2(2UL);
  balance_t b3(2L);
  balance_t b4;
  balance_t b5;

  b0 += b1;
  b2 += b3;
  b3 += a1;
  b3 += a2;
  b4 += b3;
  b5 += a1;
  b5 += a4;

  BOOST_CHECK_EQUAL(balance_t(1.00), b0);
  BOOST_CHECK_EQUAL(b3 += a3, b4);
  BOOST_CHECK_EQUAL(balance_t(4L), b2);
  BOOST_CHECK_EQUAL(balance_t() += amount_t("$3"), b5);

  BOOST_CHECK_THROW(b3 += a0, balance_error);

  BOOST_CHECK(b0.valid());
  BOOST_CHECK(b1.valid());
  BOOST_CHECK(b2.valid());
  BOOST_CHECK(b3.valid());
  BOOST_CHECK(b4.valid());
  BOOST_CHECK(b5.valid());
}

BOOST_AUTO_TEST_CASE(testSubtraction)
{
  amount_t a0;
  amount_t a1("$1");
  amount_t a2("2 EUR");
  amount_t a3("0.00 CAD");
  amount_t a4("$2");

  balance_t b0;
  balance_t b1(1.00);
  balance_t b2(2UL);
  balance_t b3(2L);
  balance_t b4;
  balance_t b5;

  b0 -= b1;
  b2 -= b3;
  b3 -= a1;
  b3 -= a2;
  b4 = b3;
  b5 -= a1;
  b5 -= a4;

  BOOST_CHECK_EQUAL(balance_t(-1.00), b0);
  BOOST_CHECK_EQUAL(b3 -= a3, b4);
  BOOST_CHECK_EQUAL(balance_t(), b2);
  BOOST_CHECK_EQUAL(b3 -= b2, b3);
  BOOST_CHECK_EQUAL(balance_t() -= amount_t("$3"), b5);

  BOOST_CHECK_THROW(b3 -= a0, balance_error);

  BOOST_CHECK(b0.valid());
  BOOST_CHECK(b1.valid());
  BOOST_CHECK(b2.valid());
  BOOST_CHECK(b3.valid());
  BOOST_CHECK(b4.valid());
  BOOST_CHECK(b5.valid());
}

BOOST_AUTO_TEST_CASE(testEqaulity)
{
  amount_t a0;
  amount_t a1("$1");
  amount_t a2("2 EUR");
  amount_t a3("0.00 CAD");

  balance_t b0;
  balance_t b1(1.00);
  balance_t b2(2UL);
  balance_t b3(2L);
  balance_t b4("EUR 2");
  balance_t b5("$-1");
  balance_t b6("0.00");
  balance_t b7("0.00");

  BOOST_CHECK(b2 == b3);
  BOOST_CHECK(b4 == a2);
  BOOST_CHECK(b1 == "1.00");
  BOOST_CHECK(b5 == amount_t("-$1"));
  BOOST_CHECK(!(b6 == "0"));
  BOOST_CHECK(!(b6 == a3));
  BOOST_CHECK(!(b6 == "0.00"));
  BOOST_CHECK(b6 == b7);

  b4 += b5;
  b5 += a2;

  BOOST_CHECK(b4 == b5);

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-value"
  BOOST_CHECK_THROW(b0 == a0, balance_error);
#pragma GCC diagnostic pop

  BOOST_CHECK(b0.valid());
  BOOST_CHECK(b1.valid());
  BOOST_CHECK(b2.valid());
  BOOST_CHECK(b3.valid());
  BOOST_CHECK(b4.valid());
  BOOST_CHECK(b5.valid());
  BOOST_CHECK(b6.valid());
  BOOST_CHECK(b7.valid());
}

BOOST_AUTO_TEST_CASE(testMultiplication)
{
  amount_t a0;
  amount_t a1("0.00");

  balance_t b0;
  balance_t b1(1.00);
  balance_t b2(2UL);
  balance_t b3("CAD -3");
  balance_t b4("EUR 4.99999");
  balance_t b5("$1");
  balance_t b6;

  BOOST_CHECK_EQUAL(b1 *= 2.00, amount_t(2.00));
  BOOST_CHECK_EQUAL(b2 *= 2L, amount_t(4L));
  BOOST_CHECK_EQUAL(b2 *= 2UL, amount_t(8UL));
  BOOST_CHECK_EQUAL(b3 *= amount_t("-8 CAD"), amount_t("CAD 24"));
  BOOST_CHECK_EQUAL(b0 *= 2UL, b0);
  BOOST_CHECK_EQUAL(b0 *= a1, a1);

  b6 += b3;
  b3 += b4;
  b3 += b5;
  b3 *= 2L;
  b6 *= 2L;
  b4 *= 2L;
  b5 *= 2L;
  b6 += b4;
  b6 += b5;

  BOOST_CHECK_EQUAL(b3, b6);

  BOOST_CHECK_THROW(b1 *= a0 , balance_error);
  BOOST_CHECK_THROW(b4 *= amount_t("1 CAD") , balance_error);
  BOOST_CHECK_THROW(b3 *= amount_t("1 CAD") , balance_error);

  BOOST_CHECK(b0.valid());
  BOOST_CHECK(b1.valid());
  BOOST_CHECK(b2.valid());
  BOOST_CHECK(b3.valid());
  BOOST_CHECK(b4.valid());
  BOOST_CHECK(b5.valid());
  BOOST_CHECK(b6.valid());
}

BOOST_AUTO_TEST_CASE(testDivision)
{
  amount_t a0;
  amount_t a1("0.00");

  balance_t b0;
  balance_t b1(4.00);
  balance_t b2(4UL);
  balance_t b3("CAD -24");
  balance_t b4("EUR 4");
  balance_t b5("$2");
  balance_t b6;

  BOOST_CHECK_EQUAL(b1 /= 2.00, amount_t(2.00));
  BOOST_CHECK_EQUAL(b2 /= 2L, amount_t(2L));
  BOOST_CHECK_EQUAL(b2 /= 2UL, amount_t(1UL));
  BOOST_CHECK_EQUAL(b3 /= amount_t("-3 CAD"), amount_t("CAD 8"));
  BOOST_CHECK_EQUAL(b0 /= 2UL, b0);

  b6 += b3;
  b3 += b4;
  b3 += b5;
  b3 /= 2L;
  b6 /= 2L;
  b4 /= 2L;
  b5 /= 2L;
  b6 += b4;
  b6 += b5;

  BOOST_CHECK_EQUAL(b3, b6);

  BOOST_CHECK_THROW(b1 /= a0 , balance_error);
  BOOST_CHECK_THROW(b1 /= a1 , balance_error);
  BOOST_CHECK_THROW(b4 /= amount_t("1 CAD") , balance_error);
  BOOST_CHECK_THROW(b3 /= amount_t("1 CAD") , balance_error);

  BOOST_CHECK(b0.valid());
  BOOST_CHECK(b1.valid());
  BOOST_CHECK(b2.valid());
  BOOST_CHECK(b3.valid());
  BOOST_CHECK(b4.valid());
  BOOST_CHECK(b5.valid());
  BOOST_CHECK(b6.valid());
}

BOOST_AUTO_TEST_CASE(testNegation)
{
  amount_t a1("0.00");
  amount_t a2("$ 123");
  amount_t a3("EUR 456");

  balance_t b0;
  balance_t b1;
  balance_t b2;
  balance_t b3;

  b1 += a1;
  b1 += a2;
  b1 += a3;
  b2 += -a1;
  b2 += -a2;
  b2 += -a3;
  b3 = -b1;

  BOOST_CHECK_EQUAL(b0.negated(), b0);
  BOOST_CHECK_EQUAL(b2, b3);
  BOOST_CHECK_EQUAL(b2, -b1);
  BOOST_CHECK_EQUAL(b2.negated(), b1);

  b2.in_place_negate();

  BOOST_CHECK_EQUAL(b2, b1);
  BOOST_CHECK_EQUAL(b1, -b3);

  BOOST_CHECK(b0.valid());
  BOOST_CHECK(b1.valid());
  BOOST_CHECK(b2.valid());
  BOOST_CHECK(b3.valid());
}

BOOST_AUTO_TEST_CASE(testAbs)
{
  amount_t a1("0.00");
  amount_t a2("$ 123");
  amount_t a3("EUR 456");

  balance_t b0;
  balance_t b1;
  balance_t b2;

  b1 += a1;
  b1 += a2;
  b1 += a3;
  b2 += -a1;
  b2 += -a2;
  b2 += -a3;

  BOOST_CHECK_EQUAL(b0.abs(), b0);
  BOOST_CHECK_EQUAL(b2.abs(), b1.abs());

  BOOST_CHECK(b0.valid());
  BOOST_CHECK(b1.valid());
  BOOST_CHECK(b2.valid());
}

BOOST_AUTO_TEST_CASE(testCeiling)
{
  amount_t a1("0.00");
  amount_t a2("$ 123.123");
  amount_t a3("EUR 456.56");
  amount_t a4(-a1);
  amount_t a5(-a2);
  amount_t a6(-a3);

  balance_t b0;
  balance_t b1;
  balance_t b2;
  balance_t b3;
  balance_t b4;

  b1 += a1;
  b1 += a2;
  b1 += a3;
  b2 += -a1;
  b2 += -a2;
  b2 += -a3;

  b3 += a1.ceilinged();
  b3 += a2.ceilinged();
  b3 += a3.ceilinged();
  b4 += a4.ceilinged();
  b4 += a5.ceilinged();
  b4 += a6.ceilinged();

  BOOST_CHECK_EQUAL(b0.ceilinged(), b0);
  BOOST_CHECK_EQUAL(b2.ceilinged(), b4);
  BOOST_CHECK_EQUAL(b1.ceilinged(), b3);

  b1.in_place_ceiling();
  BOOST_CHECK_EQUAL(b1, b3);

  BOOST_CHECK(b0.valid());
  BOOST_CHECK(b1.valid());
  BOOST_CHECK(b2.valid());
  BOOST_CHECK(b3.valid());
  BOOST_CHECK(b4.valid());
}

BOOST_AUTO_TEST_CASE(testFloor)
{
  amount_t a1("0.00");
  amount_t a2("$ 123.123");
  amount_t a3("EUR 456.56");
  amount_t a4(-a1);
  amount_t a5(-a2);
  amount_t a6(-a3);

  balance_t b0;
  balance_t b1;
  balance_t b2;
  balance_t b3;
  balance_t b4;

  b1 += a1;
  b1 += a2;
  b1 += a3;
  b2 += -a1;
  b2 += -a2;
  b2 += -a3;

  b3 += a1.floored();
  b3 += a2.floored();
  b3 += a3.floored();
  b4 += a4.floored();
  b4 += a5.floored();
  b4 += a6.floored();

  BOOST_CHECK_EQUAL(b0.floored(), b0);
  BOOST_CHECK_EQUAL(b2.floored(), b4);
  BOOST_CHECK_EQUAL(b1.floored(), b3);

  b1.in_place_floor();
  BOOST_CHECK_EQUAL(b1, b3);

  BOOST_CHECK(b0.valid());
  BOOST_CHECK(b1.valid());
  BOOST_CHECK(b2.valid());
  BOOST_CHECK(b3.valid());
  BOOST_CHECK(b4.valid());
}

BOOST_AUTO_TEST_CASE(testRound)
{
  amount_t a1("0.00");
  amount_t a2("$ 123.123");
  amount_t a3("EUR 456.567");
  amount_t a4("0.00");
  amount_t a5("$ 123.12");
  amount_t a6("EUR 456.57");

  balance_t b0;
  balance_t b1;
  balance_t b2;
  balance_t b3;
  balance_t b4;

  b1 += a1;
  b1 += a2;
  b1 += a3;
  b2 += a4;
  b2 += a5;
  b2 += a6;

  a1.in_place_roundto(2);
  a2.in_place_roundto(2);
  a3.in_place_roundto(2);
  a4.in_place_roundto(2);
  a5.in_place_roundto(2);
  a6.in_place_roundto(2);

  b3 += a1;
  b3 += a2;
  b3 += a3;
  b4 += a4;
  b4 += a5;
  b4 += a6;

  BOOST_CHECK_EQUAL(b0.rounded(), b0);
  BOOST_CHECK_EQUAL(b2.rounded(), b4);
  BOOST_CHECK_EQUAL(b1.rounded(), b4);

  b1.in_place_round();
  BOOST_CHECK_EQUAL(b1, b3);

  BOOST_CHECK(b0.valid());
  BOOST_CHECK(b1.valid());
  BOOST_CHECK(b2.valid());
  BOOST_CHECK(b3.valid());
  BOOST_CHECK(b4.valid());
}

BOOST_AUTO_TEST_CASE(testTruth)
{
  amount_t a1("0.00");
  amount_t a2("$ 123");
  amount_t a3("EUR 456");

  balance_t b0;
  balance_t b1;

  b1 += a1;
  b1 += a2;
  b1 += a3;

  BOOST_CHECK(!b0);
  BOOST_CHECK(b1);

  BOOST_CHECK(b0.valid());
  BOOST_CHECK(b1.valid());
}

BOOST_AUTO_TEST_CASE(testForZero)
{
  amount_t a1("0.00");
  amount_t a2("$ 123");
  amount_t a3("EUR 456");

  balance_t b0;
  balance_t b1;

  b1 += a1;
  b1 += a2;
  b1 += a3;

  BOOST_CHECK(b0.is_empty());
  BOOST_CHECK(b0.is_zero());
  BOOST_CHECK(b0.is_realzero());
  BOOST_CHECK(!b0.is_nonzero());
  BOOST_CHECK(!b1.is_empty());
  BOOST_CHECK(!b1.is_zero());
  BOOST_CHECK(!b1.is_realzero());
  BOOST_CHECK(b1.is_nonzero());

  BOOST_CHECK(b0.valid());
  BOOST_CHECK(b1.valid());
}

BOOST_AUTO_TEST_SUITE_END()


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

  @copyright 2017 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_CLAMP_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_CLAMP_HPP_INCLUDED

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

 /*!

    @ingroup group-arithmetic
    This function object clamps @c x between  @c lo and  @c hi.
    If  @c x is less than @c hi, returns the larger of @c x and @c lo, otherwise returns the smaller of @c x and @c hi.

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

    @par Decorators

      - pedantic_ ensures standard  conformity for a Nan first parameter.
      - std_ is not available yet as the stlibc++ version is only defined since C++17.

    @par Example:

       @snippet clamp.cpp clamp

    @par Possible output:

       @snippet clamp.txt clamp

  **/
  Value clamp(Value const& x, Value const& lo, Value const& hi);
} }
#endif

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

#endif


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <core/util/test_macros.hpp>
#include <string>
#include <cassert>
#include <iostream>
#include <ml/sketches/quantile_sketch.hpp>
#include <ml/sketches/streaming_quantile_sketch.hpp>
#include <core/random/random.hpp>
struct quantile_sketch_test {
  static constexpr double epsilon = 0.01;
  typedef turi::sketches::quantile_sketch<double> sketch_type;
  typedef turi::sketches::streaming_quantile_sketch<double> streaming_sketch_type;

  template <typename SKETCH_TYPE>
  void compare_quantiles_at(std::vector<double>& values,
                            SKETCH_TYPE& sketch,
                            double quantile) {
    size_t index = quantile * values.size();
    index = std::min(values.size() - 1, index);
    int lower_index = index - values.size() * epsilon;
    int upper_index = index + values.size() * epsilon;
    lower_index = std::max<int>(lower_index, 0);
    upper_index = std::min<int>(upper_index, values.size() - 1);
    double lower = values[lower_index];
    double upper = values[upper_index];
    double query = sketch.query(index);

    TS_ASSERT_LESS_THAN_EQUALS(lower, query);
    TS_ASSERT_LESS_THAN_EQUALS(query, upper);
    TS_ASSERT_EQUALS(query, sketch.query_quantile(quantile));
    TS_ASSERT_EQUALS(sketch.fast_query(index),
                     sketch.fast_query_quantile(quantile));

    std::cout << quantile*100 << "% : " << values[index] << " vs "
              << sketch.query_quantile(quantile)
              << " true epsilon interval:[" << lower << ", " << upper << "]  "
              << "(fast query: " << sketch.fast_query_quantile(quantile) << ")\n";
  }

  void quantile_test(const std::vector<double>& values) {
    std::vector<double> sorted_values = values;
    std::sort(sorted_values.begin(), sorted_values.end());
    /////////////////////// Fixed Sketches /////////////////////////////
    {
      // single sketch
      sketch_type sketch(values.size(), epsilon);
      for (size_t i = 0;i < values.size(); ++i) {
        sketch.add(values[i]);
      }
      sketch.finalize();
      TS_ASSERT_EQUALS(sketch.size(), values.size());
      std::cout << "------ Sequential Sketching ------\n";
      std::cout << "Sketch size = " << sketch.memory_usage() << " bytes\n";
      compare_quantiles_at(sorted_values, sketch, 0);
      compare_quantiles_at(sorted_values, sketch, 0.01);
      compare_quantiles_at(sorted_values, sketch, 0.05);
      compare_quantiles_at(sorted_values, sketch, 0.50);
      compare_quantiles_at(sorted_values, sketch, 0.95);
      compare_quantiles_at(sorted_values, sketch, 0.99);
      compare_quantiles_at(sorted_values, sketch, 1);
      std::cout << "\n";

      // test multiple streams
      std::vector<sketch_type> sketches(16, sketch_type(values.size(), epsilon));
      for (size_t i = 0;i < values.size(); ++i) {
        sketches[i % sketches.size()].add(values[i]);
      }
      sketch_type final_sketch(values.size(), epsilon);
      for (size_t i = 0;i < sketches.size(); ++i) {
        final_sketch.combine(sketches[i]);
      }
      final_sketch.finalize();
      TS_ASSERT_EQUALS(final_sketch.size(), values.size());
      std::cout << "------ 16-way Parallel Sketching of the same stream ------\n";
      std::cout << "Sketch size = " << final_sketch.memory_usage() << " bytes\n";
      compare_quantiles_at(sorted_values, final_sketch, 0);
      compare_quantiles_at(sorted_values, final_sketch, 0.01);
      compare_quantiles_at(sorted_values, final_sketch, 0.05);
      compare_quantiles_at(sorted_values, final_sketch, 0.50);
      compare_quantiles_at(sorted_values, final_sketch, 0.95);
      compare_quantiles_at(sorted_values, final_sketch, 0.99);
      compare_quantiles_at(sorted_values, final_sketch, 1);
      std::cout << "\n\n";
    }
    ////////////////////// Streaming ////////////////////////////////
    {
      // streaming sketch
      streaming_sketch_type sketch(epsilon);
      for (size_t i = 0;i < values.size(); ++i) {
        sketch.add(values[i]);
      }
      sketch.finalize();
      TS_ASSERT_EQUALS(sketch.size(), values.size());
      std::cout << "------ Sequential Streaming Sketching ------\n";
      std::cout << "Sketch size = " << sketch.memory_usage() << " bytes\n";
      compare_quantiles_at(sorted_values, sketch, 0);
      compare_quantiles_at(sorted_values, sketch, 0.01);
      compare_quantiles_at(sorted_values, sketch, 0.05);
      compare_quantiles_at(sorted_values, sketch, 0.50);
      compare_quantiles_at(sorted_values, sketch, 0.95);
      compare_quantiles_at(sorted_values, sketch, 0.99);
      compare_quantiles_at(sorted_values, sketch, 1);
      std::cout << "\n";

      // test multiple streams
      std::vector<streaming_sketch_type> sketches(16, streaming_sketch_type(epsilon));
      for (size_t i = 0;i < values.size(); ++i) {
        sketches[i % sketches.size()].add(values[i]);
      }
      streaming_sketch_type final_sketch(epsilon);
      for (size_t i = 0;i < sketches.size(); ++i) {
        sketches[i].substream_finalize();
        final_sketch.combine(sketches[i]);
      }
      final_sketch.combine_finalize();
      TS_ASSERT_EQUALS(final_sketch.size(), values.size());
      std::cout << "------ 16-way Parallel Streaming Sketching of the same stream ------\n";
      std::cout << "Sketch size = " << final_sketch.memory_usage() << " bytes\n";
      compare_quantiles_at(sorted_values, final_sketch, 0);
      compare_quantiles_at(sorted_values, final_sketch, 0.01);
      compare_quantiles_at(sorted_values, final_sketch, 0.05);
      compare_quantiles_at(sorted_values, final_sketch, 0.50);
      compare_quantiles_at(sorted_values, final_sketch, 0.95);
      compare_quantiles_at(sorted_values, final_sketch, 0.99);
      compare_quantiles_at(sorted_values, final_sketch, 1);
      std::cout << "\n\n";
    }
  }

  std::vector<double> vals;

  void generate_gaussian_vals() {
    turi::random::seed(1001);
    for (size_t i = 0;i < vals.size(); ++i) {
      vals[i] = turi::random::gaussian(0, 10);
    }
  }
  void generate_gamma_vals() {
    turi::random::seed(1001);
    for (size_t i = 0;i < vals.size(); ++i) {
      vals[i] = turi::random::gamma();
    }
  }
  void generate_uniform_vals() {
    turi::random::seed(1001);
    for (size_t i = 0;i < vals.size(); ++i) {
      vals[i] = turi::random::uniform<double>(0, 1);
    }
  }
 public:
  quantile_sketch_test() {
    vals.resize(1000*1000);
  }
  void test_gaussian() {
    std::cout << "Gaussian:\n";
    generate_gaussian_vals();
    quantile_test(vals);
  }
  void test_sorted_gaussian() {
    std::cout << "Gaussian Sorted:\n";
    generate_gaussian_vals();
    std::sort(vals.begin(), vals.end());
    quantile_test(vals);
  }

  void test_gamma() {
    std::cout << "Gamma:\n";
    generate_gamma_vals();
    quantile_test(vals);
  }
  void test_sorted_gamma() {
    std::cout << "Gamma Sorted:\n";
    generate_gamma_vals();
    std::sort(vals.begin(), vals.end());
    quantile_test(vals);
  }

  void test_uniform() {
    std::cout << "Uniform:\n";
    generate_uniform_vals();
    quantile_test(vals);
  }
  void test_sorted_uniform() {
    std::cout << "Uniform Sorted:\n";
    generate_uniform_vals();
    std::sort(vals.begin(), vals.end());
    quantile_test(vals);
  }
};

BOOST_FIXTURE_TEST_SUITE(_quantile_sketch_test, quantile_sketch_test)
BOOST_AUTO_TEST_CASE(test_gaussian) {
  quantile_sketch_test::test_gaussian();
}
BOOST_AUTO_TEST_CASE(test_sorted_gaussian) {
  quantile_sketch_test::test_sorted_gaussian();
}
BOOST_AUTO_TEST_CASE(test_gamma) {
  quantile_sketch_test::test_gamma();
}
BOOST_AUTO_TEST_CASE(test_sorted_gamma) {
  quantile_sketch_test::test_sorted_gamma();
}
BOOST_AUTO_TEST_CASE(test_uniform) {
  quantile_sketch_test::test_uniform();
}
BOOST_AUTO_TEST_CASE(test_sorted_uniform) {
  quantile_sketch_test::test_sorted_uniform();
}
BOOST_AUTO_TEST_SUITE_END()


#include "utils.hh"
#include <boost/iostreams/stream.hpp>
#include <boost/iostreams/device/null.hpp>
#include <chrono>
#include <iomanip>
#include <random>

#include "defs.hh"

using namespace std;

// Time utilities
double elapsedSecs(time_point start) {
  return chrono::duration_cast<chrono::milliseconds>(
    chrono::system_clock::now() - start).count() / 1000.0;
}

// Logging utilities
bool verboseMode = false;
boost::iostreams::stream< boost::iostreams::null_sink >
nullOstream( ( boost::iostreams::null_sink() ) );

void setVerboseMode(bool enabled) { verboseMode = enabled; }

std::ostream& logstream(std::ostream& s, std::string label) {
  time_point now = chrono::system_clock::now();
  time_t t = chrono::system_clock::to_time_t(now);
  return s << "[" << put_time(localtime(&t), "%X") << " " << label << "] ";
}
std::ostream& err()  { return logstream(cerr, "ERROR"); }
std::ostream& warn() { return logstream(cerr, "WARNING"); }
std::ostream& info() { return logstream(cerr, "INFO"); }
std::ostream& dbg()  { return verboseMode ? logstream(cerr, "DBG") : nullOstream; }

// Random generator utilities
mt19937 randEngine;
void randSetSeed(int seed) { randEngine.seed(seed); }
s32 randInt(s32 exclusiveMax) { return uniform_int_distribution<s32>(0, exclusiveMax - 1)(randEngine); }
double randProb() { return uniform_real_distribution<double>(0, 1)(randEngine); }


#include <cctype> // for std::isdigit
#include <locale>
#include <tuple>
#include <string_view>
#include <boost/predef.h>
#include <boost/locale/collator.hpp>

#include <ext/range/str_view.hpp>
#include <ext/algorithm/tribool_compare.hpp>

/// natural order related stuff. natural order is where number parts of string are collated as numbers and not just chars.
/// So str1txt < str12txt with natural order, while if comparing them just as strings str1txt > str12txt
namespace ext::natural_order
{
	class default_number_traits;
	class default_char_traits;
	class localized_char_traits;

	template <class NumberTraits, class StringTraits>
	class comparator;

	/// default number traits, is_digit forwarded to std::isdigit
	/// leading zeroes are ignored, numbers are compared by longest run or greatest number, see implementation
	class default_number_traits
	{
	public:
		using result_type = int;

	public:
		template <class CharType>
		bool is_digit(CharType ch) const;

		template <class CharType>
		result_type compare(const CharType * s1_first, const CharType * s1_last, const CharType * s2_first, const CharType * s2_last) const;
	};

	template <class CharType>
	inline bool default_number_traits::is_digit(CharType ch) const
	{
		using uchar_type = std::make_unsigned_t<CharType>;
		return std::isdigit(static_cast<uchar_type>(ch));
	}

	template <class CharType>
	auto default_number_traits::compare(const CharType * s1_first, const CharType * s1_last, const CharType * s2_first, const CharType * s2_last) const -> result_type
	{
		// The longest run of digits wins, but if digit numbers is same - greatest number wins.
		// We remember relation, for later, and check it if digits number equal, otherwise - longest wins
		std::size_t z1 = 0, z2 = 0;
		for (; s1_first != s2_last and *s1_first == '0'; ++s1_first, ++z1);
		for (; s2_first != s2_last and *s2_first == '0'; ++s2_first, ++z2);

		result_type result = 0;
		for (;; ++s1_first, ++s2_first)
		{
			// equal runs - return stored result
			if (s1_first == s1_last and s2_first == s2_last)
				return result != 0 ? result
					// if equal - who has more zeroes - wins(is less)
					: ext::tribool_compare(z2, z1);

			// left shorter -> less
			if (s1_first == s1_last) return -1;
			// left longer  -> greater
			if (s2_first == s2_last) return +1;

			// remember less
			if (*s1_first < *s2_first)
			{ if (result == 0) result = -1; }
			// remember greater
			else if (*s1_first > *s2_first)
			{ if (result == 0) result = +1; }
		}

		return result != 0 ? result
			// if equal - who has more zeroes - wins(is less)
			: ext::tribool_compare(z2, z1);
	}

	/// default char_traits implementation. Compares string using std::char_traits
	class default_char_traits
	{
	public:
		using result_type = int;

	public:
		template <class CharType>
		result_type compare(const CharType * s1_first, const CharType * s1_last, const CharType * s2_first, const CharType * s2_last) const;
	};

	template <class CharType>
	auto default_char_traits::compare(const CharType * s1_first, const CharType * s1_last, const CharType * s2_first, const CharType * s2_last) const -> result_type
	{
		std::size_t n1 = s1_last - s1_first;
		std::size_t n2 = s2_last - s2_first;

		std::size_t n = std::min(n1, n2);
		result_type result = std::char_traits<CharType>::compare(s1_first, s2_first, n);
		if (result != 0) return result;

		return ext::tribool_compare(n1, n2);
	}

	/// char_traits implementation using std::locale via std::collator
	class localized_char_traits
	{
	public:
		using result_type = int;

	private:
		std::locale m_loc;
		boost::locale::collator_base::level_type m_compare_level = boost::locale::collator_base::identical;

		std::tuple<
			  result_type (localized_char_traits::*)(const char     * s1_first, const char     * s1_last, const char     * s2_first, const char     * s2_last) const
			, result_type (localized_char_traits::*)(const wchar_t  * s1_first, const wchar_t  * s1_last, const wchar_t  * s2_first, const wchar_t  * s2_last) const
		// on msvc std lib does likes locales for char16_t, char32_t :(
		// locale(126): warning C4273: 'id': inconsistent dll linkage
		// error C2491: 'std::collate<CharType>::id': definition of dllimport static data member not allowed
		// https://social.msdn.microsoft.com/Forums/en-US/3e1d37a5-0b50-41fe-ac18-2b4db5e1262b/compile-error-for-stl-stream-containers-in-visual-studio?forum=vcgeneral
		// maybe static linkage of stdlib would work?
		#if not BOOST_LIB_STD_DINKUMWARE
			, result_type (localized_char_traits::*)(const char16_t * s1_first, const char16_t * s1_last, const char16_t * s2_first, const char16_t * s2_last) const
			, result_type (localized_char_traits::*)(const char32_t * s1_first, const char32_t * s1_last, const char32_t * s2_first, const char32_t * s2_last) const
		#endif
		> m_methods;

	private:
		template <class CharType> result_type   std_compare(const CharType * s1_first, const CharType * s1_last, const CharType * s2_first, const CharType * s2_last) const;
		template <class CharType> result_type boost_compare(const CharType * s1_first, const CharType * s1_last, const CharType * s2_first, const CharType * s2_last) const;

	public:
		template <class CharType>
		result_type compare(const CharType * s1_first, const CharType * s1_last, const CharType * s2_first, const CharType * s2_last) const;

	public:
		localized_char_traits(const std::locale & loc = std::locale());
		localized_char_traits(boost::locale::collator_base::level_type level, const std::locale & loc = std::locale());
	};

	localized_char_traits::localized_char_traits(const std::locale & loc)
	    : m_loc(loc)
	{
		m_methods = std::make_tuple(
			  &localized_char_traits::std_compare<char>
			, &localized_char_traits::std_compare<wchar_t>
		#if not BOOST_LIB_STD_DINKUMWARE
			, &localized_char_traits::std_compare<char16_t>
			, &localized_char_traits::std_compare<char32_t>
		#endif
		);
	}

	localized_char_traits::localized_char_traits(boost::locale::collator_base::level_type level, const std::locale & loc)
	    : m_loc(loc), m_compare_level(level)
	{
		if (m_compare_level == boost::locale::collator_base::identical)
		{
			m_methods = std::make_tuple(
				  &localized_char_traits::std_compare<char>
				, &localized_char_traits::std_compare<wchar_t>
			#if not BOOST_LIB_STD_DINKUMWARE
				, &localized_char_traits::std_compare<char16_t>
				, &localized_char_traits::std_compare<char32_t>
			#endif
			);
		}
		else
		{
			m_methods = std::make_tuple(
				  &localized_char_traits::boost_compare<char>
				, &localized_char_traits::boost_compare<wchar_t>
			#if not BOOST_LIB_STD_DINKUMWARE
				, &localized_char_traits::boost_compare<char16_t>
				, &localized_char_traits::boost_compare<char32_t>
			#endif
			);
		}
	}

	template <class CharType>
	auto localized_char_traits::std_compare(const CharType * s1_first, const CharType * s1_last, const CharType * s2_first, const CharType * s2_last) const -> result_type
	{
		using std_collator_facet = std::collate<CharType>;
		const auto & facet = std::use_facet<std_collator_facet>(m_loc);
		return facet.compare(s1_first, s1_last, s2_first, s2_last);
	}

	template <class CharType>
	auto localized_char_traits::boost_compare(const CharType * s1_first, const CharType * s1_last, const CharType * s2_first, const CharType * s2_last) const -> result_type
	{
		using boost_collator_facet = boost::locale::collator<CharType>;
		const auto & facet = std::use_facet<boost_collator_facet>(m_loc);

#if not defined NDEBUG
		// boost::locale::collator and std::collate sort of share same id in locale facet database,
		// so if std::collate is present - boost::locale::collator is present to, even if it's actually not.
		// So we better check if it's really boost::locale::collator.
		using std_collator_facet = std::collate<CharType>;
		auto * std_collator = static_cast<const std_collator_facet *>(&facet);
		assert(dynamic_cast<const boost_collator_facet *>(std_collator));
#endif

		return facet.compare(m_compare_level, s1_first, s1_last, s2_first, s2_last);
	}

	template <class CharType>
	inline auto localized_char_traits::compare(const CharType * s1_first, const CharType * s1_last, const CharType * s2_first, const CharType * s2_last) const -> result_type
	{
		using method = result_type (localized_char_traits::*)(const CharType * s1_first, const CharType * s1_last, const CharType * s2_first, const CharType * s2_last) const;
		return (this->*std::get<method>(m_methods))(s1_first, s1_last, s2_first, s2_last);
		//return std::invoke(std::get<method>(m_methods), this, s1_first, s1_last, s2_first, s2_last);
	}

	/// Comparator that can compare strings according to natural order.
	/// It is implemented via 2 traits classes:
	///  * NumberTraits - number related stuff:
	///    + is char a digit
	///    + compare numbers
	///  * CharTraits - compare string parts
	///
	/// it provides compare method returning +1/0/-1
	/// as well as relation predicates: less, greater, etc
	template <class NumberTraits, class CharTraits>
	class comparator
	{
	public:
		using number_traits = NumberTraits;
		using char_traits = CharTraits;
		using result_type = int;

	private:
		number_traits m_number_traits;
		char_traits m_char_traits;

	public:
		template <class StrViewCharType, class StrViewCharTraits>
		result_type compare(std::basic_string_view<StrViewCharType, StrViewCharTraits> s1, std::basic_string_view<StrViewCharType, StrViewCharTraits> s2) const;

		template <class String1, class String2>
		result_type compare(const String1 & s1, const String2 & s2) const { return compare(ext::str_view(s1), ext::str_view(s2)); }

	public:
		auto less()          const { return [*this](const auto & ...args) { return compare(args...) <  0; }; }
		auto less_equal()    const { return [*this](const auto & ...args) { return compare(args...) <= 0; }; }
		auto greater()       const { return [*this](const auto & ...args) { return compare(args...) >  0; }; }
		auto greater_equal() const { return [*this](const auto & ...args) { return compare(args...) >= 0; }; }
		auto equal_to()      const { return [*this](const auto & ...args) { return compare(args...) == 0; }; }
		auto not_equal_to()  const { return [*this](const auto & ...args) { return compare(args...) != 0; }; }

	public:
		comparator() = default;
		constexpr comparator(number_traits number_traits, char_traits char_traits)
		    : m_number_traits(std::move(number_traits)), m_char_traits(std::move(char_traits)) {}
	};

	template <class NumberTraits, class CharTraits>
	template <class StrViewCharType, class StrViewCharTraits>
	auto comparator<NumberTraits, CharTraits>::compare(std::basic_string_view<StrViewCharType, StrViewCharTraits> s1, std::basic_string_view<StrViewCharType, StrViewCharTraits> s2) const -> result_type
	{
		using string_type_view = std::basic_string_view<StrViewCharType, StrViewCharTraits>;
		using char_type = typename string_type_view::value_type;

		const char_type * s1_first = s1.data();
		const char_type * s1_last  = s1_first + s1.size();

		const char_type * s2_first = s2.data();
		const char_type * s2_last  = s2_first + s2.size();

		decltype(s1_first) s1_dfirst, s1_dlast;
		decltype(s2_first) s2_dfirst, s2_dlast;

		result_type result;

		while (s1_first != s1_last and s2_first != s2_last)
		{
			s1_dfirst = s1_first;
			s2_dfirst = s2_first;

			while (s1_dfirst != s1_last and not m_number_traits.is_digit(*s1_dfirst))
				++s1_dfirst;

			while (s2_dfirst != s2_last and not m_number_traits.is_digit(*s2_dfirst))
				++s2_dfirst;

			result = m_char_traits.compare(s1_first, s1_dfirst, s2_first, s2_dfirst);
			if (result != 0) return result;

			s1_dlast = s1_dfirst;
			s2_dlast = s2_dfirst;

			while (s1_dlast != s1_last and m_number_traits.is_digit(*s1_dlast))
				++s1_dlast;

			while (s2_dlast != s2_last and m_number_traits.is_digit(*s2_dlast))
				++s2_dlast;

			result = m_number_traits.compare(s1_dfirst, s1_dlast, s2_dfirst, s2_dlast);
			if (result != 0) return result;

			s1_first = s1_dlast;
			s2_first = s2_dlast;
		}

		return 0;
	}

	auto locale_comparator(                                                std::locale loc = std::locale()) { return comparator(default_number_traits(), localized_char_traits(loc)); }
	auto locale_comparator(boost::locale::collator_base::level_type level, std::locale loc = std::locale()) { return comparator(default_number_traits(), localized_char_traits(level, loc)); }

	constexpr comparator<default_number_traits, default_char_traits> natural_comparator {};
}

namespace ext
{
	using natural_order::natural_comparator;
}


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

#include "curve.hpp"
#include "igl/arap.h"

#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/StdVector>

namespace grapholon {

	class CurveDeformer {

	public:

		static Eigen::Vector3d to_eigen(Vector3f vec) {
			return Eigen::Vector3d(vec.X(), vec.Y(), vec.Z());
		}

		static Vector3f to_vec3(Eigen::Vector3d eigen) {
			return Vector3f(eigen.x(), eigen.y(), eigen.z());
		}

		/** Deforms the curve in a As-Rigid-As-Possible manner using the Interactive Geometry Library (IGL)*/
		static bool deform_curve(DeformableSplineCurve& in_curve, GRuint control_point_index, Vector3f target_position) {

			if (in_curve.size() < 4) {
				return false;
			}

			if (control_point_index > in_curve.size() - 1) {
				return false;
			}

			if (in_curve.original_points_.size() != in_curve.size()) {

				in_curve.set_original_shape();
			}

			std::vector<Vector3f> curve = in_curve.original_points_;

			Eigen::MatrixXd V(curve.size(), 3);
			Eigen::MatrixXd U(V.cols(), V.rows());
			Eigen::MatrixXi F((size_t)((GRdouble)curve.size() / 2.0), 3);

			for (GRuint i(0); i < curve.size(); i++) {
				V.row(i) = to_eigen(curve[i]);
			}

			for (GRuint i(0); i < F.rows() - 1; i++) {
				F.row(i) = Eigen::Vector3i(i * 2, i * 2 + 1, i * 2 + 2);
			}
			F.row(F.rows() - 1) = Eigen::Vector3i(V.rows() - 3, V.rows() - 2, V.rows() - 1);

			U = V;

			Eigen::VectorXi S, b;

			if (control_point_index == 0 || control_point_index == curve.size() - 1) {
				b = Eigen::VectorXi(2);
			}
			else {
				b = Eigen::VectorXi(3);
				b(2) = control_point_index;
			}

			b(0) = 0;
			b(1) = (int)(curve.size() - 1);

			Eigen::MatrixXd bc(b.size(), V.cols());

			Eigen::VectorXd new_position(to_eigen(target_position));

			if (control_point_index == 0) {
				bc.row(0) = new_position;
			}
			else {
				bc.row(0) = to_eigen(in_curve.front().first);
			}

			if (control_point_index == (V.rows() - 1)) {
				bc.row(1) = new_position;
			}else{
				bc.row(1) = to_eigen(in_curve.back().first);
			}

			if (bc.rows() == 3) {
				bc.row(2) = new_position;
			}

			/*std::cout << " V : " << V << std::endl;
			std::cout << "F : " << F << std::endl;
			std::cout << " b : " << b << std::endl;
			std::cout << " bc : " << bc << std::endl;*/

			igl::ARAPData arap_data;
			arap_data.energy = igl::ARAP_ENERGY_TYPE_SPOKES;
			arap_data.max_iter = 100;
			arap_data.with_dynamics = true;

			igl::arap_precomputation(V, F, V.cols(), b, arap_data);

			igl::arap_solve(bc, arap_data, U);

			for (GRuint i(0); i < V.rows(); i++) {
				in_curve[i].first = to_vec3(U.row(i));
			}
			in_curve.update_tangents();

			return true;
		}

	};
};


#pragma once

#include "cho_util/core/geometry/geometry_base.hpp"

#include <Eigen/Core>

namespace cho {
namespace core {

template <typename _Scalar, int N>
class Sphere : public GeometryBase<Sphere<_Scalar, N>> {
  using Scalar = _Scalar;

 public:
  explicit constexpr Sphere() {}
  Sphere(std::initializer_list<Scalar> args) {
    std::copy(args.begin(), args.end(), data_.data());
  }

  const Scalar* GetPtr() const { return data_.data(); }
  Scalar* GetPtr() { return data_.data(); }
  static constexpr int GetSize() { return N + 1; }

  auto& GetData() { return data_; }
  const auto& GetData() const { return data_; }

  auto GetCenter() { return data_.template head<N>(); }
  auto& GetRadius() { return data_(N); }
  const auto GetCenter() const { return data_.template head<N>(); }
  const auto& GetRadius() const { return data_(N); }

  template <typename Derived>
  void SetCenter(const Eigen::DenseBase<Derived>& center) {
    data_.template head<N>() = center;
  }

  void SetRadius(const Scalar radius) { data_(N) = radius; }

 private:
  // [Center, Radius]
  Eigen::Matrix<Scalar, N + 1, 1> data_;

 public:
  friend class GeometryBase<Sphere>;
  // EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

template <typename _Scalar, int N>
struct GeometryTraits<Sphere<_Scalar, N>> {
  using Scalar = _Scalar;
};

}  // namespace core
}  // namespace cho


/** @file
 *****************************************************************************

 Test for Merkle tree.

 *****************************************************************************
 * @author     This file is part of libzerocash, developed by the Zerocash
 *             project and contributors (see AUTHORS).
 * @copyright  MIT license (see LICENSE file)
 *****************************************************************************/

#include "libzerocash/IncrementalMerkleTree.h"

#include <iostream>
#include <vector>

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

using namespace libzerocash;
using namespace std;

void constructNonzeroTestVector(std::vector< std::vector<bool> > &values, uint32_t size)
{
    values.resize(0);
    std::vector<bool> dummy;
    dummy.resize(256);
    dummy[0] = true;

    for (uint32_t i = 0; i < size; i++)
    {
        values.push_back(dummy);
    }
}

void constructZeroTestVector(std::vector< std::vector<bool> > &values, uint32_t size)
{
    values.resize(0);
    std::vector<bool> dummy;
    dummy.resize(256);

    for (uint32_t i = 0; i < size; i++)
    {
        values.push_back(dummy);
    }
}

size_t countOnes(std::vector<bool> bits)
{
    size_t count = 0;
    for (size_t i = 0; i < bits.size(); i++) {
        if (bits.at(i)) {
            count++;
        }
    }
    return count;
}

BOOST_AUTO_TEST_CASE( testRootOfTreeOfZerosIsZero ) {
    IncrementalMerkleTree incTree;
    std::vector< std::vector<bool> > values;
    std::vector<bool> actual_root;

    constructZeroTestVector(values, 2);

    // Create an IncrementalMerkleTree over the values.
    if (incTree.insertVector(values) == false) {
        BOOST_ERROR("Could not insert into the tree.");
    }
    incTree.prune();
    incTree.getRootValue(actual_root);

    std::vector<bool> expected_root(32*8, 0);
    BOOST_CHECK( expected_root == actual_root );
}

BOOST_AUTO_TEST_CASE( testRootOfTreeOfNonZeroIsNonZero ) {
    IncrementalMerkleTree incTree;
    std::vector< std::vector<bool> > values;
    std::vector<bool> actual_root;

    constructNonzeroTestVector(values, 2);

    // Create an IncrementalMerkleTree over the values.
    if (incTree.insertVector(values) == false) {
        BOOST_ERROR("Could not insert into the tree.");
    }
    incTree.prune();
    incTree.getRootValue(actual_root);

    std::vector<bool> expected_root(32*8, 0);
    BOOST_CHECK( expected_root != actual_root );
}

BOOST_AUTO_TEST_CASE( testCompactRepresentation ) {
    for (uint32_t num_entries = 0; num_entries < 100; num_entries++) {
        std::vector< std::vector<bool> > values;
        std::vector<bool> root1, root2;
        IncrementalMerkleTree incTree(64);

        constructNonzeroTestVector(values, num_entries);

        BOOST_REQUIRE( incTree.insertVector(values) );
        BOOST_REQUIRE( incTree.prune() );

        IncrementalMerkleTreeCompact compact = incTree.getCompactRepresentation();

        BOOST_REQUIRE( compact.getTreeHeight() == 64 );

        // Calculate what the path to the next-added element should be.
        std::vector<unsigned char> path_bytes(8);
        std::vector<bool> path_bits;
        libzerocash::convertIntToBytesVector(num_entries, path_bytes);
        libzerocash::convertBytesVectorToVector(path_bytes, path_bits);

        // Make sure the paths match.
        BOOST_REQUIRE( compact.getHashList() == path_bits );
        BOOST_REQUIRE( compact.getHashListBytes() == path_bytes );

        // Make sure there's a hash for every '1' bit down the path.
        BOOST_REQUIRE( compact.getHashVec().size() == countOnes(path_bits) );

        // Make sure 'restoring' the tree results in the same root.
        IncrementalMerkleTree newTree(compact);
        incTree.getRootValue(root1);
        incTree.getRootValue(root2);
        BOOST_REQUIRE( root1 == root2 );
    }
}


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/ifix.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/ldexp.hpp>

STF_CASE_TPL (" ifix real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ifix;
  using r_t = decltype(ifix(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<T, signed>));

  // specific values tests
  STF_EQUAL(ifix(T(2)*bs::Valmax<r_t>()),  bs::Valmax<r_t>());
  STF_EQUAL(ifix(T(2)*bs::Valmin<r_t>()),  bs::Valmin<r_t>());
  STF_EQUAL(ifix(T(1.5)*bs::Valmax<r_t>()),  bs::Valmax<r_t>());
  STF_EQUAL(ifix(T(1.5)*bs::Valmin<r_t>()),  bs::Valmin<r_t>());

  STF_EQUAL(ifix(bs::Inf<T>()),  bs::Inf<r_t>());
  STF_EQUAL(ifix(bs::Minf<T>()), bs::Minf<r_t>());
  STF_EQUAL(ifix(bs::Mone<T>()), bs::Mone<r_t>());
  STF_EQUAL(ifix(bs::Nan<T>()),  bs::Zero<r_t>());
  STF_EQUAL(ifix(bs::One<T>()),  bs::One<r_t>());
  STF_EQUAL(ifix(bs::Zero<T>()), bs::Zero<r_t>());

  T v = T(1);
  r_t iv = 1;
  int N = sizeof(T)*8-1;
  for(int i=0; i < N ; i++, v*= 2, iv <<= 1)
  {
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
    STF_EQUAL(ifix(v), iv);
    STF_EQUAL(ifix(-v), -iv);
  }
  STF_EQUAL(ifix(bs::ldexp(bs::One<T>(), N)), bs::Valmax<r_t>());
  STF_EQUAL(ifix(bs::ldexp(bs::One<T>(), N+1)), bs::Valmax<r_t>());
  STF_EQUAL(ifix(-bs::ldexp(bs::One<T>(), N+1)), bs::Valmin<r_t>());
  STF_EQUAL(ifix(-bs::ldexp(bs::One<T>(), N+1)), bs::Valmin<r_t>());

} // end of test for floating_

STF_CASE_TPL (" ifix unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::ifix;
  using r_t = decltype(ifix(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<T, signed>));

  // specific values tests
  STF_EQUAL(ifix(bs::One<T>()),  bs::One<r_t>());
  STF_EQUAL(ifix(bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for unsigned_int_

STF_CASE_TPL (" ifix signed",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::ifix;
  using r_t = decltype(ifix(T()));
  // return type conformity test
  STF_TYPE_IS(r_t, (bd::as_integer_t<T, signed>));

  // specific values tests
  STF_EQUAL(ifix(bs::Mone<T>()), bs::Mone<r_t>());
  STF_EQUAL(ifix(bs::One<T>()),  bs::One<r_t>());
  STF_EQUAL(ifix(bs::Zero<T>()), bs::Zero<r_t>());
} // end of test for signed_int_


/*
 * This file belongs to the Galois project, a C++ library for exploiting parallelism.
 * The code is being released under the terms of the 3-Clause BSD License (a
 * copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

#include "Point.h"
#include "Cavity.h"
#include "Verifier.h"

#include "galois/Galois.h"
#include "galois/Bag.h"
#include "galois/Timer.h"
#include "galois/graphs/SpatialTree.h"
#include "Lonestar/BoilerPlate.h"
#include "llvm/Support/CommandLine.h"

#include "galois/runtime/Profile.h"

#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/counting_iterator.hpp>

#include <algorithm>
#include <deque>
#include <fstream>
#include <iostream>
#include <limits>
#include <vector>

#include <string.h>
#include <unistd.h>

namespace cll = llvm::cl;

static const char* name = "Delaunay Triangulation";
static const char* desc =
    "Produces a Delaunay triangulation for a set of points";
static const char* url = "delaunay_triangulation";

static cll::opt<std::string>
    inputname(cll::Positional, cll::desc("<input file>"), cll::Required);
static cll::opt<std::string>
    doWriteMesh("writemesh",
                cll::desc("Write the mesh out to files with basename"),
                cll::value_desc("basename"));

using Tree = typename galois::graphs::SpatialTree2d<Point*>;

//! All Point* refer to elements in this bag
using basePointBag = typename galois::InsertBag<Point>;

//! [Define Insert Bag]
using ptrPointBag = typename galois::InsertBag<Point*>;

//! Our main functor
struct Process {
  Graph& graph;
  Tree& tree;
  ptrPointBag& ptrPoints;

  Process(Graph& g, Tree& t, ptrPointBag& p)
      : graph(g), tree(t), ptrPoints(p) {}

  typedef galois::PerIterAllocTy Alloc;

  struct ContainsTuple {
    const Graph& graph;
    Tuple tuple;
    ContainsTuple(const Graph& g, const Tuple& t) : graph(g), tuple(t) {}
    bool operator()(const GNode& n) const {
      assert(!graph.getData(n, galois::MethodFlag::UNPROTECTED).boundary());
      return graph.getData(n, galois::MethodFlag::UNPROTECTED)
          .inTriangle(tuple);
    }
  };

  void computeCenter(const Element& e, Tuple& t) const {
    for (int i = 0; i < 3; ++i) {
      const Tuple& o = e.getPoint(i)->t();
      for (int j = 0; j < 2; ++j) {
        t[j] += o[j];
      }
    }
    for (int j = 0; j < 2; ++j) {
      t[j] *= 1 / 3.0;
    }
  }

  void findBestNormal(const Element& element, const Point* p,
                      const Point*& bestP1, const Point*& bestP2) {
    Tuple center(0);
    computeCenter(element, center);
    int scale = element.clockwise() ? 1 : -1;

    Tuple origin = p->t() - center;
    //        double length2 = origin.x() * origin.x() + origin.y() *
    //        origin.y();
    bestP1 = bestP2 = NULL;
    double bestVal  = 0.0;
    for (int i = 0; i < 3; ++i) {
      int next = i + 1;
      if (next > 2)
        next -= 3;

      const Point* p1 = element.getPoint(i);
      const Point* p2 = element.getPoint(next);
      double dx       = p2->t().x() - p1->t().x();
      double dy       = p2->t().y() - p1->t().y();
      Tuple normal(scale * -dy, scale * dx);
      double val = normal.dot(origin); // / length2;
      if (bestP1 == NULL || val > bestVal) {
        bestVal = val;
        bestP1  = p1;
        bestP2  = p2;
      }
    }
    assert(bestP1 != NULL && bestP2 != NULL && bestVal > 0);
  }

  GNode findCorrespondingNode(GNode start, const Point* p1, const Point* p2) {
    for (auto ii : graph.edges(start)) {
      GNode dst  = graph.getEdgeDst(ii);
      Element& e = graph.getData(dst, galois::MethodFlag::UNPROTECTED);
      int count  = 0;
      for (int i = 0; i < e.dim(); ++i) {
        if (e.getPoint(i) == p1 || e.getPoint(i) == p2) {
          if (++count == 2)
            return dst;
        }
      }
    }
    GALOIS_DIE("unreachable");
    return start;
  }

  bool planarSearch(const Point* p, GNode start, GNode& node) {
    // Try simple hill climbing instead
    ContainsTuple contains(graph, p->t());
    while (!contains(start)) {
      Element& element = graph.getData(start, galois::MethodFlag::WRITE);
      if (element.boundary()) {
        // Should only happen when quad tree returns a boundary point which is
        // rare There's only one way to go from here
        assert(std::distance(graph.edge_begin(start), graph.edge_end(start)) ==
               1);
        start = graph.getEdgeDst(
            graph.edge_begin(start, galois::MethodFlag::WRITE));
      } else {
        // Find which neighbor will get us to point fastest by computing normal
        // vectors
        const Point *p1, *p2;
        findBestNormal(element, p, p1, p2);
        start = findCorrespondingNode(start, p1, p2);
      }
    }

    node = start;
    return true;
  }

  bool findContainingElement(const Point* p, GNode& node) {
    Point** rp = tree.find(p->t().x(), p->t().y());
    if (!rp)
      return false;

    (*rp)->get(galois::MethodFlag::WRITE);

    GNode someNode = (*rp)->someElement();

    // Not in mesh yet
    if (!someNode) {
      GALOIS_DIE("unreachable");
      return false;
    }

    return planarSearch(p, someNode, node);
  }

  void generateMesh() {
    typedef galois::worklists::PerThreadChunkLIFO<32> CA;
    galois::for_each(galois::iterate(ptrPoints),
                     [&, self = this](Point* p, auto& ctx) {
                       p->get(galois::MethodFlag::WRITE);
                       assert(!p->inMesh());

                       GNode node;
                       if (!self->findContainingElement(p, node)) {
                         // Someone updated an element while we were searching,
                         // producing a semi-consistent state ctx.push(p);
                         // Current version is safe with locking so this
                         // shouldn't happen
                         GALOIS_DIE("unreachable");
                         return;
                       }

                       assert(self->graph.getData(node).inTriangle(p->t()));
                       assert(self->graph.containsNode(node));

                       Cavity<Alloc> cav(self->graph, ctx.getPerIterAlloc());
                       cav.init(node, p);
                       cav.build();
                       cav.update();
                       self->tree.insert(p->t().x(), p->t().y(), p);
                     },
                     galois::no_pushes(), galois::per_iter_alloc(),
                     galois::loopname("Main"), galois::wl<CA>());
  }
};

typedef std::vector<Point> PointList;

class ReadPoints {
  void addBoundaryPoints() {
    double minX, maxX, minY, maxY;

    minX = minY = std::numeric_limits<double>::max();
    maxX = maxY = std::numeric_limits<double>::min();

    for (const auto& p : points) {
      double x = p.t().x();
      double y = p.t().y();
      if (x < minX)
        minX = x;
      else if (x > maxX)
        maxX = x;
      if (y < minY)
        minY = y;
      else if (y > maxY)
        maxY = y;
    }

    tree.init(minX, minY, maxX, maxY);

    size_t size      = points.size();
    double width     = maxX - minX;
    double height    = maxY - minY;
    double maxLength = std::max(width, height);
    double centerX   = minX + width / 2.0;
    double centerY   = minY + height / 2.0;
    double radius =
        maxLength * 3.0; // radius of circle that should cover all points

    for (int i = 0; i < 3; ++i) {
      double dX = radius * cos(2 * M_PI * (i / 3.0));
      double dY = radius * sin(2 * M_PI * (i / 3.0));
      points.push_back(Point(centerX + dX, centerY + dY, size + i));
    }
  }

  void nextLine(std::ifstream& scanner) {
    scanner.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
  }

  void fromTriangle(std::ifstream& scanner) {
    double x, y;
    long numPoints;

    scanner >> numPoints;

    int dim;
    scanner >> dim;
    assert(dim == 2);
    int k;
    scanner >> k; // number of attributes
    assert(k == 0);
    scanner >> k; // has boundary markers?

    for (long id = 0; id < numPoints; ++id) {
      scanner >> k; // point id
      scanner >> x >> y;
      nextLine(scanner);
      points.push_back(Point(x, y, id));
    }
  }

  void fromPointList(std::ifstream& scanner) {
    double x, y;

    // comment line
    nextLine(scanner);
    size_t id = 0;
    while (!scanner.eof()) {
      scanner >> x >> y;
      if (x == 0 && y == 0)
        break;
      points.push_back(Point(x, y, id++));
      x = y = 0;
      nextLine(scanner);
    }
  }

  PointList& points;
  Tree& tree;

public:
  ReadPoints(PointList& p, Tree& t) : points(p), tree(t) {}

  void from(const std::string& name) {
    std::ifstream scanner(name.c_str());
    if (!scanner.good()) {
      GALOIS_DIE("Could not open file: ", name);
    }
    if (name.find(".node") == name.size() - 5) {
      fromTriangle(scanner);
    } else {
      fromPointList(scanner);
    }
    scanner.close();

    if (points.size())
      addBoundaryPoints();
    else {
      GALOIS_DIE("No points found in file: ", name);
    }
  }
};

struct ReadInput {
  Graph& graph;
  Tree& tree;
  basePointBag& basePoints;
  ptrPointBag& ptrPoints;

  ReadInput(Graph& g, Tree& t, basePointBag& b, ptrPointBag& p)
      : graph(g), tree(t), basePoints(b), ptrPoints(p) {}

  void addBoundaryNodes(Point* p1, Point* p2, Point* p3) {
    Element large_triangle(p1, p2, p3);
    GNode large_node = graph.createNode(large_triangle);
    graph.addNode(large_node);

    p1->addElement(large_node);
    p2->addElement(large_node);
    p3->addElement(large_node);

    tree.insert(p1->t().x(), p1->t().y(), p1);

    Element border_ele1(p1, p2);
    Element border_ele2(p2, p3);
    Element border_ele3(p3, p1);

    GNode border_node1 = graph.createNode(border_ele1);
    GNode border_node2 = graph.createNode(border_ele2);
    GNode border_node3 = graph.createNode(border_ele3);

    graph.addNode(border_node1);
    graph.addNode(border_node2);
    graph.addNode(border_node3);

    graph.getEdgeData(graph.addEdge(large_node, border_node1)) = 0;
    graph.getEdgeData(graph.addEdge(large_node, border_node2)) = 1;
    graph.getEdgeData(graph.addEdge(large_node, border_node3)) = 2;

    graph.getEdgeData(graph.addEdge(border_node1, large_node)) = 0;
    graph.getEdgeData(graph.addEdge(border_node2, large_node)) = 0;
    graph.getEdgeData(graph.addEdge(border_node3, large_node)) = 0;
  }

  struct centerXCmp {
    template <typename T>
    bool operator()(const T& lhs, const T& rhs) const {
      return lhs.t().x() < rhs.t().x();
    }
  };

  struct centerYCmp {
    template <typename T>
    bool operator()(const T& lhs, const T& rhs) const {
      return lhs.t().y() < rhs.t().y();
    }
  };

  struct centerYCmpInv {
    template <typename T>
    bool operator()(const T& lhs, const T& rhs) const {
      return rhs.t().y() < lhs.t().y();
    }
  };

  template <typename Iter>
  void divide(const Iter& b, const Iter& e) {
    if (std::distance(b, e) > 64) {
      std::sort(b, e, centerXCmp());
      Iter m = galois::split_range(b, e);
      std::sort(b, m, centerYCmpInv());
      std::sort(m, e, centerYCmp());
      divide(b, galois::split_range(b, m));
      divide(galois::split_range(b, m), m);
      divide(m, galois::split_range(m, e));
      divide(galois::split_range(m, e), e);
    } else {
      std::random_shuffle(b, e);
    }
  }

  void layoutPoints(PointList& points) {
    divide(points.begin(), points.end() - 3);
    galois::do_all(galois::iterate(points.begin(), points.end() - 3),
                   [&](Point& p) {
                     Point* pr = &basePoints.push(p);
                     ptrPoints.push(pr);
                   });
    //! [Insert elements into InsertBag]
    Point* p1 = &basePoints.push(*(points.end() - 1));
    Point* p2 = &basePoints.push(*(points.end() - 2));
    Point* p3 = &basePoints.push(*(points.end() - 3));
    //! [Insert elements into InsertBag]
    addBoundaryNodes(p1, p2, p3);
  }

  void operator()(const std::string& filename) {
    PointList points;
    ReadPoints(points, tree).from(filename);

    std::cout << "configuration: " << points.size() << " points\n";

    galois::preAlloc(2 * numThreads // some per-thread state
                     + 2 * points.size() *
                           sizeof(Element) // mesh is about 2x number of points
                                           // (for random points)
                           * 32            // include graph node size
                           / (galois::runtime::pagePoolSize()) // in pages
    );
    galois::reportPageAlloc("MeminfoPre");

    layoutPoints(points);
  }
};

static void writePoints(const std::string& filename, const PointList& points) {
  std::ofstream out(filename.c_str());
  // <num vertices> <dimension> <num attributes> <has boundary markers>
  out << points.size() << " 2 0 0\n";
  // out.setf(std::ios::fixed, std::ios::floatfield);
  out.setf(std::ios::scientific, std::ios::floatfield);
  out.precision(10);
  long id = 0;
  for (const auto& p : points) {
    const Tuple& t = p.t();
    out << id++ << " " << t.x() << " " << t.y() << " 0\n";
  }

  out.close();
}

static void writeMesh(const std::string& filename, Graph& graph) {
  long numTriangles = 0;
  long numSegments  = 0;
  for (auto n : graph) {
    Element& e = graph.getData(n);
    if (e.boundary()) {
      numSegments++;
    } else {
      numTriangles++;
    }
  }

  long tid = 0;
  long sid = 0;
  std::string elementName(filename);
  std::string polyName(filename);

  elementName.append(".ele");
  polyName.append(".poly");

  std::ofstream eout(elementName.c_str());
  std::ofstream pout(polyName.c_str());
  // <num triangles> <nodes per triangle> <num attributes>
  eout << numTriangles << " 3 0\n";
  // <num vertices> <dimension> <num attributes> <has boundary markers>
  // ...
  // <num segments> <has boundary markers>
  pout << "0 2 0 0\n";
  pout << numSegments << " 1\n";
  for (auto n : graph) {
    const Element& e = graph.getData(n);
    if (e.boundary()) {
      // <segment id> <vertex> <vertex> <is boundary>
      pout << sid++ << " " << e.getPoint(0)->id() << " " << e.getPoint(1)->id()
           << " 1\n";
    } else {
      // <triangle id> <vertex> <vertex> <vertex> [in ccw order]
      eout << tid++ << " " << e.getPoint(0)->id() << " ";
      if (e.clockwise()) {
        eout << e.getPoint(2)->id() << " " << e.getPoint(1)->id() << "\n";
      } else {
        eout << e.getPoint(1)->id() << " " << e.getPoint(2)->id() << "\n";
      }
    }
  }

  eout.close();
  // <num holes>
  pout << "0\n";
  pout.close();
}

int main(int argc, char** argv) {
  galois::SharedMemSys G;
  LonestarStart(argc, argv, name, desc, url);

  Graph graph;
  Tree tree;
  basePointBag basePoints;
  ptrPointBag ptrPoints;

  ReadInput(graph, tree, basePoints, ptrPoints)(inputname);

  galois::StatTimer T;
  T.start();
  galois::runtime::profileVtune(
      [&]() { Process(graph, tree, ptrPoints).generateMesh(); },
      "MeshGeneration");
  T.stop();
  std::cout << "mesh size: " << graph.size() << "\n";

  galois::reportPageAlloc("MeminfoPost");

  if (!skipVerify) {
    Verifier verifier;
    if (!verifier.verify(&graph)) {
      GALOIS_DIE("Triangulation failed");
    }
    std::cout << "Triangulation OK\n";
  }

  if (doWriteMesh.size()) {
    std::string base = doWriteMesh;
    std::cout << "Writing " << base << "\n";
    writeMesh(base.c_str(), graph);

    PointList points;
    // Reordering messes up connection between id and place in pointlist
    ReadPoints(points, tree).from(inputname);
    writePoints(base.append(".node"), points);
  }

  return 0;
}


// This file is part of LatticeTester.
//
// LatticeTester
// Copyright (C) 2012-2021  The LatNet Builder author's, supervised by Pierre L'Ecuyer, Universite de Montreal.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "netbuilder/Helpers/JoeKuo.h"
#include "netbuilder/Helpers/Path.h"
#include <cmath>

#include <string>
#include <fstream>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>

using namespace std;

namespace NetBuilder { namespace JoeKuo {

//===========================================================================

LatticeTester::Weight Weights::getWeight (const LatticeTester::Coordinates& projection) const
{
   if (projection.size()==2)
   {
         return std::pow((LatticeTester::Weight).9999, *projection.rbegin());
   }
   // fall back to zero
   return 0.0;
}

//===========================================================================

void Weights::format(ostream& os) const
{
   using LatticeTester::operator<<;
   os << "Weights()";
}

//===========================================================================

const char* ws = " \t\n\r\f\v";

// trim from end (right)
inline std::string& rtrim(std::string& s, const char* t = ws)
{
      s.erase(s.find_last_not_of(t) + 1);
      return s;
}

// trim from beginning (left)
inline std::string& ltrim(std::string& s, const char* t = ws)
{
      s.erase(0, s.find_first_not_of(t));
      return s;
}

// trim from both ends (left & right)
inline std::string& trim(std::string& s, const char* t = ws)
{
      return ltrim(rtrim(s, t), t);
}

std::vector<std::vector<uInteger>> readJoeKuoDirectionNumbers(Dimension dimension)
{
      assert(dimension >= 1 && dimension <= 21201);
      std::string path = PATH_TO_LATNETBUILDER_DIR + "/../share/latnetbuilder/data/JoeKuoSobolNets.csv";
      std::vector<std::vector<uInteger>> res(dimension);
      if (boost::filesystem::exists(path)){
            std::ifstream file(path);
            std::string sent;

            do
            {
            getline(file,sent);
            trim(sent);
            }
            while (sent != "###");

            getline(file,sent);

            for(unsigned int i = 1; i <= dimension; ++i)
            {
                  if(getline(file,sent))
                  {
                        std::vector<std::string> fields;
                        boost::split( fields, sent, boost::is_any_of( ";" ) );
                        for( const auto& token : fields)
                        {
                              res[i-1].push_back(std::stol(token));
                        }
                  }
                  else
                  {
                        break;
                  }
            }
      }
      else{
            throw runtime_error("Unable to locate data folder. The value of PATH_TO_LATNETBUILDER_DIR is probably incorrect. See netbuilder/Path.h.");
      }
      return res;
}

std::vector<DirectionNumbers> getJoeKuoDirectionNumbers(Dimension dimension)
{
      std::vector<std::vector<uInteger>> tmp = readJoeKuoDirectionNumbers(dimension);
      std::vector<DirectionNumbers> genVals(dimension);
      for(unsigned int j = 0; j < dimension; ++j)
      {
            genVals[j] = DirectionNumbers(j,tmp[j]);
      }
      return genVals;
}

DigitalNet<NetConstruction::SOBOL> createJoeKuoSobolNet(Dimension dimension, MatrixSize size)
{
      auto genVals = getJoeKuoDirectionNumbers(dimension);
      return DigitalNet<NetConstruction::SOBOL>(dimension, size, std::move(genVals));
}

std::unique_ptr<DigitalNet<NetConstruction::SOBOL>> createPtrToJoeKuoSobolNet(Dimension dimension, MatrixSize size)
{
      auto genVals = getJoeKuoDirectionNumbers(dimension);
      return std::make_unique<DigitalNet<NetConstruction::SOBOL>>(dimension, size, std::move(genVals));
}

}} // namespace