#include <utility>
#include <iostream>
#include <algorithm>
#include <functional>

#include <boost/range/adaptor/filtered.hpp>
#include <boost/range/algorithm/set_algorithm.hpp>

template<class D, class S>
template<class Functor, class Iterator>
void
dionysus::Rips<D,S>::
generate(Dimension k, DistanceType max, const Functor& f, Iterator bg, Iterator end) const
{
    auto neighbor = [this, max](Vertex u, Vertex v) { return this->distances()(u,v) <= max; };

    // current      = empty
    // candidates   = everything
    VertexContainer current;
    VertexContainer candidates(bg, end);
    bron_kerbosch(current, candidates, std::prev(candidates.begin()), k, neighbor, f);
}

template<class D, class S>
template<class Functor, class Iterator>
void
dionysus::Rips<D,S>::
vertex_cofaces(IndexType v, Dimension k, DistanceType max, const Functor& f, Iterator bg, Iterator end) const
{
    auto neighbor = [this, max](Vertex u, Vertex v) { return this->distances()(u,v) <= max; };

    // current      = [v]
    // candidates   = everything - [v]
    VertexContainer current; current.push_back(v);
    VertexContainer candidates;
    for (Iterator cur = bg; cur != end; ++cur)
        if (*cur != v && neighbor(v, *cur))
            candidates.push_back(*cur);

    bron_kerbosch(current, candidates, std::prev(candidates.begin()), k, neighbor, f);
}

template<class D, class S>
template<class Functor, class Iterator>
void
dionysus::Rips<D,S>::
edge_cofaces(IndexType u, IndexType v, Dimension k, DistanceType max, const Functor& f, Iterator bg, Iterator end) const
{
    auto neighbor = [this, max](Vertex u, Vertex v) { return this->distances()(u,v) <= max; };

    // current      = [u,v]
    // candidates   = everything - [u,v]
    VertexContainer current; current.push_back(u); current.push_back(v);

    VertexContainer candidates;
    for (Iterator cur = bg; cur != end; ++cur)
        if (*cur != u && *cur != v && neighbor(v,*cur) && neighbor(u,*cur))
            candidates.push_back(*cur);

    bron_kerbosch(current, candidates, std::prev(candidates.begin()), k, neighbor, f);
}

template<class D, class S>
template<class Functor, class Iterator>
void
dionysus::Rips<D,S>::
cofaces(const Simplex& s, Dimension k, DistanceType max, const Functor& f, Iterator bg, Iterator end) const
{
    namespace ba = boost::adaptors;

    auto neighbor = [this, max](Vertex u, Vertex v) { return this->distances()(u,v) <= max; };

    // current      = s
    VertexContainer current(s.begin(), s.end());

    // candidates   = everything - s     that is a neighbor of every vertex in the simplex
    VertexContainer candidates;
    boost::set_difference(std::make_pair(bg, end) |
                                ba::filtered([this,&s,&neighbor](Vertex cur)
                                             { for (auto& v : s)
                                                   if (!neighbor(v, cur))
                                                       return false;
                                             }),
                          s,
                          std::back_inserter(candidates));

    bron_kerbosch(current, candidates, std::prev(candidates.begin()), k, neighbor, f, false);
}

template<class D, class S>
template<class Functor, class NeighborTest>
void
dionysus::Rips<D,S>::
bron_kerbosch(VertexContainer&                          current,
              const VertexContainer&                    candidates,
              typename VertexContainer::const_iterator  excluded,
              Dimension                                 max_dim,
              const NeighborTest&                       neighbor,
              const Functor&                            functor,
              bool                                      check_initial)
{
    if (check_initial && !current.empty())
        functor(Simplex(current));

    if (current.size() == static_cast<size_t>(max_dim) + 1)
        return;

    for (auto cur = std::next(excluded); cur != candidates.end(); ++cur)
    {
        current.push_back(*cur);

        VertexContainer new_candidates;
        for (auto ccur = candidates.begin(); ccur != cur; ++ccur)
            if (neighbor(*ccur, *cur))
                new_candidates.push_back(*ccur);
        size_t ex = new_candidates.size();
        for (auto ccur = std::next(cur); ccur != candidates.end(); ++ccur)
            if (neighbor(*ccur, *cur))
                new_candidates.push_back(*ccur);
        excluded  = new_candidates.begin() + (ex - 1);

        bron_kerbosch(current, new_candidates, excluded, max_dim, neighbor, functor);
        current.pop_back();
    }
}

template<class Distances_, class Simplex_>
typename dionysus::Rips<Distances_, Simplex_>::DistanceType
dionysus::Rips<Distances_, Simplex_>::
distance(const Simplex& s1, const Simplex& s2) const
{
    DistanceType mx = 0;
    for (auto a : s1)
        for (auto b : s2)
            mx = std::max(mx, distances_(a,b));
    return mx;
}

template<class Distances_, class Simplex_>
typename dionysus::Rips<Distances_, Simplex_>::DistanceType
dionysus::Rips<Distances_, Simplex_>::
max_distance() const
{
    DistanceType mx = 0;
    for (IndexType a = distances_.begin(); a != distances_.end(); ++a)
        for (IndexType b = std::next(a); b != distances_.end(); ++b)
            mx = std::max(mx, distances_(a,b));
    return mx;
}

template<class Distances_, class Simplex_>
typename dionysus::Rips<Distances_, Simplex_>::DistanceType
dionysus::Rips<Distances_, Simplex_>::Evaluator::
operator()(const Simplex& s) const
{
    DistanceType mx = 0;
    for (auto a = s.begin(); a != s.end(); ++a)
        for (auto b = std::next(a); b != s.end(); ++b)
            mx = std::max(mx, distances_(*a,*b));
    return mx;
}


/**
 * @file solver.hpp
 * @author Paul Kirth
 * @date 3/20/15
 */

#ifndef _SOLVER_HPP_
#define _SOLVER_HPP_

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

struct node {
  size_t count;
  size_t id;
  bool val;
};

struct clause {
  int a;
  int b;
};

struct solution {
  boost::dynamic_bitset<> ans;
  int value;
  solution(size_t size = 0, int val = 0) : ans(size) { value = val; };
  solution(boost::dynamic_bitset<> bs, int val = 0) : ans(bs), value(val) {}
};

/**
 * Finds solutions for MAX 2 SAT instances
 * has exact solution, and approximate solutions
 *
 */
class solver {
public:
  solver(std::string file);

  solution exact();
  solution approx();
  void reset()
  {
    x.reset();
  }

private:
  boost::dynamic_bitset<> x;
  std::vector<clause> clauses;
  std::vector<node> nodes;

  inline bool check(const clause &c) noexcept {
    bool a;
    bool b;
    int ida = abs(c.a), idb = abs(c.b);
    a = (c.a > 0) ? x[ida] : !x[ida];
    b = (c.b > 0) ? x[idb] : !x[idb];
    return a || b;
  }

  void increment(boost::dynamic_bitset<> &bitset) {
    auto len = std::max(bitset.size(), (size_t)1);
    // printf("count:%d\nlen:  %d\n", bitset.count(), len);
    for (int loop = 0; loop < len; ++loop) {
      if ((bitset[loop] ^= 0x1) == 0x1) {
        break;
      }
    }
  }

}; // end class solver

#endif // end solver.hpp


/*
 * =====================================================================================
 *
 *       Filename:  roman_numerals.cpp
 *
 *    Description:
 *
 *        Version:  1.0
 *        Created:  16.08.2015 21:17:23
 *       Revision:  none
 *       Compiler:  gcc
 *
 *
 * =====================================================================================
 */

#include "roman_numerals.h"
#include <array>
#include <boost/tuple/tuple.hpp>
#include <iostream>

namespace roman
{
	typedef std::array< boost::tuple<int, char, int>,7> transMat;
	typedef std::array< boost::tuple<int, char, int>,7>::const_iterator transMatIt;
	typedef std::array< boost::tuple<int, char, int>,7>::const_reverse_iterator transMatRevIt;

	static const transMat transformMatrix {{
		{1,'I',3}, {5,'V',1}, {10,'X',3}, {50,'L',1}, {100,'C',3}, {500,'D',1}, {1000,'M',3} }};

	class numeralConversion
	{
		public:
			numeralConversion(unsigned a): arabic_{a}, revIt{transformMatrix.rbegin()}, revEnd{transformMatrix.rend()}, roman_{""}{convertToRoman();}

			std::string getRoman() const
			{
				return roman_;
			}
		private:

			void convertToRoman()
			{
				for( transMatRevIt it{revIt}; (it !=revEnd and arabic_ >0); ++it)
				{
					while( arabic_ >= it->get<0>())
					{
						roman_.push_back(it->get<1>());
						arabic_ -= it->get<0>();
					}

					checkForTurnedRomans(it);
				}
			}

			/* checkForTurnedRomans controlls whether the arabic numeral can be appended to roman_
			 * as a smaller roman numeral substraced from a higher roman numeral i.e.: IX
			 */
			void checkForTurnedRomans( transMatRevIt Revit)
			{
				int originalInt{Revit->get<0>()};
				char originalChar{Revit->get<1>()};
				++Revit;
				for(transMatIt it{transformMatrix.begin()} ; it != Revit.base(); ++it)
				{
					if( ((originalInt - it->get<0>()) <= arabic_) and arabic_ > 0 and it->get<2>()== 3)
					{
						roman_.push_back(it->get<1>());
						roman_.push_back(originalChar);
						arabic_ -= originalInt - it->get<0>();
						break;
					}
				}
			}

			/*
			 * arabic_ stores the value thats supposed to be transformed, respectively the
			 * untransformed rest
			 */
			unsigned arabic_;

			/*
			 * roman_ stores the characters eventually given back the roman numeral
			 */
			std::string roman_;

			/*
			 * A reverse_iterator on the the reverse begin of the global transformmatrix transformMatrix.
			 * used because the program requires checks from highest to lowest value
			 */
			transMatRevIt revIt;

			/*
			 * A reverse_iterator on the the reverse end (one before the first value) of the global transformmatrix transMat.
			 * used to stop for-loop running the reverse_iterator revIt
			 */
			transMatRevIt revEnd;

	};

	std::string convert(unsigned a)
	{
		return numeralConversion(a).getRoman();
	}
}


#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include "parse.hpp"

CodeSequence parse_code_sequence(const std::string& str) {

    const auto strs = split(str, " ");

    std::vector<CodeNumber> code_numbers{};
    for (const auto& num : strs) {
        const auto n = boost::lexical_cast<CodeNumber>(num);
        code_numbers.push_back(n);
    }

    return CodeSequence{code_numbers};
}

InitialAngles parse_initial_angles(const std::string& str) {
    if (str == "xy") {
        return InitialAngles{XYZ::X, XYZ::Y};
    } else if (str == "xz") {
        return InitialAngles{XYZ::X, XYZ::Z};
    } else if (str == "yx") {
        return InitialAngles{XYZ::Y, XYZ::X};
    } else if (str == "yz") {
        return InitialAngles{XYZ::Y, XYZ::Z};
    } else if (str == "zx") {
        return InitialAngles{XYZ::Z, XYZ::X};
    } else if (str == "zy") {
        return InitialAngles{XYZ::Z, XYZ::Y};
    } else {
        throw std::runtime_error("parse_initial_angles: unable to parse: " + str);
    }
}

static bool is_digit(const char ch) {
    return ('0' <= ch) && (ch <= '9');
}

static Coeff32 parse_number(const std::string& num) {
    if (num.empty()) {
        return 1;
    } else if (num == "-") {
        return -1;
    } else {
        // The input string should match this regex: D+ U +D+ U -D+, where D is 0-9
        // If it doesn't, it should throw an exception
        return boost::lexical_cast<Coeff32>(num);
    }
}

LinComArrZ<XY> parse_lin_com_arr_xy(const std::string& str) {

    LinComArrZ<XY> builder{};

    if (str == "0") {
        return builder;
    }

    size_t index = 0;
    size_t length = 0;
    for (size_t i = 0; i < str.length(); i += 1) {
        const char ch = str.at(i);
        if (ch == '-') {
            index = i;
            length = 1;
        } else if (ch == 'x') {
            const auto coeff = parse_number(str.substr(index, length));
            builder.add(coeff, XY::X);
            // not parsing number any more
            length = 0;

        } else if (ch == 'y') {
            const auto coeff = parse_number(str.substr(index, length));
            builder.add(coeff, XY::Y);
            // not parsing number any more
            length = 0;

        } else if (is_digit(ch)) {
            if (length == 0) {
                // we are not reading a number currently,
                // so restart
                index = i;
            }

            length += 1;

        } else if (ch == '+') {
            // the next character should be the start of an integer
            // 3x+y
            index = i + 1;
            length = 0;

        } else {
            std::ostringstream err;
            err << "unrecognized character '" << ch << "' in \"" << str << "\" at position " << i;
            throw std::runtime_error(err.str());
        }
    }

    return builder;
}

LinComArrZ<XYPi> parse_lin_com_arr_xypi(const std::string& str) {

    LinComArrZ<XYPi> builder{};

    if (str == "0") {
        return builder;
    }

    size_t index = 0;
    size_t length = 0;
    for (size_t i = 0; i < str.length(); i += 1) {
        const char ch = str.at(i);
        if (ch == '-') {
            index = i;
            length = 1;
        } else if (ch == 'x') {
            const auto coeff = parse_number(str.substr(index, length));
            builder.add(coeff, XYPi::X);
            // not parsing number any more
            length = 0;

        } else if (ch == 'y') {
            const auto coeff = parse_number(str.substr(index, length));
            builder.add(coeff, XYPi::Y);
            // not parsing number any more
            length = 0;

        } else if (ch == 'p') {
            const char next_char = str.at(i + 1);
            if (next_char == 'i') {
                const auto coeff = parse_number(str.substr(index, length));
                builder.add(coeff, XYPi::Pi);
                length = 0;
                // skip over the 'i'
                i += 1;
            } else {
                std::ostringstream err;
                err << "unrecognized character '" << ch << "' in \"" << str << "\" at position " << i;
                throw std::runtime_error(err.str());
            }
        } else if (is_digit(ch)) {
            if (length == 0) {
                // we are not reading a number currently,
                // so restart
                index = i;
            }

            length += 1;

        } else if (ch == '+') {
            // the next character should be the start of an integer
            // 3x+y
            index = i + 1;
            length = 0;

        } else {
            std::ostringstream err;
            err << "unrecognized character '" << ch << "' in \"" << str << "\" at position " << i;
            throw std::runtime_error(err.str());
        }
    }

    return builder;
}

LinComArrZ<XYEta> parse_lin_com_arr_xyeta(const std::string& str) {

    LinComArrZ<XYEta> builder{};

    if (str == "0") {
        return builder;
    }

    size_t index = 0;
    size_t length = 0;
    for (size_t i = 0; i < str.length(); i += 1) {
        const char ch = str.at(i);
        if (ch == '-') {
            index = i;
            length = 1;
        } else if (ch == 'x') {
            const auto coeff = parse_number(str.substr(index, length));
            builder.add(coeff, XYEta::X);
            // not parsing number any more
            length = 0;

        } else if (ch == 'y') {
            const auto coeff = parse_number(str.substr(index, length));
            builder.add(coeff, XYEta::Y);
            // not parsing number any more
            length = 0;

        } else if (ch == 'e') {
            const char next_char = str.at(i + 1);
            const char next_next_char = str.at(i + 2);
            if ((next_char == 't') && (next_next_char == 'a')) {
                const auto coeff = parse_number(str.substr(index, length));
                builder.add(coeff, XYEta::Eta);
                length = 0;
                // skip over the "ta"
                i += 2;
            } else {
                std::ostringstream err;
                err << "unrecognized characters \"" << next_char << next_next_char << "\" in \"" << str << "\" at position " << i;
                throw std::runtime_error(err.str());
            }
        } else if (is_digit(ch)) {
            if (length == 0) {
                // we are not reading a number currently,
                // so restart
                index = i;
            }

            length += 1;

        } else if (ch == '+') {
            // the next character should be the start of an integer
            // 3x+y
            index = i + 1;
            length = 0;

        } else {
            std::ostringstream err;
            err << "unrecognized character '" << ch << "' in \"" << str << "\" at position " << i;
            throw std::runtime_error(err.str());
        }
    }

    return builder;
}

template <template <typename> class Trig>
LinComMapZ<Trig<LinComArrZ<XY>>> parse_lin_com_map_xy(const std::string& str, const std::string& trig) {

    LinComMapZ<Trig<LinComArrZ<XY>>> builder{};

    if (str == "0") {
        return builder;
    }

    size_t index = 0;
    size_t length = 0;
    for (size_t i = 0; i < str.length(); i += 1) {
        const char ch = str.at(i);
        if (ch == '-') {
            index = i;
            length = 1;
        } else if (ch == trig.at(0)) {
            const auto coeff = parse_number(str.substr(index, length));

            // here is where the magic happens
            if (str.substr(i, 4) != (trig + '(')) {
                std::ostringstream err;
                err << "\"" << str.substr(i, 4) << "\" != \"" << trig << "(\" in \"" << str << "\" at position " << i;
                throw std::runtime_error(err.str());
            }

            const size_t ind = str.find(')', i);

            const auto arg = parse_lin_com_arr_xy(str.substr(i + 4, ind - i - 4));

            builder.add(coeff, Trig<LinComArrZ<XY>>{arg});
            // now skip past the sin
            i = ind;
            // not parsing number any more
            length = 0;

        } else if (is_digit(ch)) {
            if (length == 0) {
                // we are not reading a number currently,
                // so restart
                index = i;
            }

            length += 1;

        } else if (ch == '+') {
            // the next character should be the start of an integer
            // 3x+y
            index = i + 1;
            length = 0;

        } else {
            std::ostringstream err;
            err << "unrecognized character '" << ch << "' in \"" << str << "\" at position " << i;
            throw std::runtime_error(err.str());
        }
    }

    return builder;
}

LinComMapZ<Sin<LinComArrZ<XY>>> parse_lin_com_map_sin_xy(const std::string& str) {
    return parse_lin_com_map_xy<Sin>(str, "sin");
}

LinComMapZ<Cos<LinComArrZ<XY>>> parse_lin_com_map_cos_xy(const std::string& str) {
    return parse_lin_com_map_xy<Cos>(str, "cos");
}


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_ML2_DATA_BASIC_COLUMN_STATISTICS_H_
#define TURI_ML2_DATA_BASIC_COLUMN_STATISTICS_H_

#include <core/data/flexible_type/flexible_type.hpp>
#include <core/logging/assertions.hpp>
#include <core/storage/serialization/serialization_includes.hpp>
#include <toolkits/ml_data_2/ml_data_column_modes.hpp>
#include <core/parallel/pthread_tools.hpp>
#include <boost/thread/lock_algorithms.hpp>
#include <mutex>

namespace turi { namespace v2 { namespace ml_data_internal {

extern size_t ML_DATA_STATS_PARALLEL_ACCESS_THRESHOLD;

/**
 * column_metadata contains "meta data" concerning indexing of a single column
 * of an SFrame. A collection of meta_data column objects is "all" the
 * metadata required in the ml_data container.
 */
class basic_column_statistics : public column_statistics {

 public:

  ////////////////////////////////////////////////////////////
  // Functions to access the statistics

  /** Returns the number of seen by the methods collecting the
   *  statistics.
   */
  size_t num_observations() const {
    return total_row_count;
  }

  /* The count; index here is the index obtained by one of the
   * map_value_to_index functions previously.
   */
  size_t count(size_t index) const {
    // Use this to
    if(mode == ml_column_mode::NUMERIC
       || mode == ml_column_mode::NUMERIC_VECTOR) {

      return total_row_count;
    } else {
      return index < counts.size() ? counts[index] : 0;
    }
  }

  /* The mean; index here is the index obtained by one of the
   * map_value_to_index functions previously.
   */
  double mean(size_t index) const {
    if(mode == ml_column_mode::CATEGORICAL
       || mode == ml_column_mode::CATEGORICAL_VECTOR) {

      double p = double(count(index)) / (std::max(1.0, double(total_row_count)));
      return p;

    } else {

      if(total_row_count)
        DASSERT_TRUE(!statistics.empty());

      return index < statistics.size() ? statistics[index].mean : 0;
    }
  }

  /* The variance; index here is the index obtained by one of the
   * map_value_to_index functions previously.
   */
  double stdev(size_t index) const {
    if(mode == ml_column_mode::CATEGORICAL
       || mode == ml_column_mode::CATEGORICAL_VECTOR) {

      double stdev = 0;
      double p = mean(index);

      if (total_row_count > 1) {
        stdev = std::sqrt(total_row_count * p * (1 - p) / (total_row_count-1));
      } else {
        stdev = 0;
      }

      return stdev;

    } else {

      if(total_row_count)
        DASSERT_TRUE(!statistics.empty());

      return index < statistics.size() ? statistics[index].stdev : 0;
    }
  }

  ////////////////////////////////////////////////////////////
  // Routines for updating the statistics.  This is done online, while
  // new categories are being added, etc., so we have to be

  /// Initialize the statistics -- counting, mean, and stdev
  void initialize();

  /// Update categorical statistics for a batch of categorical indices.
  void update_categorical_statistics(size_t thread_idx, const std::vector<size_t>& cat_index_vect) GL_HOT_FLATTEN;

  /// Update categorical statistics for a batch of real values.
  void update_numeric_statistics(size_t thread_idx, const std::vector<double>& value_vect) GL_HOT_FLATTEN;

  /// Update statistics after observing a dictionary.
  void update_dict_statistics(size_t thread_idx, const std::vector<std::pair<size_t, double> >& dict) GL_HOT_FLATTEN;

  /// Perform final computations on the different statistics.  Must be
  /// called after all the data is filled.
  void finalize();

 private:
  /// Perform finalizations; split between the threadlocal stuff and the global stuff.
  void _finalize_threadlocal(size_t, bool, bool);
  void _finalize_global(size_t, bool, bool);

  ////////////////////////////////////////////////////////////////////////////////
  // Stuff for the saving and loading

  /** Returns the current serialization version of this model.
   */
  size_t get_version() const { return 2; }

  /**
   *  Serialize the object (save).
   */
  void save_impl(turi::oarchive& oarc) const;

  /**
   *  Load the object.
   */
  void load_version(turi::iarchive& iarc, size_t version);

  /** For debugging purposes.
   */
  bool is_equal(const column_statistics* other_ptr) const;

  /** Create a copy with the index cleared.
   */
   std::shared_ptr<column_statistics> create_cleared_copy() const;

 private:

  ////////////////////////////////////////////////////////////////////////////////
  // Functions for updating the categorical variables

  // Put all of these in one single structure, and store a vector of
  // these structures.  Since these are always accessed together, this
  // reduces cache misses on the lookups -- we now only have one fetch
  // instead of three.

  std::vector<size_t> counts;

  struct element_statistics : public turi::IS_POD_TYPE {
    double mean  = 0;   /**< Mean of column. */
    double stdev = 0;  /**< Stdev of column. */
  };

  std::vector<element_statistics> statistics;
  size_t total_row_count = 0;

  // The issue with having seperate accumulators for each thread is
  // that it can take an inordinate amount of memory.  Thus we use
  // parallel access for the first million or so items, which are
  // likely to be the most common.  For the rest, we use a larger one
  // with locking.

  size_t parallel_threshhold = 1024*1024;

  std::vector<size_t> by_thread_row_counts;

  struct element_statistics_accumulator : public turi::IS_POD_TYPE {
    double mean    = 0;  /**< Mean of column. */
    double var_sum = 0;  /**< Stdev of column. */
  };

  std::vector<std::vector<size_t> > by_thread_element_counts;
  std::vector<std::vector<element_statistics_accumulator> > by_thread_mean_var_acc;

  // The locks are done by
  static constexpr size_t n_locks = 64; // Should be power of 2

  std::array<simple_spinlock, n_locks> global_element_locks;
  std::vector<size_t> global_element_counts;
  std::vector<element_statistics_accumulator> global_mean_var_acc;

  volatile size_t global_size = 0;

  /**  Return the index of the appropriate lock.
   *
   */
  inline size_t get_lock_index(size_t idx) const {
    size_t lock_idx = (idx % n_locks);
    return lock_idx;
  }

  /**  Possibly resize the local array.
   */
  template <typename T>
  inline void check_local_array_size(size_t idx, std::vector<T>& v) {
    DASSERT_LT(idx, parallel_threshhold);

      // See if a resize is needed.
    if(idx >= v.size() ) {

      if(UNLIKELY(v.capacity() < idx + 1)) {
        size_t new_capacity = std::min(parallel_threshhold, 3 * (idx + 1) / 2);

        // If it's likely to go to max capacity, just make it that to avoid future resizes.
        if(new_capacity > parallel_threshhold / 2)
          new_capacity = parallel_threshhold;

        v.reserve(new_capacity);
      }

      v.resize(idx + 1);
    }
  }

  /**  Check global array size. Possibly resize it.
   */
  template <typename T>
  inline void check_global_array_size(size_t idx, std::vector<T>& v) {

    // If needed, increase the value of global_size.
    if(global_size <= idx) {

      do {
        size_t g = global_size;

        if(g > idx)
          break;

        bool success = atomic_compare_and_swap(global_size, g, idx+1);

        if(!success)
          continue;

      } while(false);
    }

    // See if a resize is needed.
    if(UNLIKELY(idx >= v.size() )) {

      // Grow aggressively, since a resize is really expensive.
      size_t new_size = 2 * (parallel_threshhold + idx + 1);

      {
        std::array<std::unique_lock<simple_spinlock>, n_locks> all_locks;
        for(size_t i = 0; i < n_locks; ++i)
          all_locks[i] = std::unique_lock<simple_spinlock>(global_element_locks[i], std::defer_lock);

        // Ensure nothing is happening with the vector by locking all
        // locks in a thread safe way.  This prevents any thread from
        // accessing it while we resize it.
        boost::lock(all_locks.begin(), all_locks.end());

        // It's possible that another thread beat us to it.
        if(v.size() < idx + 1)
          v.resize(new_size);

        // The destructor of all_locks takes care of the unlocking.
      }
    }
  }

  /** One way to set the statistics.  Used by the serialization converters.
   *
   *  "counts" -- std::vector<size_t>.  Counts.
   *
   *  "mean" -- std::vector<double>.  Mean.
   *
   *  "stdev" -- std::vector<double>.  std dev.
   *
   *  "total_row_count" -- size_t.  Total row count.
   */
  void set_data(const std::map<std::string, variant_type>& params);

};

}}}

#endif


//===------------------------------------------------------------*- C++ -*-===//
///
/// \file
/// \brief 轴对齐盒子。
///
/// \sa [Eigen::AlignedBox](https://eigen.tuxfamily.org/dox/classEigen_1_1AlignedBox.html)
///
/// \version 2021-11-05
/// \since 2021-10-26
/// \authors zhengrr
/// \copyright Unlicense
///
//===----------------------------------------------------------------------===//

#include <Eigen/Geometry>  // 几何
#include <gtest/gtest.h>

/// \brief 扩展。
/// \details 扩展 `*this`，使其包含点 `p`，并返回一个对 `*this` 的引用。
TEST(AlignedBox, Extend)
{
    using AlignedBox = Eigen::AlignedBox2i;
    using Point = Eigen::Vector2i;
    using Vector = Eigen::Vector2i;

    AlignedBox box;
    EXPECT_TRUE(box.isEmpty());

    box.extend(Point {3, 5});
    EXPECT_EQ(box.sizes(), Vector(0, 0));

    box.extend(Point {1, 1});
    EXPECT_EQ(box.sizes(), Vector(2, 4));
}


///////////////////////////////////////////////////////////////////////////////
// mpl::rest.hpp                              				 				 //
//                                                                           //
//  Copyright 2010 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_MPL_REST_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_MPL_REST_HPP_ER_2010
#include <boost/mpl/iterator_range.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/next.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace mpl{

	template<typename Seq>
    struct rest : boost::mpl::iterator_range<
        typename boost::mpl::next<
            typename boost::mpl::begin<Seq>::type
        >::type,
        typename boost::mpl::end<Seq>::type
    >{};

}// mpl
}// detail
}// statistics
}// boost

#endif


// Copyright 2008 The Native Client SDK Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can
// be found in the LICENSE file.

#include "examples/scriptable/matrix/matrix_comp.h"

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <cstdio>
#include <sstream>
#include <vector>

#include <ppapi/cpp/instance.h>
#include <ppapi/cpp/module.h>

// constructor for the MatrixScriptableObject class -- does logging for debug
MatrixScriptableObject::MatrixScriptableObject()
    : pp::deprecated::ScriptableObject() {
}

// destructor for the MatrixScriptableObject class
MatrixScriptableObject::~MatrixScriptableObject() {
}

// Converts an pp::Var to a number of type T.
// Type <class T> would normally be an int, float, double, unsigned int, etc.
// and so whether AsInt() or AsDouble() is used,
// we end up with it static_cast to the desired type...
template <class T>
class ppVarToNumber {
  public:
    ppVarToNumber() : conversion_error_(false) { }

    // functor that tries to convert |arg| to a number
    // Returns a value of type |T|
    // Indicates an error occurred by setting |conversion_error_| to true
    //   and putting an explanation in |error_message_|
    T operator()(const pp::Var& arg) {
       // clear out old/existing error & message
       conversion_error_ = false;
       error_message_ = "";

       if (arg.is_double()) {
         return static_cast<T>(arg.AsDouble());
       } else if (arg.is_int()) {
         return static_cast<T>(arg.AsInt());
       } else if (arg.is_bool()) {
         return static_cast<T>(arg.AsBool());
       } else if (arg.is_string()) {
         std::string result = arg.AsString();
         std::stringstream ss(result);  // create a string-stream from |result|
         T val = 0;
         // try to read a number (type T) from the string-stream
         if ((ss >> val).fail()) {
            conversion_error_ = true;
            error_message_ = "Error converting string ";
            error_message_ += arg.DebugString();
            error_message_ += " to a number";
         }
         return val;
       }
       // error case
       conversion_error_ = true;
       error_message_ = "Error converting unhandled type " + arg.DebugString()
                        + " to a number";
       return 0;
    }
    // Getters for conversion_error_ and error_message_
    bool conversion_error() const { return conversion_error_;}
    std::string error_message() const { return error_message_;}
  private:
    bool conversion_error_;
    std::string error_message_;
};

// GetNthArray takes a pp::Var |var| that is known to be a Javascript array
// and gets |index| element of the array.  Note that |index| has to be
// converted to a string and accessed as a property.  If an error_occurs
// converting retrieving the property with that index or converting the
// pp::Var for that property to a double, then |error_occurred| is set to true.
double GetNthArrayElement(const pp::Var& var, int index, bool* error_occurred) {
  pp::Var exception;
  *error_occurred = false;
  std::ostringstream oss;
  oss << index;

  bool has_prop = var.HasProperty(oss.str(), &exception);
  if (has_prop) {
    pp::Var indexVar = var.GetProperty(oss.str(), &exception);

    ppVarToNumber<double> dbl_converter;
    double value = dbl_converter(indexVar);
    if (dbl_converter.conversion_error()) {
      return 0.0;
    }
    return value;
  } else {
    *error_occurred = true;
  }
  return 0.0;
}

//
// InvertMatrix -- this function is copied from the Effective UBLAS page
//   for boost libraries:
// http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?Effective_UBLAS
// The algorithm is here:
// http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?LU_Matrix_Inversion
// and is based on "Numerical Recipies in C", 2nd ed.,
// by Press, Teukolsky, Vetterling & Flannery.
//
template<class T>
bool InvertMatrix(const boost::numeric::ublas::matrix<T>& input,
  boost::numeric::ublas::matrix<T>& inverse) {
  // create a working copy of the input
  boost::numeric::ublas::matrix<T> A(input);
  // create a permutation matrix for the LU-factorization
  typedef boost::numeric::ublas::permutation_matrix<std::size_t> pmatrix;
  pmatrix pm(A.size1());

  // perform LU-factorization
  int res = boost::numeric::ublas::lu_factorize(A, pm);
  if (res != 0)
    return false;

  // create identity matrix of "inverse"
  inverse.assign(boost::numeric::ublas::identity_matrix<T> (A.size1()));

  // backsubstitute to get the inverse
  boost::numeric::ublas::lu_substitute(A, pm, inverse);
  return true;
}

// Convert a Boost matrix |boost_matrix| to a simple html table
// Uses ostringstream to write a mix of strings and numbers into a stream,
//  and then convert it all to a string
// The |title| and |border| inputs are for formatting the table
// The html table is returned as a string
std::string ConvertMatrixToHtml(
    std::string title, unsigned int border,
    boost::numeric::ublas::matrix<double> boost_matrix ) {
  std::ostringstream ss;
  ss << title << "
<table border=\"" << border << "\">";
  for (unsigned int i = 0; i < boost_matrix.size1(); ++i) {
    ss <<  "
";
    for (unsigned int j = 0; j < boost_matrix.size2(); ++j) {
      ss << " " << boost_matrix(i, j) << " 	";
    }
    ss <<  "
";
  }
  ss << "
";
  return ss.str();
}

// Called by Javascript -- this is a static method to process the matrix that
// is passed as a Javascript array object.  The array object is a 1-d array
// whose first two values are integer values for the width and height
// of the matrix, and then the rest of the values are the 2D matrix content as
// a series of string values.  This function ensures we have a single object
// argument, then grabs its "length" property, and then the properties for
// the indices: "0", "1" ... up to length-1
std::string MatrixScriptableObject::ComputeUsingArray(
    const std::vector<pp::Var>& args) {

  // There should be exactly one arg, which should be an object
  if (args.size() != 1) {
    return "Unexpected number of args";
  }
  if (!args[0].is_object()) {
    return "Arg from Javascript is not an object!";
  }

  pp::Var exception;  // Use initial "void" Var for exception
  pp::Var arg0 = args[0];
  pp::Var var_length = arg0.GetProperty("length", &exception);

  if (!exception.is_undefined()) {
    printf("Error getting 'length' of JS object\n");
    return "Error getting length property of JS object";
  }
  int length = var_length.AsInt();

  // The length should be > 2 since we always have a width and height plus data
  if (length <= 2) {
    // then we didn't get width, height, and then the cell values...
    return "Error: Invalid length";
  }

  // grab the width and height
  bool error_occurred = false;
  int width = static_cast<int>(GetNthArrayElement(arg0, 0, &error_occurred));
  int height = static_cast<int>(GetNthArrayElement(arg0, 1, &error_occurred));

  boost::numeric::ublas::matrix<double>  initial_matrix(height, width);
  // the transpose will have |width| rows....instead of |width| columns
  boost::numeric::ublas::matrix<double> trans_matrix(width, height);

  int row = 0, column = 0;

  // Fill the initial_matrix with values from the JS array object
  for (int i = 2; i < length; ++i) {
    double cur_value = GetNthArrayElement(arg0, i, &error_occurred);
    if (error_occurred) {
      return "Error getting matrix values";
    } else {
      // got good value from Javascript array
      initial_matrix(row, column) = cur_value;
      ++column;
      if (column == width) {
        column = 0;
        ++row;
      }
    }
  }

  // Note -- throughout the program I pass in various values for the 3rd
  // argument |border| which controls the size of the border in the
  // html string that is created by ConvertMatrixToHtml.  There is not a
  // special reason for the different values -- but I wanted to call it
  // with different values so that the result visual effect in html can
  // be seen on the matrix.html web page.
  std::string full_message = ConvertMatrixToHtml(
    "Orig matrix from JS array", 1, initial_matrix);

  // transpose the matrix
  trans_matrix = boost::numeric::ublas::trans(initial_matrix);

  full_message += ConvertMatrixToHtml("Transposed matrix from JS array",
    2, trans_matrix);

  return full_message;
}

// Called by Javascript -- this is a static method to process the matrix that
// is passed as a series of doubles in |args|. The first two values are the
// width and height of the matrix -- the rest of the values are the array
// contents.
std::string MatrixScriptableObject::ComputeAnswer(
    const std::vector<pp::Var>& args) {
  std::vector<double> input_vector;

  if (args.size() < 2) {
    return "Error in ComputeAnswer, arg_count too small";
  }
  int matrix_width = 0, matrix_height = 0;

  ppVarToNumber<double> dbl_converter;
  ppVarToNumber<int> int_converter;

  matrix_width = int_converter(args[0]);
  if (int_converter.conversion_error()) {
    return int_converter.error_message();
  }

  matrix_height = int_converter(args[1]);
  if (int_converter.conversion_error()) {
    return int_converter.error_message();
  }

  if (matrix_width <= 0 || matrix_height <= 0) {
    return "Invalid matrix sizes";
  }

  for (unsigned int index = 2; index < args.size(); ++index) {
    double arg_value = dbl_converter(args[index]);
    if (dbl_converter.conversion_error()) {
      return dbl_converter.error_message();
    } else {
      input_vector.push_back(arg_value);
    }
  }

  // filling matrix data
  boost::numeric::ublas::matrix<double>
    matrix_data(matrix_height, matrix_width);

  int column = 0, row = 0;
  for ( std::vector<double>::iterator iter = input_vector.begin();
        iter != input_vector.end(); ++iter) {
    double value = *iter;
    matrix_data(row, column) = value;
    ++column;
    if (column >= matrix_width) {
      column = 0;
      ++row;
    }
  }

  std::string js_message;   // message to send back to Javascript
  js_message = ConvertMatrixToHtml("Original Matrix:", 3, matrix_data);

  if ((matrix_height == matrix_width) && matrix_height>0) {
    std::string inverted_matrix_string;

    boost::numeric::ublas::matrix<double>
      inverted_matrix(matrix_height, matrix_width);

    bool can_be_inverted = InvertMatrix(matrix_data, inverted_matrix);

    if (can_be_inverted) {
      inverted_matrix_string = ConvertMatrixToHtml("Inverted Matrix:", 4,
        inverted_matrix);
      // now let's multiply matrix_data X inverted_matrix and print it
      // to visually double-check the inverse
      boost::numeric::ublas::matrix<double>
        prod_matrix(matrix_height, matrix_width);

      prod_matrix = prod(matrix_data, inverted_matrix);

      inverted_matrix_string += ConvertMatrixToHtml(
        "Matrix Multiply -- Original Matrix x Inverse Matrix:", 4, prod_matrix);
    } else {
      inverted_matrix_string = "MATRIX CANNOT BE INVERTED";
    }
    js_message += inverted_matrix_string;
  }

  return js_message;
}

// The Instance class.  One of these exists for each instance of your NaCl
// module on the web page.  The browser will ask the Module object to create
// a new Instance for each occurence of the <embed> tag that has these
// attributes:
//     type="application/x-ppapi-nacl-srpc"
//     nexes="ARM: hello_world_arm.nexe
//            ..."
// The Instance can return a ScriptableObject representing itself.  When the
// browser encounters JavaScript that wants to access the Instance, it calls
// the GetInstanceObject() method.  All the scripting work is done though
// the returned ScriptableObject.
class MatrixInstance : public pp::Instance {
 public:
  explicit MatrixInstance(PP_Instance instance) : pp::Instance(instance) {}
  virtual ~MatrixInstance() {}

  // The pp::Var takes over ownership of the MatrixInstance.
  virtual pp::Var GetInstanceObject() {
    MatrixScriptableObject* hw_object = new MatrixScriptableObject();
    return pp::Var(this, hw_object);
  }
};

// The Module class.  The browser calls the CreateInstance() method to create
// an instance of your NaCl module on the web page.
class MatrixModule : public pp::Module {
 public:
  MatrixModule() : pp::Module() {}
  virtual ~MatrixModule() {}

  // Create and return a MatrixInstance object.
  virtual pp::Instance* CreateInstance(PP_Instance instance) {
    return new MatrixInstance(instance);
  }
};

// Factory function called by the browser when the module is first loaded.
// The browser keeps a singleton of this module.  It calls the
// CreateInstance() method on the object you return to make instances.  There
// is one instance per <embed> tag on the page.  This is the main binding
// point for your NaCl module with the browser.
namespace pp {
Module* CreateModule() {
  return new MatrixModule();
}
}  // namespace pp


#include <stdio.h>
#include <string>

#define CLOOG_INT_GMP

#include <cloog/cloog.h>
#include <osl/scop.h>

#include <boost/python.hpp>

using namespace boost::python;

void tosigma(const char * filein, const char * fileout)
{

  CloogState *state;
  CloogInput *input;
  CloogOptions * options ;
  struct clast_stmt *root;

  state = cloog_state_malloc();
  options = cloog_options_malloc(state);
  options->openscop = 1;
  options->strides = 1;

  FILE * fin = fopen(filein, "r");
  FILE * fout = fopen(fileout, "w");
  osl_scop_p scop = osl_scop_read(fin);

	input = cloog_input_from_osl_scop(state, scop);
	cloog_options_copy_from_osl_scop(scop, options);

  root = cloog_clast_create_from_input(input, options);
  clast_pprint(fout, root, 0, options);

  fclose(fin);
  fclose(fout);
  cloog_clast_free(root);
  cloog_options_free(options) ;
  cloog_state_free(state);

}

object tosigma_str(const char * filein)
{

  CloogState *state;
  CloogInput *input;
  CloogOptions * options ;
  struct clast_stmt *root;

  state = cloog_state_malloc();
  options = cloog_options_malloc(state);
  options->openscop = 1;
  options->strides = 1;

  FILE * fin = fopen(filein, "r");
  osl_scop_p scop = osl_scop_read(fin);

	input = cloog_input_from_osl_scop(state, scop);
	cloog_options_copy_from_osl_scop(scop, options);

  root = cloog_clast_create_from_input(input, options);
  std::string sigma = sll_pprint("", root, 0, options);

  fclose(fin);
  cloog_clast_free(root);
  cloog_options_free(options) ;
  cloog_state_free(state);

  return object(sigma);
}

BOOST_PYTHON_MODULE(sigmacloog)
{
    def("tosigma", tosigma);
    def("tosigma_str", tosigma_str);
}


/*
 * HCTree.hpp
 *
 *  Created on: Jan 20, 2014
 *      Author: andresf
 */

#ifndef HCMTREE_HPP_
#define HCMTREE_HPP_

#include <boost/iostreams/filter/gzip.hpp>
#include <boost/iostreams/filtering_stream.hpp>

#include <CentersChooser.h>
#include <DynamicMat.hpp>

#include <stdlib.h>

namespace vlr {

typedef uchar TDescriptor;
typedef cv::Hamming Distance;
typedef typename Distance::ResultType DistanceType;

struct HCTreeParams: public cvflann::IndexParams {
	HCTreeParams(int branching = 10, int maxLeafSize = 7) {
		// Branching factor
		(*this)["branching"] = branching;
		// Maximum leaf size
		(*this)["maxLeafSize"] = maxLeafSize;
	}
};

class HCTree {

private:

	/**
	 * Structure representing a node in the hierarchical k-means tree.
	 */
	struct HCTreeNode {
		// The node id
		int nodeId;
		// The cluster center
		TDescriptor* center;
		// Children nodes (only for non-terminal nodes)
		HCTreeNode** children;
		// Empty constructor
		HCTreeNode() :
				nodeId(-1), center(NULL), children(NULL) {
		}
//		// Assignment operator
//		HCTreeNode& operator=(const HCTreeNode& node) {
//			nodeId = node.nodeId;
//			if (node.center != NULL) {
//				// Deep copy
//				center = new TDescriptor[m_veclen];
//				for (size_t k = 0; k < m_veclen; ++k) {
//					center[k] = node.center[k];
//				}
//			} else {
//				center = NULL;
//			}
//			children = node.children;
//			return *this;
//		}
	};

	typedef HCTreeNode* HCTreeNodePtr;

protected:

	/** Attributes useful for building the tree **/
	// The data set over which to build the tree
	vlr::Mat& m_dataset;

	/** Attributes of the tree **/
	// Branching factor (number of partitions in which
	// data is divided at each level of the tree)
	int m_branching;
	// Threshold on the number of points inside a cluster
	// to consider a node as a leaf
	int m_maxLeafSize;
	// Length of each feature
	size_t m_veclen;
	// Number of nodes in the tree
	size_t m_size;
	// The root node of the tree
	HCTreeNodePtr m_root;

	/** Other attributes **/
	Distance m_distance;

public:

	/**
	 * Class constructor.
	 *
	 * @param inputData - Reference to the matrix with the data to be clustered.
	 * @param params - Parameters to the hierarchical clustering tree
	 */
	HCTree(vlr::Mat& inputData = DEFAULT_INPUTDATA,
			const HCTreeParams& params = HCTreeParams());

	/**
	 * Class destroyer, releases the memory used by the tree.
	 */
	virtual ~HCTree();

	/**
	 * Returns the tree size.
	 *
	 * @return number of nodes in the tree
	 */
	size_t size() const;

	/**
	 * Returns the dimensionality of the data points being clustered.
	 *
	 * @return number of dimensions of clustering data
	 */
	size_t getVeclen() const {
		return m_veclen;
	}

	int getBranching() const {
		return m_branching;
	}

	int getMaxLeafSize() const {
		return m_maxLeafSize;
	}

	HCTreeNodePtr getRoot() const {
		return m_root;
	}

	/**
	 * Returns whether the tree is empty.
	 *
	 * @return true if and only if the tree is empty
	 */
	bool empty() const;

	/**
	 * Builds the tree.
	 */
	void build();

	/**
	 * Strict equality operator.
	 *
	 * @param other
	 * @return true if objects are equal, false otherwise
	 */
	bool operator==(const HCTree &other) const;

	/**
	 * Strict inequality operator.
	 *
	 * @param other
	 * @return true if objects are unequal, false otherwise
	 */
	bool operator!=(const HCTree &other) const;

	/**
	 * Saves the tree to a file stream.
	 *
	 * @param filename - The name of the file stream where to save the tree
	 */
	void save(const std::string& filename) const;

	/**
	 * Loads the tree from a file stream.
	 *
	 * @param filename - The name of the file stream from where to load the tree
	 */
	void load(const std::string& filename);

private:

	/**
	 * Recursively releases the memory allocated to store the tree node centers.
	 *
	 * @param node - A pointer to a node in the tree where to start the releasing
	 */
	void free_centers(HCTreeNodePtr node);

	/**
	 * Responsible with actually doing the recursive hierarchical clustering.
	 *
	 * @param node - The node to cluster
	 * @param indices - Indices of the points belonging to the current node
	 * @param indices_length
	 * @param level
	 * @param fitted
	 */
	void computeClustering(HCTreeNodePtr node, int* indices, int indices_length,
			int level, bool fitted);

	/**
	 * Saves to a stream the tree starting at a given node.
	 *
	 * @param fs - A reference to the file storage pointing to the file where to save the tree
	 * @param node - The node indicating the root of the tree to save
	 */
	void save_tree(cv::FileStorage& fs, HCTreeNodePtr node) const;

	/**
	 * Loads the tree from a stream and stores it into a given node pointer.
	 *
	 * @param filename - A reference to the file storage where to read node parameters
	 * @param node - The node where to store the loaded tree
	 */
	void load_tree(boost::iostreams::filtering_istream& is,
			HCTreeNodePtr& node);

	bool compareEqual(const HCTreeNodePtr a, const HCTreeNodePtr b) const;

	// Make private the copy constructor and the assignment operator
	// to prevent obtaining copies of the instance
	HCTree(HCTree const&); // Don't Implement
	void operator=(HCTree const&); // Don't implement

};

} /* namespace vlr */

#endif /* HCMTREE_HPP_ */


#include <boost/msm/mpl_graph/breadth_first_search.hpp>


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

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

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

// #include <boost/simd/function/scalar/abs.hpp>
// #include <boost/simd/function/scalar/exp10.hpp>
// #include <boost/simd/function/scalar/floor.hpp>
// #include <boost/simd/function/scalar/inc.hpp>
// #include <boost/simd/function/scalar/is_eqz.hpp>
// #include <boost/simd/function/scalar/rec.hpp>
// #include <boost/simd/function/scalar/selinc.hpp>
// #include <boost/simd/function/scalar/tofloat.hpp>
// #include <boost/simd/function/scalar/toint.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
//   BOOST_DISPATCH_OVERLOAD ( chop_
//                           , (typename A0)
//                           , bd::cpu_
//                           , bd::scalar_< bd::integer_<A0> >
//                           )
//   {
//     using result_t = A0;
//     BOOST_FORCEINLINE result_t operator() ( A0 const& a0) const BOOST_NOEXCEPT
//     {
//       return a0;
//     }
//   };

//   BOOST_DISPATCH_OVERLOAD ( chop_
//                           , (typename A0, typename A1)
//                           , bd::cpu_
//                           , bd::scalar_< bd::integer_<A0> >
//                           , bd::scalar_< bd::integer_<A1> >
//                           )
//   {
//     using result_t = A0;

//     BOOST_FORCEINLINE result_t operator() ( A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
//     {
//       return toint(chop(tofloat(a0), a1));
//     }
//   };

//   BOOST_DISPATCH_OVERLOAD ( chop_
//                           , (typename A0, typename A1)
//                           , bd::cpu_
//                           , bd::scalar_< bd::integer_<A0> >
//                           , bd::scalar_< bd::integer_<A1> >
//                           , bd::scalar_< bd::integer_<A2> >
//                           )
//   {
//     using result_t = A0;

//     BOOST_FORCEINLINE result_t operator() ( A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
//     {
//       return toint(chop(tofloat(a0), a1, a2));
//     }
//   };

//   BOOST_DISPATCH_OVERLOAD ( chop_
//                           , (typename A0)
//                           , bd::cpu_
//                           , bd::scalar_< bd::floating_<A0> >
//                           )
//   {
//     using result_t = A0;
//     BOOST_FORCEINLINE result_t operator() ( A0 const& a0) const BOOST_NOEXCEPT
//     {
//       A0 tmp = selinc(is_eqz(tmp), nt2::abs(x));
//       tmp = floor(inc(log10(tmp)));
//       inflate = exp10(- tmp);
//       A0 deflate = rec(inflate);
//       return deflate*chop(x*inflate);
//     }
//   };

//   BOOST_DISPATCH_OVERLOAD ( chop_
//                           , (typename A0, typename A1)
//                           , bd::cpu_
//                           , bd::scalar_< bd::floating_<A0> >
//                           , bd::scalar_< bd::integer_<A1> >
//                           )
//   {
//     using result_t = A0;

//     BOOST_FORCEINLINE result_t operator() ( A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
//     {
//       A0 tmp = selinc(is_eqz(tmp), nt2::abs(x));
//       tmp = floor(inc(log10(tmp)));
//       inflate = exp10(tofloat(a1) - tmp);
//       return rec(inflate)*chop(x*inflate);
//     }
//   };

//   BOOST_DISPATCH_OVERLOAD ( chop_
//                           , (typename A0, typename A1)
//                           , bd::cpu_
//                           , bd::scalar_< bd::floating_<A0> >
//                           , bd::scalar_< bd::integer_<A1> >
//                           , bd::scalar_< bd::integer_<A2> >
//                           )
//   {
//     using result_t = A0;

//     BOOST_FORCEINLINE result_t operator() ( A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
//     {
//       A0 tmp = selinc(is_eqz(tmp), nt2::abs(x));
//       tmp = floor(inc(log10(tmp)));
//       inflate = exp10(tofloat(a1) - tmp);
//       return a2*rec(inflate)*chop(chop(x*inflate)/a2);
//     }
//   };

} } }

#endif


#include <frovedis.hpp>
#include <frovedis/matrix/crs_matrix.hpp>
#include <frovedis/ml/nb/naive_bayes.hpp>

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

using namespace frovedis;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    std::string m_matf = "./input/m_mat";
    std::string m_lblf = "./input/m_lbl";
    std::string b_matf = "./input/b_mat";
    std::string b_lblf = "./input/b_lbl";

    auto b_mat = make_crs_matrix_load<double>(b_matf);
    auto b_lbl = make_dvector_loadline<double>(b_lblf);
    auto m1 = bernoulli_nb<double,crs_matrix<double>,crs_matrix_local<double>>(b_mat,b_lbl);
    auto b_tmat = make_crs_matrix_local_load<double>(b_matf);
    auto pd1 = m1.predict(b_tmat);
    //for(auto& e: pd1) std::cout << e << " "; std::cout << std::endl;

    auto m_mat = make_crs_matrix_load<double>(m_matf);
    auto m_lbl = make_dvector_loadline<double>(m_lblf);
    auto m2 = multinomial_nb<double,crs_matrix<double>,crs_matrix_local<double>>(m_mat,m_lbl);
    auto m_tmat = make_crs_matrix_local_load<double>(m_matf);
    auto pd2 = m2.predict(m_tmat);
    //for(auto& e: pd2) std::cout << e << " "; std::cout << std::endl;

    // confirming correctness of result
    std::vector<double> expected = {1,0,1,1,1,0,1,1};
    BOOST_CHECK (pd1 == expected && pd2 == expected);
}


#ifndef ROSE499_WORLD_HPP
#define ROSE499_WORLD_HPP

#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <list>
#include <vector>

namespace geom = boost::geometry;

struct World
{
    static constexpr double XMax = 430.0;
    static constexpr double YMax = 630.0;

    using Point = geom::model::point<double, 2, geom::cs::cartesian>;
    using Polygon = geom::model::polygon<Point>;
    using Box = geom::model::box<Point>;
    using RTree = geom::index::rtree< std::pair<Box, int>, geom::index::linear<3, 1> >;

    /** Construct empty world **/
    World() = default;

    void addBoxObstacle(int id, double x, double y, double w, double h);
    std::list< std::pair<Box, int> > observeWorld(Polygon viewregion) const;
    RTree const & sceneTree() const;

private:
    RTree mMap;
};

std::list< World::Point > planRRT(  World::RTree knownWorld,
                                    double xMax,
                                    double yMax,
                                    World::Point initialPoint,
                                    double direction,
                                    World::Point goal,
                                    double radius );

#endif


/**
 * @file windvector.cpp
 */

#include "windvector.h"
#include "forecast_time.h"
#include "lambert_conformal_grid.h"
#include "latitude_longitude_grid.h"
#include "level.h"
#include "logger.h"
#include "plugin_factory.h"
#include "stereographic_grid.h"
#include "util.h"
#include <boost/thread.hpp>
#include <iostream>
#include <math.h>

#include "cache.h"
#include "fetcher.h"

using namespace std;
using namespace himan::plugin;

#include "cuda_helper.h"

typedef tuple<double, double, double, double> coefficients;

boost::thread_specific_ptr<map<size_t, coefficients>> myCoefficientCache;

windvector::windvector() : itsCalculationTarget(kUnknownElement), itsVectorCalculation(false)
{
	itsCudaEnabledCalculation = true;

	itsLogger = logger("windvector");
}

void windvector::Process(const std::shared_ptr<const plugin_configuration> conf)
{
	Init(conf);

	/*
	 * Set target parameter to windvector
	 */

	vector<param> theParams;

	param requestedDirParam;
	param requestedSpeedParam;
	param requestedVectorParam;

	if (itsConfiguration->Exists("do_vector") && itsConfiguration->GetValue("do_vector") == "true")
	{
		itsVectorCalculation = true;
	}

	if (itsConfiguration->Exists("for_ice") && itsConfiguration->GetValue("for_ice") == "true")
	{
		requestedSpeedParam = param("IFF-MS", 389, 10, 2, 3);
		requestedDirParam = param("IDD-D", 390, 10, 2, 2);

		itsCalculationTarget = kIce;

		if (itsVectorCalculation)
		{
			itsLogger.Warning("Unable to calculate vector for ice");
		}
	}
	else if (itsConfiguration->Exists("for_sea") && itsConfiguration->GetValue("for_sea") == "true")
	{
		requestedSpeedParam = param("SFF-MS", 163, 10, 1, 1);
		requestedDirParam = param("SDD-D", 164, 10, 1, 0);

		itsCalculationTarget = kSea;

		if (itsVectorCalculation)
		{
			itsLogger.Warning("Unable to calculate vector for sea");
		}
	}
	else if (itsConfiguration->Exists("for_gust") && itsConfiguration->GetValue("for_gust") == "true")
	{
		requestedSpeedParam = param("FFG-MS", 417, 0, 2, 22);

		itsCalculationTarget = kGust;

		if (itsVectorCalculation)
		{
			itsLogger.Warning("Unable to calculate vector for wind gust");
		}
	}
	else
	{
		// By default assume we'll calculate for wind

		requestedDirParam = param("DD-D", 20, 0, 2, 0);
		requestedSpeedParam = param("FF-MS", 21, 0, 2, 1);

		if (itsVectorCalculation)
		{
			requestedVectorParam = param("DF-MS", 22);
		}

		itsCalculationTarget = kWind;
	}

	theParams.push_back(requestedSpeedParam);

	if (itsCalculationTarget != kGust)
	{
		theParams.push_back(requestedDirParam);
	}

	if (itsVectorCalculation)
	{
		theParams.push_back(requestedVectorParam);
	}

	SetParams(theParams);

	Start();
}

/*
 * Calculate()
 *
 * This function does the actual calculation.
 */

void windvector::Calculate(shared_ptr<info> myTargetInfo, unsigned short threadIndex)
{
	if (!myCoefficientCache.get())
	{
		myCoefficientCache.reset(new map<size_t, coefficients>());
	}

	// Required source parameters

	param UParam;
	param VParam;

	double directionOffset = 180;  // For wind direction add this

	switch (itsCalculationTarget)
	{
		case kSea:
			UParam = param("WVELU-MS");
			VParam = param("WVELV-MS");
			directionOffset = 0;
			break;

		case kIce:
			UParam = param("IVELU-MS");
			VParam = param("IVELV-MS");
			directionOffset = 0;
			break;

		case kGust:
			UParam = param("WGU-MS");
			VParam = param("WGV-MS");
			break;

		case kWind:
			UParam = param("U-MS");
			VParam = param("V-MS");
			break;

		default:
			throw runtime_error("Invalid calculation target element: " +
			                    to_string(static_cast<int>(itsCalculationTarget)));
			break;
	}

	auto myThreadedLogger = logger("windvectorThread #" + to_string(threadIndex));

	forecast_time forecastTime = myTargetInfo->Time();
	level forecastLevel = myTargetInfo->Level();
	forecast_type forecastType = myTargetInfo->ForecastType();

	myThreadedLogger.Info("Calculating time " + static_cast<string>(forecastTime.ValidDateTime()) + " level " +
	                      static_cast<string>(forecastLevel));

	info_t UInfo = Fetch(forecastTime, forecastLevel, UParam, forecastType, itsConfiguration->UseCudaForPacking());
	info_t VInfo = Fetch(forecastTime, forecastLevel, VParam, forecastType, itsConfiguration->UseCudaForPacking());

	if (!UInfo || !VInfo)
	{
		myThreadedLogger.Warning("Skipping step " + to_string(forecastTime.Step()) + ", level " +
		                         static_cast<string>(forecastLevel));
		return;
	}

	assert(UInfo->Grid()->AB() == VInfo->Grid()->AB());

	for (myTargetInfo->ResetParam(); myTargetInfo->NextParam();)
	{
		SetAB(myTargetInfo, UInfo);
	}

	assert(UInfo->Grid()->Type() == VInfo->Grid()->Type());

	string deviceType;

#ifdef HAVE_CUDA
	if (itsConfiguration->UseCuda() &&
	    (UInfo->Grid()->Type() == kLatitudeLongitude || UInfo->Grid()->Type() == kRotatedLatitudeLongitude ||
	     UInfo->Grid()->Type() == kLambertConformalConic))
	{
		deviceType = "GPU";

		auto opts = CudaPrepare(myTargetInfo, UInfo, VInfo);

		windvector_cuda::Process(*opts);
	}
	else
#endif
	{
		deviceType = "CPU";

#ifdef HAVE_CUDA
		Unpack({UInfo, VInfo});
#endif
		myTargetInfo->ParamIndex(0);

		auto& FFVec = VEC(myTargetInfo);
		vector<double> DDVec(FFVec.size(), kFloatMissing);

		for (auto&& tup : zip_range(FFVec, DDVec, VEC(UInfo), VEC(VInfo)))
		{
			double& speed = tup.get<0>();
			double& dir = tup.get<1>();
			double U = tup.get<2>();
			double V = tup.get<3>();

			if (U == kFloatMissing || V == kFloatMissing)
			{
				continue;
			}

			speed = sqrt(U * U + V * V);

			if (itsCalculationTarget == kGust)
			{
				continue;
			}

			if (speed > 0)
			{
				dir = himan::constants::kRad * atan2(U, V) + directionOffset;

				// reduce the angle
				dir = fmod(dir, 360);

				// force it to be the positive remainder, so that 0 <= dir < 360
				dir = round(fmod((dir + 360), 360));
			}
		}

		if (myTargetInfo->SizeParams() > 1)
		{
			myTargetInfo->ParamIndex(1);
			myTargetInfo->Data().Set(DDVec);
		}
	}

	myThreadedLogger.Info("[" + deviceType + "] Missing values: " + to_string(myTargetInfo->Data().MissingCount()) +
	                      "/" + to_string(myTargetInfo->Data().Size()));
}

shared_ptr<himan::info> windvector::Fetch(const forecast_time& theTime, const level& theLevel, const param& theParam,
                                          const forecast_type& theType, bool returnPacked) const
{
	auto f = GET_PLUGIN(fetcher);
	f->DoVectorComponentRotation(true);

	info_t ret;

	try
	{
		ret = f->Fetch(itsConfiguration, theTime, theLevel, theParam, theType, itsConfiguration->UseCudaForPacking());

#ifdef HAVE_CUDA
		if (!returnPacked && ret->Grid()->IsPackedData())
		{
			assert(ret->Grid()->PackedData().ClassName() == "simple_packed");

			util::Unpack({ret->Grid()});

			auto c = GET_PLUGIN(cache);

			c->Insert(*ret);
		}
#endif
	}
	catch (HPExceptionType& e)
	{
		if (e != kFileDataNotFound)
		{
			throw runtime_error(ClassName() + ": Unable to proceed");
		}
	}

	return ret;
}

#ifdef HAVE_CUDA

unique_ptr<windvector_cuda::options> windvector::CudaPrepare(shared_ptr<info> myTargetInfo, shared_ptr<info> UInfo,
                                                             shared_ptr<info> VInfo)
{
	unique_ptr<windvector_cuda::options> opts(new windvector_cuda::options);

	opts->N = UInfo->Grid()->Size();

	opts->target_type = itsCalculationTarget;

	opts->u = UInfo->ToSimple();
	opts->v = VInfo->ToSimple();

	myTargetInfo->ParamIndex(0);
	opts->speed = myTargetInfo->ToSimple();

	if (opts->target_type != kGust)
	{
		myTargetInfo->ParamIndex(1);
		opts->dir = myTargetInfo->ToSimple();
	}

	return opts;
}

#endif


#include <boost/geometry/formulas/spherical.hpp>


#ifndef _GEOMETRY_OBJECT3D_HPP_
#define _GEOMETRY_OBJECT3D_HPP_
#ifdef __cplusplus

#include <geometry/line3.hpp>
#include <map>
#include <sstream>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
//#include <boost/filesystem/fstream.hpp>     // <--- Opengl has trouble if included

#include <fstream>
#include <string>

namespace lsfm {

    struct Triangle {
        Triangle(std::size_t va = 0, std::size_t vb = 0, std::size_t vc = 0, std::size_t vn = 0, std::size_t texa = 0, std::size_t texb = 0, std::size_t texc = 0) :
            a(va), b(vb), c(vc), normal(vn), ta(texa), tb(texb), tc(texc) {}

        std::size_t a,b,c, normal;
        std::size_t ta,tb,tc;  // Texture Coordinates
    };

    template<class FT>
    struct Object3D {
        typedef std::vector<Vec3<FT>> VertexList;
        typedef std::vector<Vec3<FT>> NormalList;
        typedef std::vector<Triangle> TriangleList;
        typedef std::vector<Vec2<FT>> TexCoordList;
        typedef std::vector<LineSegment3<FT>> EdgeList;

        VertexList points;
        NormalList normals;
        TriangleList triangles;
        TexCoordList textureCoords;
        std::string name;

        Object3D() {}
        Object3D(const std::string &n, const VertexList& p, const TriangleList& tri, const TexCoordList& texC) : name(n), points(p), triangles(tri), textureCoords(texC) {}
        Object3D(const std::istream &in) {
            read(in);
        }

        //! read single object from stream
        void read(std::istream &in, int &pointCount, int &normalCount, int &textureCount) {
            name.clear();
            points.clear();
            normals.clear();
            triangles.clear();
            textureCoords.clear();
            int pointOffset = pointCount;
            int normalOffset = normalCount;
            int textureOffset = textureCount;

            std::string line;
            std::istream::pos_type g = in.tellg();
            while (std::getline(in,line)) {
                boost::trim(line);
                if (line.size() < 3) {
                    g = in.tellg();
                    continue;
                }
                std::istringstream sin(line);
                std::string first;
                sin >> first;
                if (first == "o") {
                    if (name.size()) {
                        in.seekg(g);
                        break;
                    } else
                        sin >> name;
                } else if (first == "v") {
                    Vec3<FT> p;
                    sin >> p.x() >> p.y() >> p.z();
                    points.push_back(p);
                    ++pointCount;
                } else if (first == "vn") {
                    Vec3<FT> vn;
                    sin >> vn.x() >> vn.y() >> vn.z();
                    normals.push_back(vn);
                    ++normalCount;
                } else if (first == "vt") {
                    Vec2<FT> vt;
                    sin >> vt.x() >> vt.y();
                    textureCoords.push_back(vt);
                    ++textureCount;
                }
                else if (first == "f") {
                    std::string a, b, c, vn;
                    std::size_t ta, tb, tc;
                    sin >> a >> b >> c;
                    std::size_t idx = a.find_last_of("/");
                    if (idx != std::string::npos)
                        vn = a.substr(idx + 1, std::string::npos);

                    idx = a.find_first_of("/");
                    if (idx != std::string::npos){
                        std::size_t idx2 = a.find_last_of("/");
                        if(idx != idx2 && idx + 1 != idx2)
                            ta = boost::lexical_cast<std::size_t>(a.substr(idx+1, idx2 - (idx+1))) - 1 - textureOffset;
                        else
                            ta = 0;
                        a = a.substr(0,idx);
                    }

                    idx = b.find_first_of("/");
                    if (idx != std::string::npos){
                        std::size_t idx2 = b.find_last_of("/");
                        if(idx != idx2 && idx + 1 != idx2)
                            tb = boost::lexical_cast<std::size_t>(b.substr(idx+1, idx2 - (idx+1))) - 1 - textureOffset;
                        else
                            tb = 0;
                        b = b.substr(0,idx);
                    }
                    idx = c.find_first_of("/");
                    if (idx != std::string::npos){
                        std::size_t idx2 = c.find_last_of("/");
                        if(idx != idx2 && idx + 1 != idx2)
                            tc = boost::lexical_cast<std::size_t>(c.substr(idx+1, idx2 - (idx+1))) - 1 - textureOffset;
                        else
                            tc = 0;
                        c = c.substr(0,idx);
                    }

                    if (vn.empty()) {
                        // compute normal from triangle
                        triangles.push_back(Triangle(boost::lexical_cast<std::size_t>(a) - 1 - pointOffset, boost::lexical_cast<std::size_t>(b) - 1 - pointOffset, boost::lexical_cast<std::size_t>(c) - 1 - pointOffset,normals.size()));
                        Triangle &tri = triangles.back();
                        Vec3<FT> v1 = points[tri.b] - points[tri.a], v2 = points[tri.c] - points[tri.a];
                        normals.push_back(v1.cross(v2));
                        normals.back().normalize();
                    }
                    else
                        triangles.push_back(Triangle(boost::lexical_cast<std::size_t>(a) - 1 - pointOffset, boost::lexical_cast<std::size_t>(b) - 1 - pointOffset, boost::lexical_cast<std::size_t>(c) - 1 - pointOffset, boost::lexical_cast<std::size_t>(vn) - 1 - normalOffset,
                                                     ta, tb, tc));
                }
                g = in.tellg();
            }
        }

        //! write single object to stream
        void write(std::ostream &out) const {
            out << "o " << name << std::endl;
            for_each(points.begin(),points.end(),[&](const Vec3<FT> &p){
               out << "v " << p.x() << " " << p.y() << " " << p.z() << std::endl;
            });

            for_each(triangles.begin(),triangles.end(),[&](const Triangle &t){
               out << "f " << t.a+1 << " " << t.b+1 << " " << t.c+1 << std::endl;
            });
        }

        //! all edges, no removal
        EdgeList edgesAll() const {
            EdgeList ret;
            for_each (triangles.begin(), triangles.end(), [&](const Triangle &t) {
                ret.push_back(LineSegment3<FT>(points[t.a],points[t.b]));
                ret.push_back(LineSegment3<FT>(points[t.b],points[t.c]));
                ret.push_back(LineSegment3<FT>(points[t.c],points[t.a]));
            });
            return ret;
        }

        //! convert triangles to edge list without duplicates
        EdgeList edges() const {
            typedef std::map<std::size_t,Vec3<FT>> InternMap;
            typedef typename InternMap::iterator InternMapIter;
            typedef std::map<std::size_t,InternMap> MyMap;
            typedef typename MyMap::iterator MyMapIter;
            MyMap m;
            EdgeList ret;

            auto addEdge = [&](size_t a, size_t b, const Vec3<FT> &n) {
                MyMapIter f = m.find(a);
                InternMapIter f2;
                if (f == m.end()) {
                    f = m.find(b);

                    // nothing found
                    if (f == m.end()) {
                        m[a] = InternMap();
                        m[a][b] = n;
                        return;
                    }

                    f2 = f->second.find(a);
                    // pair not found
                    if (f2 == f->second.end()) {
                        f->second.insert(typename InternMap::value_type(b,n));
                        return;
                    }
                } else {
                    f2 = f->second.find(b);
                    // pair not found
                    if (f2 == f->second.end()) {
                        f->second.insert(typename InternMap::value_type(a,n));
                        return;
                    }
                }

                // found duplicate -> check if we have to remove edge
                Vec3<FT> nd = n - f2->second;
                // same normal, remove from list
                if (std::fabs(nd.x() + nd.y() + nd.z()) < LIMITS<FT>::tau()) {
                    if (f->second.size() > 1)
                        f->second.erase(f2);
                    else
                        m.erase(f);
                }
                // else just keep existing edge in list
            };

            // detect multiple edges, sort them out and remove edges on same plane
            for_each (triangles.begin(), triangles.end(), [&](const Triangle &t) {
                Vec3<FT> n = (points[t.b] - points[t.a]).cross(points[t.c] - points[t.b]);
                addEdge(t.a,t.b,n);
                addEdge(t.b,t.c,n);
                addEdge(t.c,t.a,n);
            });

            // write edges to list
            for_each(m.begin(), m.end(), [&](const typename MyMap::value_type &p1){
                for_each(p1.second.begin(), p1.second.end(), [&](const typename InternMap::value_type &p2){
                    ret.push_back(LineSegment3<FT>(points[p1.first],points[p2.first]));
                });
            });
            return ret;
        }

        void findSimilarVertexes(){
            std::vector<std::pair<int, int>> similarities;

            for(int i = 0; i < points.size(); ++i )
                for(int j = 0; j < points.size(); ++j){
                    if(i == j)
                        continue;
                    FT a = (points[i][0] - points[j][0]), b = (points[i][1] - points[j][1]), c = (points[i][2] - points[j][2]);
                    FT dist = std::sqrt(a * a + b * b + c * c);
                    if(dist < FT(0.01))
                        similarities.push_back(std::pair<int,int>(i,j));
                }

            //for(int i = 0; i < similarities.size(); ++i)
            //    std::cout << "similar: " << similarities[i].first << "  " << similarities[i].second << std::endl;
        }

    };

    template<class FT>
    std::ostream& operator<<(std::ostream &os, const Object3D<FT> &obj) {
        obj.write(os);
        return os;
    }

    template<class FT>
    std::istream& operator<<(std::istream &is, Object3D<FT> &obj) {
        obj.read(is);
        return is;
    }

    template<class FT>
    using Object3DList = std::vector<Object3D<FT>>;

    template<class FT>
    void loadObjects(std::istream &is, Object3DList<FT> &data) {
        Object3D<FT> obj;
        int pointCount = 0;         // need to keep offset to identify points of each object, otherwise their index is incremented continously
        int normalCount = 0;        // same as above
        int texCount = 0;           // same as above
        while(is.good()) {
            obj.read(is, pointCount, normalCount, texCount);
            if (obj.points.size())
                data.push_back(obj);
            obj.findSimilarVertexes();
        }
    }
/*
    template<class FT>
    bool loadObjects(std::string &file, Object3DList<FT> &data) {
        return loadObjects(file,data);
    }
*/
    template<class FT>
    bool loadObjects(const std::string &file, Object3DList<FT> &data) {
        std::fstream fs;
        fs.open(file,std::ios_base::in);
        if (fs.is_open()) {
            loadObjects(fs,data);
            return true;
        }
        return false;
    }

    template<class FT>
    void saveObjects(std::ostream &os, const Object3DList<FT> &data) {
        os << "# obj file generator" << std::endl;
        for_each(data.begin(),data.end(),[&](const Object3D<FT> &obj) {
           obj.write(os);
        });
    }

    template<class FT>
    bool saveObjects(std::string &file, Object3DList<FT> &data) {
        return saveObjects(file,data);
    }

    template<class FT>
    bool saveObjects(const std::string &file, Object3DList<FT> &data) {
        std::fstream fs;
        fs.open(file,std::ios_base::out | std::ios_base::trunc);
        if (fs.is_open()) {
            saveObjects(fs,data);
            return true;
        }
        return false;
    }

}

#endif
#endif


#define BOOST_TEST_MODULE pcraster calc transient
#include <boost/test/unit_test.hpp>
#include <algorithm>
#include <functional>
#include <numeric>
#include "stddefx.h"
#include "api.h"
#include "csftypes.h"
#include "calc.h"

// Test if fixed head cells stay constant.
BOOST_AUTO_TEST_CASE(fixed_head)
{
  size_t nrRows = 3;
  size_t nrCols = 3;
  size_t nrCells = nrRows * nrCols;
  double cellSize = 15.0;
  CSF_PT projection = PT_YINCT2B;

  BootTestApi(cellSize, projection == PT_YINCT2B);

  // Set cell values.
  REAL4 resultElevationCells[9];
  REAL4 elevationCells[]           = { 1.0F, 2.0F, 3.0F,
                                       4.0F, 5.0F, 6.0F,
                                       7.0F, 8.0F, 9.0F };
  REAL4 rechargeCells[]            = { 1.0F };
  REAL4 transmissivityCells[]      = { 1.0F };
  INT4 flowConditionCells[]        = { 2 };      // Fixed head.
  REAL4 storageCoefficientCells[]  = { 1.0F };
  REAL4 intervalCells[]            = { 1.0F };
  REAL4 toleranceCells[]           = { 0.001F };

  // Create maps.
  MAP_REAL8* elevation = InitMapREAL8(nrRows, nrCols,
                   elevationCells, TRUE, CR_REAL4);
  MAP_REAL8* resultElevation = InitMapREAL8(nrRows, nrCols,
                   resultElevationCells, TRUE, CR_REAL4);
  MAP_REAL8* recharge = InitMapREAL8(nrRows, nrCols,
                   rechargeCells, FALSE, CR_REAL4);
  MAP_REAL8* transmissivity = InitMapREAL8(nrRows, nrCols,
                   transmissivityCells, FALSE, CR_REAL4);
  MAP_INT4* flowCondition = InitMapINT4(nrRows, nrCols,
                   flowConditionCells, FALSE, CR_INT4);
  MAP_REAL8* storageCoefficient = InitMapREAL8(nrRows, nrCols,
                   storageCoefficientCells, FALSE, CR_REAL4);
  MAP_REAL8* interval = InitMapREAL8(nrRows, nrCols,
                   intervalCells, FALSE, CR_REAL4);
  MAP_REAL8* tolerance = InitMapREAL8(nrRows, nrCols,
                   toleranceCells, FALSE, CR_REAL4);

  // Call function.
  void* out[]       = { resultElevation };
  const void* in[]  = { elevation, recharge, transmissivity, flowCondition,
                   storageCoefficient, interval, tolerance };
  BOOST_CHECK(!Transient(out, in, 7));

  // Delete maps.
  DeleteInternalMAP_REAL8(resultElevation);
  DeleteInternalMAP_REAL8(elevation);
  DeleteInternalMAP_REAL8(recharge);
  DeleteInternalMAP_REAL8(transmissivity);
  DeleteInternalMAP_INT4(flowCondition);
  DeleteInternalMAP_REAL8(storageCoefficient);
  DeleteInternalMAP_REAL8(interval);
  DeleteInternalMAP_REAL8(tolerance);

  // Check result.
  BOOST_CHECK(std::equal(elevationCells, elevationCells + nrCells,
                   resultElevationCells));
}

BOOST_AUTO_TEST_CASE(budget)
{
  size_t nrRows = 3;
  size_t nrCols = 3;
  size_t nrCells = nrRows * nrCols;
  double cellSize = 100.0;
  CSF_PT projection = PT_YINCT2B;

  BootTestApi(cellSize, projection == PT_YINCT2B);

  // Set cell values.
  REAL4 resultElevationCells[9];
  REAL4 elevationCells[]           = { 0.0F };    // Flat surface.
  REAL4 rechargeCells[]            = { 0.0F };    // No discharge / recharge.
  REAL4 transmissivityCells[]      = { 300.0F };
  INT4 flowConditionCells[]        = { 1 };      // Calculate head.
  REAL4 storageCoefficientCells[]  = { 0.002F };
  REAL4 intervalCells[]            = { 0.001F };
  REAL4 toleranceCells[]           = { 0.001F };

  // Create maps.
  MAP_REAL8* elevation = InitMapREAL8(nrRows, nrCols,
                   elevationCells, FALSE, CR_REAL4);
  MAP_REAL8* resultElevation = InitMapREAL8(nrRows, nrCols,
                   resultElevationCells, TRUE, CR_REAL4);
  MAP_REAL8* recharge = InitMapREAL8(nrRows, nrCols,
                   rechargeCells, FALSE, CR_REAL4);
  MAP_REAL8* transmissivity = InitMapREAL8(nrRows, nrCols,
                   transmissivityCells, FALSE, CR_REAL4);
  MAP_INT4* flowCondition = InitMapINT4(nrRows, nrCols,
                   flowConditionCells, FALSE, CR_INT4);
  MAP_REAL8* storageCoefficient = InitMapREAL8(nrRows, nrCols,
                   storageCoefficientCells, FALSE, CR_REAL4);
  MAP_REAL8* interval = InitMapREAL8(nrRows, nrCols,
                   intervalCells, FALSE, CR_REAL4);
  MAP_REAL8* tolerance = InitMapREAL8(nrRows, nrCols,
                   toleranceCells, FALSE, CR_REAL4);

  // Prepare arguments.
  void* out[1];
  const void* in[7];
  out[0] = resultElevation;
  in[0] = elevation;
  in[1] = recharge;
  in[2] = transmissivity;
  in[3] = flowCondition;
  in[4] = storageCoefficient;
  in[5] = interval;
  in[6] = tolerance;

  // Call function.
  BOOST_CHECK(!Transient(out, in, 7));

  // Nothing should have changed.
  for(size_t i = 0; i < nrCells; ++i) {
    BOOST_CHECK(resultElevationCells[i] == elevationCells[0]);
  }

  //----------------------------------------------------------------------------
  rechargeCells[0] = 0.2F;         // Add something, everywhere.
  DeleteInternalMAP_REAL8(recharge);
  recharge = InitMapREAL8(nrRows, nrCols, rechargeCells, FALSE, CR_REAL4);
  in[1] = recharge;

  // Call function.
  BOOST_CHECK(!Transient(out, in, 7));

  // No flow: equal amount of stuff on all cells.
  BOOST_CHECK(std::count_if(resultElevationCells, resultElevationCells + nrCells,
         std::bind2nd(std::equal_to<REAL4>(), resultElevationCells[0])));

  // Test amount added to all cells.
  // yepyep

/*
 *  std::cout << endl;
 *  std::cout << elevationCells[0] << endl;
 * std::copy(resultElevationCells, resultElevationCells + nrCells,
 *                 std::ostream_iterator<REAL4>(std::cout, "\t"));
 * std::cout << endl;
 *  cout << rechargeCells[0] * cellSize * cellSize * intervalCells[0] << endl;
 *
 * std::cout << std::accumulate(elevationCells, elevationCells + nrCells, 0.0) << endl;
 * std::cout << std::accumulate(resultElevationCells, resultElevationCells + nrCells, 0.0) << endl;
 * double volumeBefore = std::accumulate(elevationCells,
 *                  elevationCells + nrCells, 0.0);
 * double volumeAfter = std::accumulate(resultElevationCells,
 *                  resultElevationCells + nrCells, 0.0);
 * double volumeAdded = rechargeCells[0] * intervalCells[0];
 *
 * cout << volumeAfter - volumeBefore << '\t' << volumeAdded << endl;
 */

  // Delete maps.
  DeleteInternalMAP_REAL8(resultElevation);
  DeleteInternalMAP_REAL8(elevation);
  DeleteInternalMAP_REAL8(recharge);
  DeleteInternalMAP_REAL8(transmissivity);
  DeleteInternalMAP_INT4(flowCondition);
  DeleteInternalMAP_REAL8(storageCoefficient);
  DeleteInternalMAP_REAL8(interval);
  DeleteInternalMAP_REAL8(tolerance);

/*
  // Check result.
  BOOST_CHECK(std::accumulate(elevationCells, elevationCells + nrCells, 0.0) ==
         std::accumulate(resultElevationCells, resultElevationCells + nrCells, 0.0));
*/
}


#include <mc_rbdyn/RobotLoader.h>
#include <mc_rbdyn/Robots.h>
#include <mc_rbdyn/rpy_utils.h>
#include <boost/test/unit_test.hpp>
#include "utils.h"
#include <chrono>
#include <random>

mc_rbdyn::Robots & get_robots()
{
  static std::shared_ptr<mc_rbdyn::Robots> robots_ptr = nullptr;
  if(robots_ptr)
  {
    return *robots_ptr;
  }
  configureRobotLoader();
  auto rm = mc_rbdyn::RobotLoader::get_robot_module("JVRC1");
  auto env = mc_rbdyn::RobotLoader::get_robot_module("env", std::string(mc_rtc::MC_ENV_DESCRIPTION_PATH),
                                                     std::string("ground"));
  robots_ptr = mc_rbdyn::loadRobots({rm, env});
  return *robots_ptr;
}

BOOST_AUTO_TEST_CASE(TestRobotPosWVelW)
{
  auto robots = get_robots();

  for(int i = 0; i < 100; ++i)
  {
    Eigen::Vector3d oriRPYRef = Eigen::Vector3d::Random();
    Eigen::Vector3d posRPYRef = Eigen::Vector3d::Random();
    sva::PTransformd refPosW(mc_rbdyn::rpyToMat(oriRPYRef), posRPYRef);
    robots.robot().posW(refPosW);
    BOOST_CHECK(robots.robot().posW().matrix().isApprox(refPosW.matrix()));
  }

  for(int i = 0; i < 100; ++i)
  {
    auto refVal = sva::MotionVecd{Eigen::Vector3d::Random(), Eigen::Vector3d::Random()};
    robots.robot().velW(refVal);
    auto actual = robots.robot().velW();
    BOOST_CHECK_MESSAGE(actual.vector().isApprox(refVal.vector()), "Error in Robot::velW"
                                                                       << "\nExpected:"
                                                                       << "\nangular:" << refVal.angular().transpose()
                                                                       << "\nlinear :" << refVal.linear().transpose()
                                                                       << "\nGot:"
                                                                       << "\nangular:" << actual.angular().transpose()
                                                                       << "\nlinear :" << actual.linear().transpose());
  }
}

BOOST_AUTO_TEST_CASE(TestRobotZMPSimple)
{
  auto robots = get_robots();
  auto & robot = robots.robot();

  // Put all mass on the left foot, ZMP should be under the sensor
  const auto normalForce = robot.mass() * 10;
  const auto sensorNames = std::vector<std::string>{"LeftFootForceSensor", "RightFootForceSensor"};
  auto & lfs = robot.forceSensor("LeftFootForceSensor");
  auto & rfs = robot.forceSensor("RightFootForceSensor");

  {
    // ZMP under left sensor
    const auto forceLeftSurface = sva::ForceVecd{Eigen::Vector3d::Zero(), {0., 0., normalForce}};
    sva::PTransformd X_0_ls = lfs.X_0_f(robot);
    X_0_ls.translation().z() = 0;
    auto X_ls_f = lfs.X_0_f(robot) * X_0_ls.inv();
    lfs.wrench(X_ls_f.dualMul(forceLeftSurface));
    rfs.wrench(sva::ForceVecd::Zero());

    auto zmpIdeal = X_0_ls.translation();
    auto zmpComputed = robot.zmp(sensorNames, Eigen::Vector3d::Zero(), {0., 0., 1.});
    BOOST_CHECK_MESSAGE(zmpComputed.isApprox(zmpIdeal, 1e-10), "Error in Robot::zmp computation with leftFootRatio="
                                                                   << "\nExpected: " << zmpIdeal.transpose()
                                                                   << "\nGot: " << zmpComputed.transpose());
  }

  {
    // ZMP under right sensor
    const auto forceRightSurface = sva::ForceVecd{Eigen::Vector3d::Zero(), {0., 0., normalForce}};
    sva::PTransformd X_0_rs = rfs.X_0_f(robot);
    X_0_rs.translation().z() = 0;
    auto X_rs_f = lfs.X_0_f(robot) * X_0_rs.inv();
    lfs.wrench(X_rs_f.dualMul(forceRightSurface));
    rfs.wrench(sva::ForceVecd::Zero());

    auto zmpIdeal = X_0_rs.translation();
    auto zmpComputed = robot.zmp(sensorNames, Eigen::Vector3d::Zero(), {0., 0., 1.});
    BOOST_CHECK_MESSAGE(zmpComputed.isApprox(zmpIdeal, 1e-10), "Error in Robot::zmp computation with leftFootRatio="
                                                                   << "\nExpected: " << zmpIdeal.transpose()
                                                                   << "\nGot: " << zmpComputed.transpose());
  }

  { // checks that zmp throws if used with null force
    rfs.wrench(sva::ForceVecd::Zero());
    lfs.wrench(sva::ForceVecd::Zero());
    BOOST_CHECK_THROW(robot.zmp(sensorNames, Eigen::Vector3d::Zero(), {0., 0., 1.}), std::runtime_error);
  }
}