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

#include <HEAAN.h>
#include <cnpy.h>
#include <errno.h>
#include <sys/stat.h>

#include <boost/program_options.hpp>
#include <chrono>
#include <ckp19/dataset.hpp>
#include <ckp19/mean_shift.hpp>
#include <ckp19/plaintext_clustering.hpp>
#include <filesystem>
#include <fstream>
#include <nlohmann/json.hpp>
#include <numeric>
#include <string>

#include "utils.hpp"

namespace fs = std::filesystem;
namespace bpo = boost::program_options;
using json = nlohmann::json;
using TimeUnit = std::chrono::system_clock::time_point;

void benchmarkClustering(const bpo::variables_map &opts) {
  long seed = opts["seed"].as<long>();
  long repeat = opts["repeat"].as<long>();
  long threads = opts["threads"].as<long>();
  std::string dataset = opts["dataset"].as<std::string>();

  srand(seed);
  SetNumThreads(threads);

  bool save_data = false;
  fs::path output_dir;
  if (opts.count("output") > 0) {
    save_data = true;
    output_dir = fs::path(opts["output"].as<std::string>());
  }

  long logq = opts["logq"].as<long>();
  long logp = opts["logp"].as<long>();
  long logt = opts["logt"].as<long>();
  long logq_boot = opts["logq-boot"].as<long>();

  long num_dusts = opts["dusts"].as<long>();
  long iterations = opts["iterations"].as<long>();
  long mode_kdegree = opts["mode-kdegree"].as<long>();
  long label_kdegree = opts["label-kdegree"].as<long>();
  long mode_inv_steps = opts["mode-invsteps"].as<long>();
  long label_inv_steps = opts["label-invsteps"].as<long>();
  long minidx_t = opts["min-idx-t"].as<long>();

  json info;

  TimePoint start;
  Ring ring;
  SecretKey secretKey(ring);
  Scheme scheme(secretKey, ring);
  scheme.addLeftRotKeys(secretKey);
  scheme.addRightRotKeys(secretKey);
  TimePoint end;
  info["stats"]["init_scheme"] = end - start;

  Dataset ds = Dataset::loadNpy(dataset);
  ds.rescaleDatasetInPlace(0.5);

  long logl = std::ceil(std::log2(ds[0].size() * num_dusts));
  start = TimePoint();
  scheme.addBootKey(secretKey, logl, logq_boot + logt);
  end = TimePoint();
  info["stats"]["gen_bootstrap_key"] = end - start;

  info["details"] = {{"seed", seed},
                     {"repeat", repeat},
                     {"threads", threads},
                     {"dataset", dataset},
                     {"logN", logN},
                     {"logQ", logQ},
                     {"logq", logq},
                     {"logp", logp},
                     {"logt", logt},
                     {"logq-boot", logq_boot},
                     {"logl", logl},
                     {"dusts", num_dusts},
                     {"iterations", iterations},
                     {"mode-kdegree", mode_kdegree},
                     {"label-kdegree", label_kdegree},
                     {"mode-invsteps", mode_inv_steps},
                     {"label-invsteps", label_inv_steps},
                     {"min-idx-t", minidx_t}};

  std::cout << "--- Information ---\n";
  for (json::iterator it = info["details"].begin(); it != info["details"].end();
       ++it) {
    std::cout << it.key() << " : " << it.value() << "\n";
  }
  std::cout << std::endl;

  std::cout << "--- Setup ---\n";
  for (json::iterator it = info["stats"].begin(); it != info["stats"].end();
       ++it) {
    std::cout << it.key() << " : " << it.value() << "\n";
  }
  std::cout << std::endl;

  info["benchmarks"] = json::array();

  fs::create_directories(output_dir);
  auto info_file = output_dir / "info.json";

  for (long i = 0; i < repeat; ++i) {
    std::cout << "--- Repetition " << i + 1 << " ---\n";

    json iter;

    EncryptedDataset eds;
    start = TimePoint();
    ds.encryptSIMD(eds, scheme, num_dusts, Nh, logp, logq);
    end = TimePoint();
    iter["encrypt_dataset_time"] = end - start;

    if (i == 0) {
      info["details"]["plaintext_points_per_ciphertext"] = eds.npoints;
      info["details"]["num_ciphertexts"] = eds.points.size();
      info["details"]["slots_used"] = eds.log_slots;
    }

    MeanShift meanshift(scheme, eds.log_slots, logq, logq_boot, logp, logt,
                        seed + i);

    std::vector<Ciphertext> clabels;

    TimePoint start;
    meanshift.clusterSIMD(clabels, eds.points, eds.dim, eds.npoints, num_dusts,
                          iterations, mode_kdegree, label_kdegree,
                          mode_inv_steps, label_inv_steps, minidx_t);
    TimePoint end;
    iter["cluster_time"] = end - start;

    std::vector<double> olabels(ds.size() * num_dusts);
    double decrypt_time = 0;
    for (size_t p = 0; p < eds.points.size(); ++p) {
      TimePoint start;
      std::unique_ptr<complex<double>[]> output(
          scheme.decrypt(secretKey, clabels[p]));
      TimePoint end;
      decrypt_time += end - start;

      for (int i = 0; i < num_dusts; ++i) {
        for (int j = 0; j < eds.npoints; ++j) {
          // Store the final label as the value in the first dimension
          olabels[p * eds.npoints * num_dusts + j * num_dusts + i] =
              output[i * eds.npoints * eds.dim + j * eds.dim].real();
        }
      }
    }
    iter["decrypt_output_time"] = decrypt_time;

    if (save_data) {
      info["benchmarks"].push_back(iter);
      saveJson(info, info_file);

      auto data_file =
          (output_dir / ("rep_" + std::to_string(i) + ".npy")).string();
      cnpy::npy_save(data_file, olabels.data(),
                     {ds.size(), static_cast<unsigned long>(num_dusts)}, "w");
    }
  }

  if (save_data) {
    // Highest virutal memory usage and physical memory usage
    info["stats"]["vmpeak"] = getProcStatus("VmPeak:");
    info["stats"]["vmhwm"] = getProcStatus("VmHWM:");
    saveJson(info, info_file);
  }
}

// clang-format off
bpo::options_description generic_program_options() {
  bpo::options_description desc("Followig options are supported by config file too");
  desc.add_options()
    ("output,o", bpo::value<std::string>(), "Directory to save benchmarks and other data.")
    ("seed", bpo::value<long>()->default_value(2602), "Seed used for RNG.")
    ("repeat,r", bpo::value<long>()->default_value(1), "Number of times to run benchmarks.")
    ("threads", bpo::value<long>()->default_value(1), "Number of threads to use.")
    ("dataset", bpo::value<std::string>()->required(), "Path to dataset in npy format.");

  desc.add_options()
    ("logq", bpo::value<long>()->default_value(logQ), "Log of ciphertext modulus.")
    ("logp", bpo::value<long>()->default_value(30), "Log of noise parameter.")
    ("logt", bpo::value<long>()->default_value(4), "Log of bootstrap noise parameter.")
    ("logq-boot", bpo::value<long>()->default_value(40), "Log of bootstrap ciphertext modulus.");

  desc.add_options()
    ("dusts", bpo::value<long>()->required(), "Number of dusts.")
    ("iterations", bpo::value<long>()->required(), "Number of iterations for mode seeking.")
    ("mode-kdegree", bpo::value<long>()->required(), "Log of kernel degree in mode seeking (\\Gamma_1).")
    ("label-kdegree", bpo::value<long>()->required(), "Log of kernel degree in point labeling (\\Gamma_2).")
    ("mode-invsteps", bpo::value<long>()->required(), "Number of iterations for inverse in mode seeking (\\zeta_1).")
    ("label-invsteps", bpo::value<long>()->required(), "Number of iterations for inverse in point labeling (\\zeta_2).")
    ("min-idx-t", bpo::value<long>()->required(), "Log of degree for minimum index algorithm (t).");

  return desc;
}
// clang-format on

int main(int argc, char *argv[]) {
  auto generic(generic_program_options());

  bpo::options_description cmdline("Run CKP19 mean shift clustering.");
  cmdline.add(generic);
  cmdline.add_options()(
      "config,c", bpo::value<std::string>(),
      "Configuration file for easy specification of cmd line arguments.")(
      "help,h", "Produce help message.");

  bpo::variables_map opts;
  bpo::store(bpo::command_line_parser(argc, argv).options(cmdline).run(), opts);

  if (opts.count("help") != 0) {
    std::cout << cmdline << std::endl;
    return 0;
  }

  if (opts.count("config") != 0) {
    std::string cpath(opts["config"].as<std::string>());
    std::ifstream fin(cpath.c_str());

    if (fin.fail()) {
      std::cerr << "Could not open configuration file at " << cpath << "\n";
      return 1;
    }

    bpo::store(bpo::parse_config_file(fin, generic), opts);
  }

  try {
    bpo::notify(opts);

    // Check if output file already exists
    if (fs::exists(opts["output"].as<std::string>())) {
      throw std::runtime_error("Output directory aready exists.");
    }
  } catch (const std::exception &ex) {
    std::cerr << ex.what() << std::endl;
    return 1;
  }

  try {
    benchmarkClustering(opts);
  } catch (const std::exception &ex) {
    std::cerr << ex.what() << "\nFatal error" << std::endl;
    return 1;
  }

  return 0;
}


#ifndef FIELD2_HPP_
#define FIELD2_HPP_

#pragma warning (disable : 4146)
#include <NTL/ZZ_pEX.h>

#include <variant>
#include <iostream>
#include <string>

namespace gadgetlib
{
	// helper function
	constexpr unsigned c_strlen(char const* str, unsigned count = 0)
	{
		return ('\0' == str[0]) ? count : c_strlen(str + 1, count + 1);
	}

	// destination "template string" type
	template < char... tt_c >
	struct exploded_string
	{
	};

	// struct to explode a `char const*` to an `exploded_string` type
	template < typename T_StrProvider, unsigned t_len, char... tt_c >
	struct explode_impl
	{
		using result =
			typename explode_impl < T_StrProvider, t_len - 1,
			T_StrProvider::str()[t_len - 1],
			tt_c... > ::result;
	};

	template < typename T_StrProvider, char... tt_c >
	struct explode_impl < T_StrProvider, 0, tt_c... >
	{
		using result = exploded_string<tt_c...>;
	};

	// syntactical sugar
	template < typename T_StrProvider >
	using explode =
		typename explode_impl < T_StrProvider,
		c_strlen(T_StrProvider::str()) > ::result;

	template<typename STR_PROVIDER>
	class Field
	{
	public:
		NTL::ZZ_p num_;
		static bool initialized_;

	private:
		void initialize()
		{
			if (!initialized_)
			{
				initialized_ = true;
				NTL::ZZ chp = NTL::conv<NTL::ZZ>(STR_PROVIDER::str());
				NTL::ZZ_p::init(chp);
			}

		}

		NTL::ZZ hexToZZ(const std::string& hexVal)
		{
			auto convert_ch = [](char c) -> int
			{
				if (c >= '0' && c <= '9')
					return (c - '0');
				if (c >= 'a' && c <= 'f')
					return (c - 'a' + 10);

			};

			NTL::ZZ val;
			val = NTL::to_ZZ(0);	//initialise the value to zero

			for (unsigned i = 0; i < hexVal.length(); i++)
			{
				val *= 16;
				val += convert_ch(hexVal[i]);

			}
			return val;
		}

	public:
		//static constexpr uint64_t safe_bitsize = SignificantBits<characteristics>::n;
		//static constexpr uint64_t chr = characteristics;

		Field(size_t num)
		{
			initialize();
			num_ = num;
		}

		Field()
		{
			initialize();
		}

		Field(const NTL::ZZ_p& num)
		{
			initialize();
			num_ = num;
		}

		Field(const std::variant<uint32_t, std::string>& v)
		{
			initialize();
			//uint32_t n;
			switch (v.index())
			{
			case 0:
			{
				NTL::ZZ num = NTL::conv<NTL::ZZ>(std::get<0>(v));
				num_ = NTL::conv<NTL::ZZ_p>(num);;
				break;
			}
			case 1:
				NTL::ZZ int_num = hexToZZ(std::get<1>(v));
				num_ = NTL::conv<NTL::ZZ_p>(int_num);
				break;
			};
		}

		Field& operator+=(const Field& rhs)
		{
			this->num_ += rhs.num_;
			return *this;
		}

		Field& operator-=(const Field& rhs)
		{
			this->num_ -= rhs.num_;
			return *this;
		}

		Field& operator*=(const Field& rhs)
		{
			this->num_ *= rhs.num_;
			return *this;
		}

		Field& operator-()
		{
			this->num_ = -this->num_;
			return *this;
		}

		Field& operator/=(const Field& rhs)
		{
			this->num_ = NTL::conv<NTL::ZZ_p>(NTL::rep(num_) / NTL::rep(rhs.num_));
			return *this;
		}

		static Field one()
		{
			return 1;
		}

		operator bool() const
		{
			return num_ != 0;
		}
	};

	template<typename STR_PROVIDER>
	Field<STR_PROVIDER> operator+(const Field<STR_PROVIDER>& left,
		const Field<STR_PROVIDER>& right)
	{
		return Field<STR_PROVIDER>(left.num_ + right.num_);
	}

	template<typename STR_PROVIDER>
	Field<STR_PROVIDER> operator-(const Field<STR_PROVIDER>& left,
		const Field<STR_PROVIDER>& right)
	{
		return Field<STR_PROVIDER>(left.num_ - right.num_);
	}

	template<typename STR_PROVIDER>
	Field<STR_PROVIDER> operator*(const Field<STR_PROVIDER>& left,
		const Field<STR_PROVIDER>& right)
	{
		return Field<STR_PROVIDER>(left.num_ * right.num_);
	}

	template<typename STR_PROVIDER>
	bool operator==(const Field<STR_PROVIDER>& left,
		const Field<STR_PROVIDER>& right)
	{
		return (left.num_ == right.num_);
	}

	template<typename STR_PROVIDER>
	bool operator!=(const Field<STR_PROVIDER>& left,
		const Field<STR_PROVIDER>& right)
	{
		return (left.num_ != right.num_);
	}

	template<typename STR_PROVIDER>
	Field<STR_PROVIDER> operator%(const Field<STR_PROVIDER>& left,
		int right)
	{
		assert(right == 2);
		return NTL::rem(rep(left.num_), right);
	}

	template<typename STR_PROVIDER>
	std::ostream& operator<< (std::ostream& stream, const Field<STR_PROVIDER>& elem)
	{
		stream << elem.num_;
		return stream;
	}

	template<typename STR_PROVIDER>
	bool Field<STR_PROVIDER>::initialized_ = false;
}

#endif


/*
 *  Copyright (c) 2010, INRIA, Project ALICE
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *  this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *  this list of conditions and the following disclaimer in the documentation
 *  and/or other materials provided with the distribution.
 *  * Neither the name of the ALICE Project-Team nor the names of its
 *  contributors may be used to endorse or promote products derived from this
 *  software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  If you modify this software, you should include a notice giving the
 *  name of the person performing the modification, the date of modification,
 *  and the reason for such modification.
 *
 *  Contact: Bruno Levy
 *
 *     Bruno.Levy@inria.fr
 *     http://alice.loria.fr
 *
 *     ALICE Project
 *     INRIA Lorraine,
 *     Campus Scientifique, BP 239
 *     54506 VANDOEUVRE LES NANCY CEDEX
 *     FRANCE
 *
 */

#include <LpCVT/algebra/F_Lp.h>
#include <LpCVT/algebra/voro_func.h>

#ifdef CVT_MULTITHREAD
#include <LpCVT/common/processor.h>
#include <boost/thread.hpp>
#include <boost/bind.hpp>
#endif

namespace Geex {

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

    // Internal version: uses pointers instead of std::vectors for 'sym' and 'C'
    // (used by both monothread and multithread implementations)

    double compute_F_Lp_internal(
        bool volumic,                  // IN: false for surface meshing, true for volume meshing
        unsigned int p,                // IN: Lp norm to be used
        Mesh* mesh,                    // IN: the PLC
        unsigned int nb,               // IN: number of integration simplices
        const int* sym,                      // IN: 10 integers per integration simplex:
                                       //   - index of x0
                                       //   - symbolic representation of C1,C2,C3 (3 integers each)
        const vec3* C,                       // IN: C vertices of integration simplices (3 per integration simplex)
        const std::vector<vec3>& X,    // IN: vertices
        const std::vector<plane3>& Q,  // IN: boundary facets
        const std::vector<mat3>& M,    // IN: anisotropy matrix (one per integration simplex)
        std::vector<double>& g         // OUT: gradient
    ) {
        assert(p >= 2 && p <= 16) ;
        assert((p/2)*2 == p) ;
        double f = 0.0 ;
        if(volumic) {
            switch(p) {
            case 2: {
                VoroFunc< IntegrationSimplex<2,TetVolume> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 4: {
                VoroFunc< IntegrationSimplex<4,TetVolume> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 6: {
                VoroFunc< IntegrationSimplex<6,TetVolume> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 8: {
                VoroFunc< IntegrationSimplex<8,TetVolume> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 10: {
                VoroFunc< IntegrationSimplex<10,TetVolume> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 12: {
                VoroFunc< IntegrationSimplex<12,TetVolume> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 14: {
                VoroFunc< IntegrationSimplex<14,TetVolume> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 16: {
                VoroFunc< IntegrationSimplex<16,TetVolume> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            }
        } else {
            switch(p) {
            case 2: {
                VoroFunc< IntegrationSimplex<2,TriArea> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 4: {
                VoroFunc< IntegrationSimplex<4,TriArea> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 6: {
                VoroFunc< IntegrationSimplex<6,TriArea> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 8: {
                VoroFunc< IntegrationSimplex<8,TriArea> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 10: {
                VoroFunc< IntegrationSimplex<10,TriArea> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 12: {
                VoroFunc< IntegrationSimplex<12,TriArea> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 14: {
                VoroFunc< IntegrationSimplex<14,TriArea> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            case 16: {
                VoroFunc< IntegrationSimplex<16,TriArea> > F ;
                f = F.eval(mesh,nb,sym,C,X,Q,M,g) ;
            } break ;
            }
        }
        return f ;
    }

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

#ifdef CVT_MULTITHREAD

    /**
     * Stores all the parameters required to evaluate F_Lp on a set of integration
     * simplices. Manages also a local storage for the gradient.
     */
    class F_Lp_thread {
    public:
        void set_parameters(
            bool volumic_in,                  // IN: false for surface meshing, true for volume meshing
            unsigned int p_in,                // IN: Lp norm to be used
            Mesh* mesh_in,                    // IN: the PLC
            unsigned int nb_in,               // IN: number of integration simplices
            const int* sym_in,                // IN: 10 integers per integration simplex:
                                              //   - index of x0
                                              //   - symbolic representation of C1,C2,C3 (3 integers each)
            const vec3* C_in,                 // IN: C vertices of integration simplices (3 per integration simplex)
            const std::vector<vec3>& X_in,    // IN: vertices
            const std::vector<plane3>& Q_in,  // IN: boundary facets
            const std::vector<mat3>& M_in     // IN: anisotropy matrix (one per integration simplex)
        ) {
            volumic = volumic_in ;
            p = p_in ;
            mesh = mesh_in ;
            nb = nb_in ;
            sym = sym_in ;
            C = C_in ;
            X = &X_in ;
            Q = &Q_in ;
            M = &M_in ;
            g.resize(X->size()*3) ;
        }

        void run() {
            std::fill(g.begin(), g.end(), 0.0) ;
            f = compute_F_Lp_internal(volumic, p, mesh, nb, sym, C, *X, *Q, *M, g) ;
        }

        bool volumic ;
        unsigned int p ;
        Mesh* mesh ;
        unsigned int nb ;
        const int* sym ;
        const vec3* C ;
        const std::vector<vec3>* X ;
        const std::vector<plane3>* Q ;
        const std::vector<mat3>* M ;
        double f ;
        std::vector<double> g ;
    } ;

    /**
     * User API function,
     * multithread implementation.
     * 1) Partitions the set of integration simplices,
     * 2) Calls compute_F_Lp_internal() in parallel,
     * 3) Gathers the result.
     */
    double compute_F_Lp(
        bool volumic,                  // IN: false for surface meshing, true for volume meshing
        unsigned int p,                // IN: Lp norm to be used
        Mesh* mesh,                    // IN: the PLC
        const std::vector<int>& sym,   // IN: 10 integers per integration simplex:
                                       //   - index of x0
                                       //   - symbolic representation of C1,C2,C3 (3 integers each)
        const std::vector<vec3>& C,    // IN: C vertices of integration simplices (3 per integration simplex)
        const std::vector<vec3>& X,    // IN: vertices
        const std::vector<plane3>& Q,  // IN: boundary facets
        const std::vector<mat3>& M,    // IN: anisotropy matrix (one per integration simplex)
        std::vector<double>& g         // OUT: gradient
    ) {
        // Get number of cores from Processor class.
        unsigned int nb_threads = Processor::number_of_cores() ;
        std::cerr << "Evaluating F-Lp, using " << nb_threads << " threads " << std::endl ;
        std::vector<F_Lp_thread> threads(nb_threads) ;

        unsigned int nb_integration_simplices = (unsigned int)sym.size() / 10 ;
        unsigned int remaining = nb_integration_simplices ;
        unsigned int batch_size = nb_integration_simplices / nb_threads ;

        // Partition work (i.e. I and C arrays) into nb_threads blocs
        const int*  cur_I = &sym[0] ;
        const vec3* cur_C = &C[0] ;
        for(unsigned int i=0; i<nb_threads-1; i++) {
            threads[i].set_parameters(volumic, p, mesh, batch_size, cur_I, cur_C, X, Q, M) ;
            cur_I += batch_size * 10 ;
            cur_C += batch_size * 3 ;
            remaining -= batch_size ;
        }
        threads[threads.size()-1].set_parameters(volumic, p, mesh, remaining, cur_I, cur_C, X, Q, M) ;

        // Run the threads in parallel, using boost threads
        //   Note: Intel TBB has lower thread creation overhead (has a thread pool),
        // but we use here boost since LpCVT depends on CGAL that also depends on boost
        // (and for large meshes, thread creation time is negligible).
        boost::thread_group threads_impl ;
        for(unsigned int i=0; i<threads.size(); i++) {
            threads_impl.create_thread(boost::bind(&F_Lp_thread::run, &threads[i])) ;
        }
        threads_impl.join_all() ; // Waits for termination of all threads.

        // Gather the result
        double result = 0 ;
        std::fill(g.begin(), g.end(), 0.0) ;
        for(unsigned int i=0; i<threads.size(); i++) {
            result += threads[i].f ;
            for(unsigned int j=0; j<g.size(); j++) {
                g[j] += threads[i].g[j] ;
            }
        }
        return result ;
    }

#else

    /**
     * User API function,
     * monothread implementation.
     * This is just a wrapper around compute_F_Lp_internal()
     */
    double compute_F_Lp(
        bool volumic,                  // IN: false for surface meshing, true for volume meshing
        unsigned int p,                // IN: Lp norm to be used
        Mesh* mesh,                    // IN: the PLC
        const std::vector<int>& sym,   // IN: 10 integers per integration simplex:
                                       //   - index of x0
                                       //   - symbolic representation of C1,C2,C3 (3 integers each)
        const std::vector<vec3>& C,    // IN: C vertices of integration simplices (3 per integration simplex)
        const std::vector<vec3>& X,    // IN: vertices
        const std::vector<plane3>& Q,  // IN: boundary facets
        const std::vector<mat3>& M,    // IN: anisotropy matrix (one per integration simplex)
        std::vector<double>& g         // OUT: gradient
    ) {
        unsigned int nb = sym.size() / 10 ;
        assert(sym.size() == nb*10) ;
        assert(C.size() == nb*3) ;
        return compute_F_Lp_internal(
            volumic, p, mesh, nb,
            &sym[0], &C[0], X, Q, M, g
        ) ;
    }

#endif

}


//------------------------------------------------------------------------------
// Construct a flaw distribution for a set of nodes according to a Weibull
// (power-law) distribution.
//------------------------------------------------------------------------------
#include "weibullFlawDistributionOwen.hh"
#include "Utilities/globalNodeIDs.hh"
#include "Utilities/mortonOrderIndices.hh"
#include "NodeList/FluidNodeList.hh"
#include "Field/Field.hh"
#include "Field/FieldList.hh"
#include "DataBase/DataBase.hh"
#include "Distributed/Communicator.hh"
#include "Utilities/allReduce.hh"

#include <boost/functional/hash.hpp>  // hash_combine

#include <set>
#include <algorithm>
#include <limits>
#include <unordered_map>
#include <random>

using std::unordered_map;
using std::vector;
using std::string;
using std::pair;
using std::make_pair;
using std::cout;
using std::cerr;
using std::endl;
using std::min;
using std::max;
using std::abs;

namespace Spheral {

//------------------------------------------------------------------------------
// This version uses my own algorithm, stochastically seeding flaws in the range
// [0, epsmax] where epsmax is chosen per node based on the nodal volume.
//------------------------------------------------------------------------------
template<typename Dimension>
Field<Dimension, vector<double> >
weibullFlawDistributionOwen(const unsigned seed,
                            const double kWeibull,
                            const double mWeibull,
                            const FluidNodeList<Dimension>& nodeList,
                            const int minFlawsPerNode,
                            const double volumeMultiplier,
                            const Field<Dimension, int>& mask) {

  // Pre-conditions.
  REQUIRE(kWeibull >= 0.0);
  REQUIRE(mWeibull > 0.0);
  REQUIRE(minFlawsPerNode > 0);
  REQUIRE(mask.nodeListPtr() == &nodeList);

  typedef typename Dimension::Scalar Scalar;
  typedef KeyTraits::Key Key;

  // Prepare the result.
  Field<Dimension, vector<double> > flaws("Weibull flaw distribution",
                                          nodeList);

  // Find the unique ordering to the nodes.
  DataBase<Dimension> db;
  db.appendNodeList(const_cast<FluidNodeList<Dimension>&>(nodeList));
  FieldList<Dimension, Key> keyList = mortonOrderIndices(db);
  const auto nglobal = db.globalNumInternalNodes();

  // Is there anything to do?
  if (nglobal > 0) {
    const auto nlocal = nodeList.numInternalNodes();

    // Identify the rank and number of domains.
    const auto procID = Process::getRank();

    // State for this NodeList.
    const Field<Dimension, Scalar>& mass = nodeList.mass();
    const Field<Dimension, Scalar>& rho = nodeList.massDensity();

    // Construct a random number generator for each point.
    // Note we hash with the ordering key to generate a unique but reproducible sequence for each point.
    vector<std::mt19937> gens(nlocal);
#pragma omp parallel for
    for (auto i = 0u; i < nlocal; ++i) {
      Key seedi = seed;
      boost::hash_combine(seedi, keyList(0,i));
      gens[i].seed(seedi);
    }
    vector<std::uniform_real_distribution<double>> uniform01(nlocal, std::uniform_real_distribution<double>(0.0, 1.0));

    // Find the minimum and maximum node volumes.
    double Vmin = std::numeric_limits<double>::max(),
           Vmax = std::numeric_limits<double>::min();
    for (auto i = 0u; i != nodeList.numInternalNodes(); ++i) {
      if (mask(i) == 1) {
        const double Vi = mass(i)/rho(i);
        Vmin = min(Vmin, Vi);
        Vmax = max(Vmax, Vi);
      }
    }
    Vmin = allReduce(Vmin*volumeMultiplier, MPI_MIN, Communicator::communicator());
    Vmax = allReduce(Vmax*volumeMultiplier, MPI_MAX, Communicator::communicator());
    CHECK(Vmin > 0.0);
    CHECK(Vmax >= Vmin);

    // Compute the maximum strain we expect for the minimum volume.
    const double epsMax2m = minFlawsPerNode/(kWeibull*Vmin);  // epsmax ** m

    // Based on this compute the maximum number of flaws any node will have.  We'll use this to
    // spin the random number generator without extra communiction.
    const auto maxFlawsPerNode = std::max(1u, unsigned(kWeibull*Vmax*epsMax2m + 0.5));

    // Generate the flaws on each node indepedently.
    const double mInv = 1.0/mWeibull;
    unsigned minNumFlaws = std::numeric_limits<int>::max();
    unsigned maxNumFlaws = 0;
    unsigned totalNumFlaws = 0;
    double epsMin = std::numeric_limits<double>::max();
    double epsMax = std::numeric_limits<double>::min();
    double sumFlaws = 0.0;
#pragma omp parallel for
    for (auto i = 0u; i < nlocal; ++i) {
      if (mask(i) == 1) {
        CHECK(rho(i) > 0.0);
        const auto Vi = mass(i)/rho(i) * volumeMultiplier;
        CHECK(Vi > 0.0);
        const auto numFlawsi = std::max(1u, std::min(maxFlawsPerNode, unsigned(kWeibull*Vi*epsMax2m + 0.5)));
        const auto Ai = numFlawsi/(kWeibull*Vi);
        CHECK(Ai > 0.0);
        for (auto j = 0u; j < numFlawsi; ++j) {
          flaws(i).push_back(pow(Ai * uniform01[i](gens[i]), mInv));
        }
        // Sort the flaws on each node by energy.
        sort(flaws(i).begin(), flaws(i).end());
#pragma omp critical
        {
          minNumFlaws = min(minNumFlaws, unsigned(flaws(i).size()));
          maxNumFlaws = max(maxNumFlaws, unsigned(flaws(i).size()));
          totalNumFlaws += flaws(i).size();
          epsMin = min(epsMin, flaws(i).front());
          epsMax = max(epsMax, flaws(i).back());
          for (auto j = 0u; j < flaws(i).size(); ++j) sumFlaws += flaws(i)[j];
        }
      }
    }

    // Some diagnostic output.
    const auto nused = std::max(1, mask.sumElements());
    if (nglobal > 0) {
      minNumFlaws = allReduce(minNumFlaws, MPI_MIN, Communicator::communicator());
      maxNumFlaws = allReduce(maxNumFlaws, MPI_MAX, Communicator::communicator());
      totalNumFlaws = allReduce(totalNumFlaws, MPI_SUM, Communicator::communicator());
      epsMin = allReduce(epsMin, MPI_MIN, Communicator::communicator());
      epsMax = allReduce(epsMax, MPI_MAX, Communicator::communicator());
      sumFlaws = allReduce(sumFlaws, MPI_SUM, Communicator::communicator());
    }
    if (procID == 0) {
      cerr << "weibullFlawDistributionOwen: Min num flaws per node: " << minNumFlaws << endl
           << "                             Max num flaws per node: " << maxNumFlaws << endl
           << "                             Total num flaws       : " << totalNumFlaws << endl
           << "                             Avg flaws per node    : " << totalNumFlaws / nused << endl
           << "                             Min flaw strain       : " << epsMin << endl
           << "                             Max flaw strain       : " << epsMax << endl
           << "                             Avg node failure      : " << sumFlaws / nused << endl;
    }

    // That's it.
    BEGIN_CONTRACT_SCOPE
    {
      for (int i = 0; i != (int)nodeList.numInternalNodes(); ++i) {
        if (mask(i) == 1) {
          for (vector<double>::const_iterator itr = flaws(i).begin() + 1;
               itr != flaws(i).end();
               ++itr) ENSURE(*itr >= *(itr - 1));
        }
      }
    }
    END_CONTRACT_SCOPE
  }

  return flaws;
}

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_HYPOT_HPP_INCLUDED

#include <boost/simd/toolbox/arithmetic/functions/hypot.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/abs.hpp>
#include <boost/simd/include/functions/simd/min.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/include/functions/simd/is_inf.hpp>
#include <boost/simd/include/functions/simd/is_nan.hpp>
#include <boost/simd/include/functions/simd/logical_and.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>
#include <boost/simd/include/functions/simd/exponent.hpp>
#include <boost/simd/include/functions/simd/ldexp.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/sqrt.hpp>
#include <boost/simd/include/functions/simd/any.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/include/constants/int_splat.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::hypot_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))
                            )
  {

    typedef typename dispatch::meta::as_floating<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return boost::simd::hypot(tofloat(a0), tofloat(a1));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::hypot_, tag::cpu_,
                              (A0)(X),
                              ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                            )
  {
    template < class T, class I = typename dispatch::meta::as_integer<T, signed>::type>
    struct hypot_ctnts {};

    template <class I, class CAT>
    struct hypot_ctnts<simd::native<float, CAT>, I>
    {
      typedef I  int_type;
      static inline int_type C1(){ return boost::simd::integral_constant<int_type, 50>();};
      static inline int_type C2(){ return boost::simd::integral_constant<int_type, 60>();};
      static inline int_type MC1(){ return boost::simd::integral_constant<int_type, -50>();};
      static inline int_type MC2(){ return boost::simd::integral_constant<int_type, -60>();};
      static inline int_type C3(){ return boost::simd::integral_constant<int_type, 0x00800000>();};
      static inline int_type M1(){ return boost::simd::integral_constant<int_type, 0xfffff000>();};
    };

    template <class I, class CAT>
    struct hypot_ctnts<simd::native<double, CAT>, I>
    {
      typedef I  int_type;
      static inline int_type C1(){ return boost::simd::integral_constant<int_type, 500>();};
      static inline int_type C2(){ return boost::simd::integral_constant<int_type, 600>();};
      static inline int_type MC1(){ return boost::simd::integral_constant<int_type, -500>();};
      static inline int_type MC2(){ return boost::simd::integral_constant<int_type, -600>();};
      static inline int_type C3(){ return boost::simd::integral_constant<int_type, 0x0010000000000000ll>();}
      static inline int_type M1(){ return boost::simd::integral_constant<int_type, 0xffffffff00000000ll>();};
    };

    typedef typename dispatch::meta::as_floating<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::as_integer<result_type>::type itype;
      result_type r =  boost::simd::abs(a0);
      result_type i =  boost::simd::abs(a1);
      itype e =  exponent(boost::simd::max(i, r));
      return  if_else( logical_or(logical_and(is_nan(a0), is_inf(a1)),
                                  logical_and(is_nan(a1), is_inf(a0))),
                       Inf<result_type>(),
                       ldexp(sqrt(sqr(ldexp(r, -e))+sqr(ldexp(i, -e))), e)
                       );
//       typedef typename meta::as_logical<A0>::type bA0;
//       typedef typename dispatch::meta::as_integer<A0, signed>::type int_type;
//       typedef typename meta::as_logical<int_type>::type bint_type;
//       typedef hypot_ctnts<A0, int_type> cts;
//       A0 x =  boost::simd::abs(a0);
//       A0 y =  boost::simd::abs(a1);
//       bA0 tinf = is_inf(x+y);
//       A0 a =  boost::simd::max(x, y);
//       A0 b =  boost::simd::min(x, y);
//       int_type ea =   exponent(a);
//       int_type eb  =  exponent(b);
//       bint_type te1 = gt(ea,cts::C1());
//       bint_type te2 = lt(eb,cts::MC1());
//       bool te3 = boost::simd::any(logical_or(te1, te2));
//       int_type e = Zero<int_type>();
//       if (te3)
//       {
//         e = select(te1, cts::MC2(), e);
//         e = select(te2, cts::C1(),  e);
//         a =  ldexp(a, e);
//         b =  ldexp(b, e);
//       }
//       A0 w = a-b;
//       bA0 test =  gt(w,b);
//       A0 t1 = a& cts::M1();
//       A0 t2 = a-t1;
//       A0 w1_2  = (t1*t1-(b*(-b)-t2*(a+t1)));
//       A0 y1 = b& cts::M1();
//       A0 y2 = b - y1;
//       t1 = bitwise_cast<A0>(bitwise_cast<int_type>(a)+cts::C3()) ;
//       t2 = (a+a) - t1;
//       A0 w2_2  = (t1*y1-(w*(-w)-(t1*y2+t2*b)));
//       w =  select(test, w1_2, w2_2);
//       w = boost::simd::sqrt(w);
//       if (te3) w = ldexp(w, -e);
//       return select(tinf, Inf<A0>(), w);
    }
  };
} } }

#endif


/*
Copyright 2009-2021 Nicolas Colombe

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 <core/coredef.hpp>
#include <core/pooledlist.hpp>
#include <math/segment.hpp>
#include <math/mathexp.hpp>

#include <boost/rational.hpp>
#include <boost/pool/object_pool.hpp>

namespace eXl
{
  template <typename IntReal = int32_t>
  struct FastRational
    :boost::less_than_comparable < FastRational<IntReal>,
    boost::equality_comparable < FastRational<IntReal>,
    boost::less_than_comparable2 < FastRational<IntReal>, IntReal,
    boost::equality_comparable2 < FastRational<IntReal>, IntReal,
    boost::addable < FastRational<IntReal>,
    boost::subtractable < FastRational<IntReal>,
    boost::multipliable < FastRational<IntReal>,
    boost::dividable < FastRational<IntReal>,
    boost::addable2 < FastRational<IntReal>, IntReal,
    boost::subtractable2 < FastRational<IntReal>, IntReal,
    boost::subtractable2_left < FastRational<IntReal>, IntReal,
    boost::multipliable2 < FastRational<IntReal>, IntReal,
    boost::dividable2 < FastRational<IntReal>, IntReal,
    boost::dividable2_left < FastRational<IntReal>, IntReal,
    boost::incrementable < FastRational<IntReal>,
    boost::decrementable < FastRational<IntReal>
    > > > > > > > > > > > > > > > >
  {
  public:

    FastRational(IntReal iNum)
      : m_N(iNum){}

    FastRational(IntReal iNum, IntReal iDen)
      : m_N(iNum)
      , m_D(iDen)
    {}

    FastRational(FastRational const& iR)
      : m_N(iR.m_N)
      , m_D(iR.m_D)
    {}

    FastRational& operator=(IntReal i) { m_N = i; m_D = 1; return *this; }
    FastRational& assign(IntReal n, IntReal d)
    {
      m_N = n;
      m_D = d;
      return *this;
    }

    IntReal numerator() const { return m_N; }
    IntReal denominator() const { return m_D; }

    FastRational& operator+= (const FastRational& r)
    {
      if(m_D == r.m_D)
      {
        m_N += r.m_N;
      }
      else
      {
        assign(m_N * r.m_D + r.m_N * m_D, m_D * r.m_D);
        sanitize();
      }
      return *this;
    }
    FastRational& operator-= (const FastRational& r)
    {
      if(m_D == r.m_D)
      {
        m_N -= r.m_N;
      }
      else
      {
        assign(m_N * r.m_D - r.m_N * m_D, m_D * r.m_D);
        sanitize();
      }
      return *this;
    }
    FastRational& operator*= (const FastRational& r)
    {
      assign(m_N * r.m_N, m_D * r.m_D);
      sanitize();
      return *this;
    }
    FastRational& operator/= (const FastRational& r)
    {
      assign(m_N * r.m_D, m_D * r.m_N);
      sanitize();
      return *this;
    }

    FastRational& operator+= (IntReal i) { m_N += i * m_D; return *this; }
    FastRational& operator-= (IntReal i) { m_N -= i * m_D; return *this; }
    FastRational& operator*= (IntReal i)
    {
      m_N *= i;
      sanitize();
    }
    FastRational& operator/= (IntReal i)
    {
      m_D *= i;
      sanitize();
    }

    const FastRational& operator++() { m_N += m_D; return *this; }
    const FastRational& operator--() { m_N -= m_D; return *this; }
    bool operator!() const { return !m_N; }

    explicit operator bool() const { return operator !() ? false : true; }

    explicit operator int() const
    {
      return m_N / m_D;
    }

    explicit operator float() const
    {
      return static_cast<float>(m_N) / m_D;
    }

    bool operator< (const FastRational& r) const
    {
      return (*this - r).m_N < 0;
    }

    bool operator== (const FastRational& r) const
    {
      return (*this - r).m_N == 0;
    }

    bool operator< (IntReal i) const
    {
      return m_N < i * m_D;
    }

    bool operator> (IntReal i) const
    {
      return m_N > i * m_D;
    }

    bool operator== (IntReal i) const
    {
      FastRational temp = *this;
      temp.reduce();
      return m_D == 1 && m_N == i;
    }

    void sanitize()
    {
      if (m_D < 0)
      {
        m_D *= -1;
        m_N *= -1;
      }
    }

    void reduce()
    {
      sanitize();

      if (m_D == 1)
      {
        return;
      }

      IntReal pgcd = Math<IntReal>::PGCD(m_N, m_D);
      m_N /= pgcd;
      m_D /= pgcd;
    }

  protected:
    IntReal m_N = 0;
    IntReal m_D = 1;
  };

  template <typename IntReal>
  inline FastRational<IntReal> operator+ (const FastRational<IntReal>& r)
  {
    return r;
  }

  template <typename IntReal>
  inline FastRational<IntReal> operator- (const FastRational<IntReal>& r)
  {
    return FastRational<IntReal>(-r.numerator(), r.denominator());
  }

  typedef /*boost::rational<int64_t>*/ /*float*/ FastRational<int64_t> QType;
  typedef Vector2<QType> Vector2Q;

  template <typename Real>
  Vector2Q ToVec2Q(Vector2<Real> const& iVec)
  {
    return Vector2Q(iVec.X(), iVec.Y());
  }

  template <typename Real>
  Vector2<Real> FromVec2Q(Vector2Q const& iVec)
  {
    return Vector2<Real>(static_cast<Real>(iVec.X()), static_cast<Real>(iVec.Y()));
    //return Vector2<Real>(iVec.X().numerator() / iVec.X().denominator(), iVec.Y().numerator() / iVec.Y().denominator());
  }

  class EXL_MATH_API Intersector
  {
  public:

    struct ActiveSegment
    {
      Vector2Q m_Point;
      uint32_t m_Idx;
    };

    struct UserEvent
    {
      Vector2i m_Position;
      uint32_t m_Idx;
    };

    typedef std::function<bool(Vector<ActiveSegment> const& iSweepLine, uint32_t iPosInSweepLine, Segment<QType> const& iCandidate)> Filter;
    typedef std::function<void(Vector<ActiveSegment> const& iSweepLine, int32_t iLowSeg, int32_t iHighSeg, UserEvent const& iEvent)> EvtCallback;

    struct Parameters
    {
      Filter m_Filter;
      EvtCallback m_UsrEvtCb;
      std::vector<UserEvent> m_UsrEvts;
    };

    Intersector();

    Err IntersectSegments(Vector<Segmenti> const& iSegments, Vector<std::pair<uint32_t, Segmenti>>& oSegs, Parameters const& iParams = Parameters());

  private:

    struct Event
    {
      enum Type
      {
        Intersection,
        End,
        Start,
        User,
      };

      Event(PooledList<uint32_t>::Pool& iPool, Vector2Q const& iPoint, uint32_t iSeg, Type iType);
      Event(PooledList<uint32_t>::Pool& iPool, Vector2Q const& iPoint, uint32_t iSeg1, uint32_t iSeg2);
      Event(Event const& iEvt);
      Event& operator=(Event const& iEvt);
      Event(Event&& iEvt);
      Event& operator=(Event&& iEvt);

      bool IsEmpty() const;

      bool operator<(Event const& iOther) const;

      Vector2Q m_Point;
      mutable PooledList<uint32_t> m_SegmentsStart;
      mutable PooledList<uint32_t> m_SegmentsInter;
      mutable PooledList<uint32_t> m_SegmentsEnd;
      //Type m_Type;
    };

    struct OrderedSeg
    {
      OrderedSeg(Segmenti const& iSeg, uint32_t iOrigSeg);

      QType GetYAt(QType iX, Event const& iEvt) const;

      Vector2Q m_Start;
      Vector2Q m_End;
      boost::optional<QType> m_Slope;
      uint32_t m_OrigSeg;
    };

    void CheckIntersection(Event const& iEvt, OrderedSeg const& iSeg1, OrderedSeg const& iSeg2);
    bool CheckIntersection(Event const& iEvt, OrderedSeg const& iSeg1, OrderedSeg const& iSeg2, boost::optional<Event>& outSeg);

    void InsertEvent(Event&& iEvt);

    void RemoveEvt(uint32_t iSeg1, uint32_t iSeg2);

    void SortSegmentExtremities();

    Vector<ActiveSegment> m_ActiveSegments;
    Vector<uint32_t> m_SegInsertCount;
    PooledList<uint32_t>::Pool m_SegListPool;
    Vector<uint32_t> m_SortArray;

    Vector<Event> m_EventQueue;

    //MemoryPool m_EvtListAlloc;
    //typedef std::multiset<Event, std::less<Event>, PooledAllocator<Event>> EventQueue;
    //EventQueue m_EventQueue;
    Vector<OrderedSeg> m_Segments;
  };
}


#include "../kontsevich_graph_series.hpp"
#include <ginac/ginac.h>
#include <iostream>
#include <fstream>
#include <limits>
#include "../util/continued_fraction.hpp"
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <Eigen/SparseQR>
#include <Eigen/OrderingMethods>
using namespace std;
using namespace GiNaC;

typedef Eigen::Triplet<double> Triplet;
typedef Eigen::SparseMatrix<double> SparseMatrix;

double threshold = 1e-5;

int main(int argc, char* argv[])
{
    if (argc != 2 && argc != 3 && argc != 4 && argc != 5)
    {
        cout << "Usage: " << argv[0] << " <graph-series-filename> [max-jacobiators] [max-jac-indegree] [--solve]\n\n"
             << "Accepts only homogeneous power series: graphs with n internal vertices at order n.\n"
             << "The optional arguments [max-jacobiators] and [max-jac-indegree] restrict the types of differential consequences of Jacobi taken into account:\n"
             << "- [max-jacobiators] restricts the number of Jacobiators per differential consequence, while\n"
             << "- [max-jac-indegree] restricts the number of arrows falling on Jacobiators.\n"
             << "When the optional argument [--solve] is specified, the undetermined variables in the input are added to the linear system to-be-solved.\n";
        return 1;
    }

    size_t max_jacobiators = numeric_limits<size_t>::max();
    size_t max_jac_indegree = numeric_limits<size_t>::max();
    if (argc == 3 || argc == 4 || argc == 5)
        max_jacobiators = stoi(argv[2]);
    if (argc == 4 || argc == 5)
        max_jac_indegree = stoi(argv[3]);

    // Reading in graph series
    string graph_series_filename(argv[1]);
    ifstream graph_series_file(graph_series_filename);
    parser coefficient_reader;
    bool homogeneous = true;
    map<size_t, set< vector<size_t> > > in_degrees;
    KontsevichGraphSeries<ex> graph_series = KontsevichGraphSeries<ex>::from_istream(graph_series_file,
        [&coefficient_reader](std::string s) -> ex { return coefficient_reader(s); },
        [&homogeneous, &in_degrees](KontsevichGraph graph, size_t order) -> bool
                                   {
                                       in_degrees[order].insert(graph.in_degrees());
                                       return homogeneous &= graph.internal() == order;
                                   }
    );
    size_t order = graph_series.precision();
    if (!homogeneous)
    {
        cerr << "Only accepting homogeneous power series: graphs with n internal vertices at order n.\n";
        return 1;
    }

    // Find number of external vertices
    // TODO: move to method in KontsevichGraphSum / KontsevichGraphSeries?
    size_t external = 0;
    for (size_t n = 0; n <= order; ++n)
    {
        if (graph_series[n].size() != 0)
        {
            external = graph_series[n].front().second.external();
            break;
        }
    }

    graph_series.reduce_mod_skew();

    size_t counter = 0;
    std::vector<symbol> coefficient_list;

    if (argc == 5 && string(argv[4]) == "--solve")
        for (auto namevar : coefficient_reader.get_syms())
            coefficient_list.push_back(ex_to<symbol>(namevar.second));

    std::map<symbol, KontsevichGraph, ex_is_less> kontsevich_jacobi_leibniz_graphs;

    for (size_t n = 2; n <= order; ++n) // need at least 2 internal vertices for Jacobi
    {
        if (graph_series[n].size() == 0)
            continue;

        cout << "h^" << n << ":\n";

        // First we choose the target vertices i,j,k of the Jacobiators (which contain 2 bivectors), in increasing order (without loss of generality)

        // Jacobi must have three distinct arguments, and not act on itself, but can act on other Jacobi

        for (size_t k = 1; k <= min(n/2, max_jacobiators); ++k)
        {
            std::vector<size_t> jacobi_vertices(3*k, n + external);
            CartesianProduct jacobi_indices(jacobi_vertices);
            for (auto jacobi_index = jacobi_indices.begin(); jacobi_index != jacobi_indices.end(); ++jacobi_index)
            {
                bool accept = true;
                for (size_t i = 0; i != k; ++i)
                {
                    if ((*jacobi_index)[i*3] >= (*jacobi_index)[i*3 + 1] || (*jacobi_index)[i*3 + 1] >= (*jacobi_index)[i*3 + 2]) // not strictly increasing
                    {
                        accept = false;
                        break;
                    }
                }
                if (!accept)
                    continue;

                // Then we choose the target vertices of the remaining n - 2*k bivectors, stored in a multi-index of length 2*(n-2*k)
                // Here we have k fewer possible targets: out of the last 2*k internal vertices, the first k act as placeholders for the respective Jacobiators,
                // to be replaced by the Leibniz rule later on
                std::vector<size_t> remaining_edges(2*(n-2*k), n + external - k);
                CartesianProduct indices(remaining_edges);
                for (auto multi_index = indices.begin(); multi_index != indices.end(); ++multi_index)
                {
                    bool accept = true;
                    for (size_t idx = 0; idx != n - 2*k; ++idx)
                    {
                        if ((*multi_index)[2*idx] >= (*multi_index)[2*idx+1])
                        {
                            accept = false; // accept only strictly increasing indices
                            break;
                        }
                        // TODO: filter out tadpoles, maybe?
                    }
                    if (!accept)
                        continue;

                    // We build the list of targets for the graph, as described above (using i,j,k and the multi-index)
                    std::vector<KontsevichGraph::VertexPair> targets(n);
                    // first part:
                    for (size_t idx = 0; idx != n - 2*k; ++idx)
                        targets[idx] = {(*multi_index)[2*idx], (*multi_index)[2*idx+1]};
                    // second part:
                    for (size_t i = 0; i != k; ++i)
                    {
                        targets[n - 2*k + 2*i].first = KontsevichGraph::Vertex((*jacobi_index)[3*i]);
                        targets[n - 2*k + 2*i].second = KontsevichGraph::Vertex((*jacobi_index)[3*i + 1]);
                        targets[n - 2*k + 2*i + 1].first = KontsevichGraph::Vertex(n + external - 2*k + 2*i);
                        targets[n - 2*k + 2*i + 1].second = KontsevichGraph::Vertex((*jacobi_index)[3*i + 2]);
                    }

                    KontsevichGraph template_graph(n, external, targets, 1, true);

                    vector<size_t> indegrees = template_graph.in_degrees();
                    if (in_degrees[n].find(indegrees) == in_degrees[n].end()) // skip terms
                        continue;

                    // Make vector of references to bad targets: those in first part with target >= (n + external - 2*k), the placeholders for the Jacobiators:
                    std::map<KontsevichGraph::Vertex*, int> bad_targets;
                    for (size_t idx = 0; idx != n - 2*k; ++idx) // look for bad targets in first part
                    {
                        if ((int)targets[idx].first >= (int)n + (int)external - 2*(int)k)
                            bad_targets[&targets[idx].first] = (int)targets[idx].first - (n + external - 2*k);
                        if ((int)targets[idx].second >= (int)n + (int)external - 2*(int)k)
                            bad_targets[&targets[idx].second] = (int)targets[idx].second - (n + external - 2*k);
                    }

                    // Count number of arrows falling on Jacs:
                    std::map<int, size_t> in_degree;
                    bool acceptable = true;
                    for (auto pair : bad_targets)
                    {
                        if (++in_degree[pair.second] == max_jac_indegree + 1)
                        {
                            acceptable = false;
                            break;
                        }
                    }
                    if (!acceptable)
                        continue;

                    KontsevichGraphSum<ex> graph_sum;

                    // Replace bad targets by Leibniz rule:
                    symbol coefficient("c_" + to_string(k) + "_" + to_string(counter));
                    std::vector<size_t> leibniz_sizes(bad_targets.size(), 2);
                    CartesianProduct leibniz_indices(leibniz_sizes);
                    for (auto leibniz_index = leibniz_indices.begin(); leibniz_index != leibniz_indices.end(); ++leibniz_index)
                    {
                        size_t idx = 0;
                        for (auto& bad_target : bad_targets)
                            *(bad_target.first) = KontsevichGraph::Vertex(external + n - 2*k + 2*(bad_target.second) + (*leibniz_index)[idx++]);

                        for (size_t i = 0; i != k; ++i)
                        {
                            for (auto jacobi_targets_choice : std::vector< std::vector<KontsevichGraph::Vertex> >({ { targets[n-2*k+2*i].first, targets[n-2*k+2*i].second, targets[n-2*k+2*i+1].second },
                                                                                { targets[n-2*k+2*i].second, targets[n-2*k+2*i+1].second, targets[n-2*k+2*i].first },
                                                                                { targets[n-2*k+2*i+1].second, targets[n-2*k+2*i].first, targets[n-2*k+2*i].second } }))
                            {
                                // Set Jacobiator targets to one of the three permutatations
                                targets[n-2*k+2*i].first = jacobi_targets_choice[0];
                                targets[n-2*k+2*i].second = jacobi_targets_choice[1];
                                targets[n-2*k+2*i+1].second = jacobi_targets_choice[2];

                                KontsevichGraph graph(n, external, targets);

                                graph_sum += KontsevichGraphSum<ex>({ { coefficient, graph } });
                            }
                        }
                    }

                    graph_sum.reduce_mod_skew();
                    if (graph_sum.size() != 0)
                    {
                        cerr << "\r" << ++counter;
                        coefficient_list.push_back(coefficient);
                        kontsevich_jacobi_leibniz_graphs[coefficient] = template_graph;
                    }
                    graph_series[n] -= graph_sum;
                }
            }
        }
    }

    cout << "\nNumber of coefficients: " << coefficient_list.size() << "\n";
    cout << "\nNumber of terms: " << graph_series[order].size() << "\n";
    cout << "\nNumber of terms per coefficient: " << (float)graph_series[order].size()/coefficient_list.size() << "\n";

    cout.flush();

    cerr << "\nReducing...\n";
    graph_series.reduce_mod_skew();

    lst equations;

    for (size_t n = 0; n <= order; ++n)
        for (auto& term : graph_series[n])
        {
            cerr << term.second.encoding() << "    " << term.first << "==0\n";
            equations.append(term.first);
        }

    // Set up sparse matrix linear system

    cerr << "Setting up linear system for numerical solution...\n";
    size_t rows = equations.nops();
    size_t cols = coefficient_list.size();

    Eigen::VectorXd b(rows);
    SparseMatrix matrix(rows,cols);

    std::vector<Triplet> tripletList;
    size_t idx = 0;
    for (ex equation : equations)
    {
        if (!is_a<add>(equation))
            equation = lst({ equation });
        for (ex term : equation)
        {
            if (!is_a<mul>(term))
                term = lst({ term });
            double prefactor = 1;
            symbol coefficient("one");
            for (ex factor : term)
            {
                if (is_a<numeric>(factor))
                    prefactor *= ex_to<numeric>(factor).to_double();
                else if (is_a<symbol>(factor))
                    coefficient = ex_to<symbol>(factor);
            }
            if (coefficient.get_name() == "one") // constant term
                b(idx) = -prefactor;
            else
                tripletList.push_back(Triplet(idx,find(coefficient_list.begin(), coefficient_list.end(), coefficient) - coefficient_list.begin(), prefactor));
                // NB: Eigen uses zero-based indices (contrast MATLAB, Mathematica)
        }
        ++idx;
    }

    matrix.setFromTriplets(tripletList.begin(), tripletList.end());

    cerr << "Solving linear system numerically...\n";

    Eigen::SparseQR< SparseMatrix, Eigen::COLAMDOrdering<int> > qr(matrix);
    Eigen::VectorXd x = qr.solve(b);

    cerr << "Residual norm = " << (matrix * x - b).squaredNorm() << "\n";

    cerr << "Rounding...\n";
    x = x.unaryExpr([](double elem) { return fabs(elem) < threshold ? 0.0 : elem; });

    cerr << "Still a solution? Residual norm = " << (matrix * x - b).squaredNorm() << "\n";

    cerr << "Approximating numerical solution by rational solution...\n";

    lst zero_substitution;
    lst solution_substitution;
    for (int i = 0; i != x.size(); i++)
    {
        ex result = best_rational_approximation(x.coeff(i), threshold);
        if (result == 0)
            zero_substitution.append(coefficient_list[i] == 0);
        else
            solution_substitution.append(coefficient_list[i] == result);
    }

    cerr << "Substituting zeros...\n";

    for (auto& order: graph_series)
        for (auto& term : graph_series[order.first])
            term.first = term.first.subs(zero_substitution);

    cerr << "Reducing zeros...\n";

    graph_series.reduce_mod_skew();

    for (size_t n = 0; n <= graph_series.precision(); ++n)
    {
        cout << "h^" << n << ":\n";
        for (auto& term : graph_series[n])
        {
            cout << term.second.encoding() << "    " << term.first << "\n";
        }
    }

    cerr << "Verifying solution...\n";

    for (auto& order: graph_series)
        for (auto& term : graph_series[order.first])
            term.first = term.first.subs(solution_substitution);

    graph_series.reduce_mod_skew();

    cout << "Do we really have a solution? " << (graph_series == 0 ? "Yes" : "No") << "\n";

    if (graph_series == 0)
    {
        for (ex subs : solution_substitution)
            cout << kontsevich_jacobi_leibniz_graphs[ex_to<symbol>(subs.lhs())].encoding() << "    " << subs << "\n";
    }
}