#ifndef _read_h_included_
#define _read_h_included_

#include <assert.h>
#include <stdlib.h>
#include <algorithm>
#include <cctype>
#include <fstream>
#include <functional>
#include <iostream>
#include <map>
#include <numeric>
#include <set>
#include <sstream>
#include <vector>
#include <sstream>
#include <string>
#include <boost/config.hpp>
#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <jsc/bioinfo/gene_anno.hpp>
#include <jsc/util/log.hpp>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#include "splicing_graph.h"
#include "accessible_read_starts.h"

using namespace std;
using namespace boost;
using namespace boost::numeric;
using namespace jsc::bioinfo;
using namespace jsc::util;

using boost::shared_ptr;

// returns the index (in the isoform) of the first matching exon in the isoform
long is_connected_exons_compatible_with_isoform(
		vector<unsigned long> const & exon_indices,
		vector<unsigned long> const & iso_exon_indices) {
	vector<unsigned long>::const_iterator itr, itr2;
	itr = exon_indices.begin();
	itr2 = iso_exon_indices.begin();
	bool is_compatible = false;
	bool found_first_match = false;
	long first_match_idx = 0;
	while (itr != exon_indices.end()) {
		if (itr2 == iso_exon_indices.end()) {
			is_compatible = false;
			break;
		}
		if (*itr != *itr2) {
			if (!found_first_match) {
				itr2++;
				first_match_idx++;
			} else {
				is_compatible = false;
				break;
			}
		} else {
			if (!found_first_match) {
				found_first_match = true;
				is_compatible = true;
				itr++;
				itr2++;
			} else {
				itr++;
				itr2++;
			}
		}
	}
	return (is_compatible ? first_match_idx : -1);
}

class ReadType {
	public:
		static const int LONG_READ = 0;
		static const int MEDIUM_READ = 1;
		static const int SHORT_READ = 2;
		static const int SHORT_CONTIG = 3;
		static const int CONTIG = 4;
		static const int LONG_READ_PE = 5;
		static const int MEDIUM_READ_PE = 6;
		static const int SHORT_READ_PE = 7;
		static const string TYPES[];
};
const string ReadType::TYPES[] = {"LONG_READ", "MEDIUM_READ", "SHORT_READ", "SHORT_CONTIG", "CONTIG", "LONG_READ_PE", "MEDIUM_READ_PE", "SHORT_READ_PE"};

class Read {
	public:
		string read_type;
		vector<unsigned long> exon_indices;
		unsigned long start_at_first_exon;
		unsigned long end_at_last_exon;

		Read(shared_ptr<AccessibleReadStarts> const & ars
				= shared_ptr<AccessibleReadStarts>())
			: ars(ars) {
			read_length = 0;
			expected_read_length = 0;
			read_type = "General read";
		}

		unsigned long get_num_isoforms() const {
			return ars->get_num_isoforms();
		}

		unsigned long get_read_length() const {
			return read_length;
		}

		shared_ptr<AccessibleReadStarts> get_ARS() const {
			return ars;
		}

		virtual unsigned long get_read_span() const {
			return read_length;
		}

		unsigned long get_expected_read_length() const {
			return expected_read_length;
		}

		virtual unsigned long get_expected_read_span() const {
			return expected_read_length;
		}

		virtual bool is_compatible_with_iso(
				unsigned long const & iso_idx) const = 0;

		virtual void build(
				interval_list<long> const & read_il,
				SetExonp const & exonps,
				interval_list<long> const & read_il2 = interval_list<long>()) = 0;

		/// Generates a read from an isoform.
		//
		/// The start position of the read in the isoform is either
		/// determined by \a fixed_read_start or chosen randomly
		/// according to the actual type of the read.
		//
		/// \param[in] iso_idx							index of the isoform from which
		///																	the read will be generated.
		///																	exons in the corresponding gene.
		/// \param[in] fixed_read_start			When >= 0, specifies the start
		///																	position (in the isoform) of the
		///																	read to be generated.
		///	\returns	The actual start position (in the isoform)
		///						of the generated read.
		virtual unsigned long generate_read(
				unsigned long const & iso_idx,
				long fixed_read_start = -1) = 0;

		virtual double prob_generated_by_iso(
				unsigned long const & iso_idx) const = 0;

		virtual string to_string() const = 0;

		virtual ~Read() {}

	protected:
		shared_ptr<AccessibleReadStarts> ars;
		unsigned long read_length;
		unsigned long expected_read_length;
};

ostream& operator << (ostream& os, Read const & r) {
	os << r.to_string();
	return os;
};

typedef shared_ptr<Read> Read_ptr;

class Read_stats {
	public:
		static const unsigned long medium_single_expected_read_length = 250;
		static const double medium_cost_per_bp = 7E-5;
		static const unsigned long short_single_expected_read_length = 30;
		static const unsigned long short_paired_expected_insert_size = 300;
		static const double short_cost_per_bp = 7E-6;
};

class Read_single : public Read {
	public:
		Read_single(shared_ptr<AccessibleReadStarts> ars
				= shared_ptr<AccessibleReadStarts>())
			: Read(ars)	{
			rng = NULL;
			read_type = "General single read";
		}

		virtual bool is_compatible_with_iso(
				unsigned long const & iso_idx) const {
			return (is_connected_exons_compatible_with_isoform(
						exon_indices, ars->iso_exon_indices[iso_idx]) >= 0);
		}

		virtual void build(
				interval_list<long> const & read_il,
				SetExonp const & exonps,
				interval_list<long> const & read_il2 = interval_list<long>()) {
			L_(debug2) << "Read::build";
			unsigned long matching_length = 0;
			set<unsigned long> index_set;
			exon_indices.clear();
			start_at_first_exon = end_at_last_exon = 0;
			bool found_start = false;
			unsigned long il_idx = 0;
			SetExonp::const_iterator itr = exonps.begin();
			long cur_start, cur_end, cur_exon_start;
			cur_exon_start = 0;
			L_(debug2) << "Read: " << read_il;
			L_(debug2) << "Exons: " << exonps;
			while (il_idx < read_il.get_num_intervals()) {
				cur_start = read_il.get_starts()[il_idx];
				cur_end = read_il.get_ends()[il_idx];
				L_(debug2) << "Current interval: [" << cur_start << ","
					<< cur_end << ")";
				// find all the exons that overlap with/contain the interval
				while (itr != exonps.end() && (*itr)->start < cur_end) {
					L_(debug2) << "Current exon: [" << (*itr)->start << ","
						<< (*itr)->end << ")";
					cur_exon_start = max(cur_exon_start, (*itr)->start);
					if (cur_start >= cur_exon_start && cur_start < (*itr)->end) {
						if (!found_start) {
							L_(debug2) << "Found start";
							found_start = true;
							start_at_first_exon = cur_start - (*itr)->start;
						} else if (cur_start > cur_exon_start) {
							break;
						}

						L_(debug2) << "Add exon #" << distance(exonps.begin(), itr);
						index_set.insert(distance(exonps.begin(), itr));
						cur_exon_start = min((*itr)->end, cur_end);
						matching_length += cur_exon_start - cur_start;

						if (cur_end < (*itr)->end) {
							cur_start = cur_end;
							end_at_last_exon = cur_end - (*itr)->start;
							++il_idx;
							break;
						} else if (cur_end == (*itr)->end) {
							cur_start = cur_end;
							end_at_last_exon = cur_end - (*itr)->start;
							++il_idx;
						} else {
							cur_start = (*itr)->end;
							end_at_last_exon = (*itr)->end - (*itr)->start;
						}
					} else if (cur_exon_start > (*itr)->start
							&& cur_exon_start < (*itr)->end) {
						break;
					}
					itr++;
				}
				if (cur_start == cur_end) {
					continue;
				} else {
					break;
				}
			}

			BOOST_FOREACH (unsigned long i, index_set) {
				exon_indices.push_back(i);
			}
			read_length = matching_length;
		}

		virtual unsigned long generate_read(
				unsigned long const & iso_idx,
				long fixed_read_start = -1) {
			assert(rng != NULL || fixed_read_start >= 0);
			vector<unsigned long> const & iso_exon_indices =
				ars->iso_exon_indices[iso_idx];
			vector<unsigned long> const & iso_exon_total_lengths =
				ars->iso_exon_total_lengths[iso_idx];

			exon_indices.clear();
			unsigned long read_start;
			if (fixed_read_start < 0) {	// randomly generate a read start
				read_start = (unsigned long)(
						gsl_ran_flat(rng, 0,
							ars->get_iso_ARS_total_length(iso_idx)));
				read_start = ars->ARStart2IsoStart(iso_idx, read_start);
			} else {
				read_start = fixed_read_start;
			}

			unsigned long read_end = read_start + get_expected_read_length();
			read_length = read_end - read_start;
			L_(debug2) << "Generating read [" << read_start
				<< ", " << read_end << ") length=" << read_length << "bp";
			vector<unsigned long>::const_iterator itr =
				upper_bound(iso_exon_total_lengths.begin(),
						iso_exon_total_lengths.end(),
						read_start);
			assert(itr != iso_exon_total_lengths.end());
			unsigned long start_exon_idx = distance(
					iso_exon_total_lengths.begin(), itr);
			itr = lower_bound(iso_exon_total_lengths.begin(),
					iso_exon_total_lengths.end(),
					read_end);
			assert(itr != iso_exon_total_lengths.end());
			unsigned long end_exon_idx = distance(
					iso_exon_total_lengths.begin(), itr);
			L_(debug2) << "Start exon: "
				<< iso_exon_indices[start_exon_idx]
				<< ", end exon: "
				<< iso_exon_indices[end_exon_idx];
			for (unsigned long i = start_exon_idx;
					i <= end_exon_idx; ++i) {
				exon_indices.push_back(iso_exon_indices[i]);
			}
			start_at_first_exon = read_start -
				(start_exon_idx == 0 ?
				 0 : iso_exon_total_lengths[start_exon_idx - 1]);
			end_at_last_exon = read_end -
				(end_exon_idx == 0 ?
				 0 : iso_exon_total_lengths[end_exon_idx - 1]);

			return read_start;
		}

		virtual double prob_generated_by_iso(
				unsigned long const & iso_idx) const {
			double num_diff_reads = ars->get_iso_ARS_total_length(iso_idx);
			if (num_diff_reads <= 0) {
				L_(warning) << read_type << ": supported read length > isoform length" << endl;
				return 0;
			}
			double prob = (double)1.0 / num_diff_reads;
			return prob;
		}

		virtual string to_string() const {
			std::ostringstream os;
			os << "[" << read_type << "] "
				<< "(Exons) ";
			BOOST_FOREACH(unsigned long const & idx, exon_indices) {
				os << idx << ",";
			}
			os << " (Start at first exon) " << start_at_first_exon << " ";
			os << "(End at last exon) " << end_at_last_exon;
			return os.str();
		}
	protected:
		gsl_rng * rng;
};

class Read_medium_single : public Read_single {
	public:
		Read_medium_single(shared_ptr<AccessibleReadStarts> const & ars
				= shared_ptr<AccessibleReadStarts>(),
				gsl_rng * r = NULL,
				unsigned long expected_length =
				Read_stats::medium_single_expected_read_length)
			: Read_single(ars) {
			rng = r;
			expected_read_length = expected_length;
			read_type = "Medium single read";
		}
};

class Read_short_single : public Read_single {
	public:
		Read_short_single(shared_ptr<AccessibleReadStarts> const & ars
				= shared_ptr<AccessibleReadStarts>(),
				gsl_rng * r = NULL,
				unsigned long expected_length =
				Read_stats::short_single_expected_read_length)
			: Read_single(ars) {
			rng = r;
			expected_read_length = expected_length;
			read_type = "Short single read";
		}
};

class Read_paired: public Read {
	public:
		Read_single end1;
		Read_single end2;
		Read_single span;

		Read_paired(shared_ptr<AccessibleReadStarts> ars
				= shared_ptr<AccessibleReadStarts>(),
				double tolerance = 0)
			: Read(ars), tolerance(tolerance) {
			insert_size = expected_insert_size = 0;
			read_type = "General paired-end read";
			end1 = Read_single(ars);
			end2 = Read_single(ars);
			span = Read_single(ars);
		}

		unsigned long get_expected_insert_size() const {
			return expected_insert_size;
		}

		virtual unsigned long get_read_span() const {
			return read_length + insert_size;
		}

		virtual unsigned long get_expected_read_span() const {
			return (end1.get_expected_read_length()
				+ end2.get_expected_read_length()
				+ expected_insert_size);
		}

		virtual bool is_compatible_with_iso(
				unsigned long const & iso_idx) const {
			assert(end1.exon_indices.size() > 0
					&& end2.exon_indices.size() > 0);
			if (end1.is_compatible_with_iso(iso_idx)
					&& end2.is_compatible_with_iso(iso_idx)) {
				if (expected_insert_size == 0) {
					return true;
				}
				vector<unsigned long> const & iso_exon_indices =
					ars->iso_exon_indices[iso_idx];
				vector<unsigned long> const & exon_total_lengths =
					ars->iso_exon_total_lengths[iso_idx];

				vector<unsigned long>::const_iterator itr;
				unsigned long end1_last_exon_idx = end1.exon_indices
					[end1.exon_indices.size() - 1];
				itr = lower_bound(iso_exon_indices.begin(),
						iso_exon_indices.end(),
						end1_last_exon_idx);
				unsigned long exon_idx = distance(
						iso_exon_indices.begin(), itr);
				unsigned long end1_end =
					(exon_idx > 0 ? exon_total_lengths[exon_idx - 1] : 0)
					+ end1.end_at_last_exon;
				unsigned long end2_first_exon_idx = end2.exon_indices[0];
				itr = lower_bound(iso_exon_indices.begin(),
						iso_exon_indices.end(),
						end2_first_exon_idx);
				exon_idx = distance(iso_exon_indices.begin(), itr);
				unsigned long end2_start =
					(exon_idx > 0 ? exon_total_lengths[exon_idx - 1] : 0)
					+ end2.start_at_first_exon;
				double gap = end2_start - end1_end;
				double diff = gap / (double)(expected_insert_size);
				if (diff <= (1.0 + tolerance + epsilon)
						&& diff >= (1.0 - tolerance - epsilon)) {
					return true;
				}
			}
			return false;
		}

		virtual double prob_generated_by_iso(
				unsigned long const & iso_idx) const {
			double num_diff_reads = ars->get_iso_ARS_total_length(iso_idx);
			if (num_diff_reads <= 0) {
				L_(warning) << read_type << ": supported paired-end read length (including insert) > isoform length" << endl;
				return 0;
			}
			double prob = (double)1.0 / num_diff_reads;
			return prob;
		}

		virtual string to_string() const {
			std::ostringstream os;
			os << "[" << read_type << "]"
				<< " End #1: " << end1.to_string()
				<< "; End #2: " << end2.to_string();
			return os.str();
		}

		virtual void build(
				interval_list<long> const & read_il,
				SetExonp const & exonps,
				interval_list<long> const & read_il2 = interval_list<long>()) {
			L_(debug2) << "Read::build";
			end1.build(read_il, exonps);
			end2.build(read_il2, exonps);
			read_length = end1.get_read_length() + end2.get_read_length();
			/// \todo compute insert size and construct span, exon_indices, etc.
		}

		virtual unsigned long generate_read(
				unsigned long const & iso_idx,
				long fixed_read_start = -1) {
			assert(rng != NULL || fixed_read_start >= 0);

			unsigned long read_start;
			if (fixed_read_start < 0) {	// randomly generate a read start
				read_start = (unsigned long)(
						gsl_ran_flat(rng, 0,
							ars->get_iso_ARS_total_length(iso_idx)));
				read_start = ars->ARStart2IsoStart(iso_idx, read_start);
			} else {
				read_start = fixed_read_start;
			}

			unsigned long end1_start =
				end1.generate_read(iso_idx, read_start);
			unsigned long end2_start =
				end1_start + end1.get_read_length()
				+ expected_insert_size;
			end2.generate_read(iso_idx, end2_start);
			read_length = end1.get_read_length() + end2.get_read_length();

			span.generate_read(iso_idx, read_start);
			exon_indices = span.exon_indices;
			start_at_first_exon = span.start_at_first_exon;
			end_at_last_exon = span.end_at_last_exon;

			return end1_start;
		}

	protected:
		unsigned long insert_size;
		unsigned long expected_insert_size;
		double tolerance;
		static const double epsilon = 1E-5;
		gsl_rng * rng;
};

void generate_reads(unsigned long const & num_reads,
		Isoforms const & iso,
		shared_ptr<Read> readp,
		gsl_rng * rng,
		ublas::matrix<double> & m_delta_G,
		bool known_isoforms_only = false) {
	gsl_ran_discrete_t * discrete_g = gsl_ran_discrete_preproc(
			iso.num_known_isoforms, iso.known_iso_probs);

	for (unsigned long i = 0; i < num_reads; ++i) {
		unsigned long iso_idx = gsl_ran_discrete(rng, discrete_g);
		L_(debug2) << "Selected known isoform " << iso_idx;
		if (!known_isoforms_only) {
			iso_idx = iso.known_iso_indices[iso_idx];
		}
		readp->generate_read(iso_idx);
		L_(debug2) << *readp;
		if (known_isoforms_only) {
			for (unsigned long j = 0;
					j < iso.num_known_isoforms; ++j) {
				if (readp->is_compatible_with_iso(j)) {
					m_delta_G(i,j) = readp->prob_generated_by_iso(j);
				}
			}
		} else {
			for (unsigned long j = 0;
					j < iso.num_possible_isoforms; ++j) {
				if (readp->is_compatible_with_iso(j)) {
					m_delta_G(i,j) = readp->prob_generated_by_iso(j);
				}
			}
		}
	}

	gsl_ran_discrete_free(discrete_g);
}

class Read_short_paired: public Read_paired {
	public:
		Read_short_paired(shared_ptr<AccessibleReadStarts> ars
				= shared_ptr<AccessibleReadStarts>(),
				gsl_rng * r = NULL,
				unsigned long expected_length =
				2 * Read_stats::short_single_expected_read_length,
				unsigned long expected_insert =
				Read_stats::short_paired_expected_insert_size,
				double tolerance = 0)
			: Read_paired(ars, tolerance) {
			rng = r;
			expected_read_length = expected_length;
			insert_size = expected_insert_size = expected_insert;
			read_type = "Short paired-end read";
			shared_ptr<AccessibleReadStarts> end_ars(
					new AccessibleReadStarts(
						ars->iso_exon_indices,
						ars->iso_exon_total_lengths,
						ars->exon_lengths,
						expected_length / 2));
			end1 = Read_short_single(end_ars, r, expected_length / 2);
			end2 = Read_short_single(end_ars, r, expected_length / 2);
			span = Read_medium_single(ars, r, expected_length + expected_insert);
		}
};

void compute_iso_probs_EM_step(
		unsigned long num_possible_isoforms,
		vector<ublas::matrix<double> > const & m_delta_Gs,
		vector<double> const & old_theta,
		vector<double> & new_theta) {
	for (unsigned k = 0; k < num_possible_isoforms; ++k) {
		double sum_zeta = 0;
		double num_total_reads = 0;
		BOOST_FOREACH (ublas::matrix<double> const & m_delta_G, m_delta_Gs) {
			unsigned long num_reads = m_delta_G.size1();
			num_total_reads += (double)num_reads;
			for (unsigned long i = 0; i < num_reads; ++i) {
				double sum_local_probs = 0;
				for (unsigned k2 = 0; k2 < num_possible_isoforms; ++k2) {
					sum_local_probs += old_theta[k2] * m_delta_G(i, k2);
				}
				if (sum_local_probs > 0) {
					double local_prob = old_theta[k] * m_delta_G(i, k);
					if (local_prob > 0) {
						sum_zeta += local_prob / sum_local_probs;
					}
				}
			}
		}
		new_theta[k] = sum_zeta / num_total_reads;
	}
}

double reads_log_likelihood(
		unsigned long num_possible_isoforms,
		vector<ublas::matrix<double> > const & m_delta_Gs,
		vector<double> const & theta) {
	double ll = 0;
	BOOST_FOREACH (ublas::matrix<double> const & m_delta_G, m_delta_Gs) {
		unsigned long num_reads = m_delta_G.size1();
		for (unsigned long i = 0; i < num_reads; ++i) {
			double sum_local_probs = 0;
			for (unsigned k = 0; k < num_possible_isoforms; ++k) {
				sum_local_probs += theta[k] * m_delta_G(i, k);
			}
			ll += log(sum_local_probs);
		}
	}
	return ll;
}

void compute_iso_probs_EM(
		unsigned long num_possible_isoforms,
		vector<ublas::matrix<double> > const & m_delta_Gs,
		vector<double> & theta) {
	theta.resize(num_possible_isoforms,
			(double)1.0 / (double)num_possible_isoforms);
	double ll, old_ll;
	vector<double> old_theta;
	unsigned long count = 0;
	do {
		old_theta = theta;
		old_ll = reads_log_likelihood(
				num_possible_isoforms, m_delta_Gs, old_theta);
		compute_iso_probs_EM_step(num_possible_isoforms,
				m_delta_Gs, old_theta, theta);
		ll = reads_log_likelihood(
				num_possible_isoforms, m_delta_Gs, theta);
		if (++count == 100) {
			L_(debug) << "log likelihood: " << old_ll << " -> " << ll;
			count = 0;
		}
	} while (abs(1.0 - old_ll / ll) > 1E-6);
}

void err_known_iso_probs(Isoforms const & iso,
		vector<double> const & theta, double & abs_err,
		double & sse) {
	abs_err = sse = 0;
	for (unsigned long i = 0; i < iso.num_known_isoforms; ++i) {
		abs_err += abs(iso.known_iso_probs[i]
				- theta[iso.known_iso_indices[i]]);
		sse += (iso.known_iso_probs[i] - theta[iso.known_iso_indices[i]]) *
			(iso.known_iso_probs[i] - theta[iso.known_iso_indices[i]]);
	}
}

void err_possible_iso_probs(Isoforms const & iso,
		vector<double> const & theta, double & abs_err,
		double & sse) {
	abs_err = sse = 0;
	for (unsigned long i = 0; i < iso.num_possible_isoforms; ++i) {
		abs_err += theta[i];
		sse += theta[i] * theta[i];
	}
	for (unsigned long i = 0; i < iso.num_known_isoforms; ++i) {
		abs_err -= theta[iso.known_iso_indices[i]];
		abs_err += abs(iso.known_iso_probs[i]
				- theta[iso.known_iso_indices[i]]);
		sse -= theta[iso.known_iso_indices[i]] * theta[iso.known_iso_indices[i]];
		sse += (iso.known_iso_probs[i] - theta[iso.known_iso_indices[i]]) *
			(iso.known_iso_probs[i] - theta[iso.known_iso_indices[i]]);
	}
}

#endif


/* -----------------------------------------------------------------------------
 * Copyright 2021 Jonathan Haigh
 * SPDX-License-Identifier: MIT
 * ---------------------------------------------------------------------------*/

#ifndef SQ_INCLUDE_GUARD_core_Primitive_inl_h_
#define SQ_INCLUDE_GUARD_core_Primitive_inl_h_

#include "core/errors.h"
#include "core/narrow.h"
#include "core/typeutil.h"

#include <gsl/gsl>
#include <iomanip>
#include <sstream>
#include <string_view>
#include <type_traits>

namespace sq {

template <PrimitiveAlternative P> struct PrimitiveTypeName {};

template <> struct PrimitiveTypeName<PrimitiveString> {
  static constexpr std::string_view value = "PrimitiveString";
};

template <> struct PrimitiveTypeName<PrimitiveInt> {
  static constexpr std::string_view value = "PrimitiveInt";
};

template <> struct PrimitiveTypeName<PrimitiveFloat> {
  static constexpr std::string_view value = "PrimitiveFloat";
};

template <> struct PrimitiveTypeName<PrimitiveBool> {
  static constexpr std::string_view value = "PrimitiveBool";
};

template <> struct PrimitiveTypeName<PrimitiveNull> {
  static constexpr std::string_view value = "PrimitiveNull";
};

constexpr PrimitiveInt to_primitive_int(auto value, auto &&...format_args) {
  return narrow<PrimitiveInt>(SQ_FWD(value), SQ_FWD(format_args)...);
}

constexpr PrimitiveFloat to_primitive_float(auto value, auto &&...format_args) {
  return narrow<PrimitiveFloat>(SQ_FWD(value), SQ_FWD(format_args)...);
}

namespace detail {

struct PrimitiveToStrVisitor {
  SQ_ND std::string operator()(const PrimitiveString &value) {
    os_ << std::quoted(value);
    return os_.str();
  }

  SQ_ND std::string operator()(PrimitiveBool value) {
    os_ << std::boolalpha << value;
    return os_.str();
  }

  SQ_ND std::string operator()(const PrimitiveAlternative auto &value) {
    os_ << value;
    return os_.str();
  }

private:
  std::ostringstream os_;
};

template <PrimitiveAlternative T> struct ConvertPrimitiveBase {
  // We can always convert T to T
  SQ_ND T operator()(const T &value) { return value; }

  // By default, don't allow any other conversions
  template <PrimitiveAlternative U> SQ_ND T operator()(SQ_MU const U &value) {
    throw InvalidConversionError{primitive_type_name_v<U>,
                                 primitive_type_name_v<T>};
  }
};

template <PrimitiveAlternative T>
struct ConvertPrimitive : ConvertPrimitiveBase<T> {
  using ConvertPrimitiveBase<T>::operator();
};

template <>
struct ConvertPrimitive<PrimitiveFloat> : ConvertPrimitiveBase<PrimitiveFloat> {
  using ConvertPrimitiveBase<PrimitiveFloat>::operator();

  // Allow PrimitiveInt -> PrimitiveFloat conversions
  SQ_ND PrimitiveFloat operator()(const PrimitiveInt &value) {
    return to_primitive_float(value);
  }
};

} // namespace detail

std::string primitive_to_str(const PrimitiveAlternative auto &value) {
  return detail::PrimitiveToStrVisitor{}(value);
}

template <PrimitiveAlternative T> T convert_primitive(const Primitive &value) {
  return std::visit(detail::ConvertPrimitive<T>{}, value);
}

} // namespace sq

#endif // SQ_INCLUDE_GUARD_core_Primitive_inl_h_


#if !defined(PETIGAPART_H)
#define PETIGAPART_H

#include <petsc.h>

PETSC_EXTERN PetscErrorCode IGA_Partition(PetscInt,PetscInt,
                                          PetscInt,const PetscInt[],
                                          PetscInt[],PetscInt[]);
PETSC_EXTERN PetscErrorCode IGA_Distribute(PetscInt,
                                           const PetscInt[],const PetscInt[],
                                           const PetscInt[],PetscInt[],PetscInt[]);

#endif/*PETIGAPART_H*/


#pragma once

#include <gsl/span>

#include "aas/aas_math.h"

namespace aas {

	struct AnimPlayerStream;
	class AnimatedModel;
	struct SkelAnim;
	struct SkelAnimEvent;
	struct SkelBoneState;
	class IAnimEventHandler;
	class Skeleton;
	struct AnimEvent;

	class AnimPlayer {
	public:
		AnimatedModel * ownerAnim;
		char field_D;
		char field_E;
		char field_F;
		float weight; // 1.0 = Fully weighted, otherwise weighted with primary anims
		float fadingSpeed; // Velocity with which weight is being changed
		int eventHandlingDepth;
		AnimPlayer * nextRunningAnim;
		AnimPlayer * prevRunningAnim;
		const SkelAnim * animation;
		int streamCount;
		AnimPlayerStream * streams[4];
		float streamFps[4]; // "frames per drive unit"
		int8_t streamVariationIndices[4]; // indices into streamVariationIds
		int8_t streamVariationIds[4];
		int8_t variationId[4];
		float currentTimeEvents;
		float currentTime;
		float duration;
		float frameRate;
		float distancePerSecond;
		std::vector<AnimEvent> events;
		IAnimEventHandler* eventHandler;
	public:

		AnimPlayer();
		~AnimPlayer();

		void GetDistPerSec(float* distPerSec);
		void GetRotationPerSec(float* rotationPerSec);
		void AdvanceEvents(float timeChanged, float distanceChanged, float rotationChanged);
		void FadeInOrOut(float timeChanged);
		void method6(gsl::span<SkelBoneState> boneStateOut, float timeChanged, float distanceChanged, float rotationChanged);
		void SetTime(float time);
		float GetCurrentFrame();
		float method9();
		float method10();
		void EnterEventHandling();
		void LeaveEventHandling();
		int GetEventHandlingDepth();

		void Attach(AnimatedModel *owner, int animIdx, IAnimEventHandler *eventHandler);
		void Setup2(float a);

	private:
		void SetEvents(const AnimatedModel *owner, const SkelAnim *anim);

	};

	AnimPlayerStream * CreateAnimPlayerStream(Skeleton* skeleton);

}


/* matrix/gsl_matrix_short.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

#ifndef __GSL_MATRIX_SHORT_H__
#define __GSL_MATRIX_SHORT_H__

#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_vector_short.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct
{
  size_t size1;
  size_t size2;
  size_t tda;
  short * data;
  gsl_block_short * block;
  int owner;
} gsl_matrix_short;

typedef struct
{
  gsl_matrix_short matrix;
} _gsl_matrix_short_view;

typedef _gsl_matrix_short_view gsl_matrix_short_view;

typedef struct
{
  gsl_matrix_short matrix;
} _gsl_matrix_short_const_view;

typedef const _gsl_matrix_short_const_view gsl_matrix_short_const_view;

/* Allocation */

gsl_matrix_short *
gsl_matrix_short_alloc (const size_t n1, const size_t n2);

gsl_matrix_short *
gsl_matrix_short_calloc (const size_t n1, const size_t n2);

gsl_matrix_short *
gsl_matrix_short_alloc_from_block (gsl_block_short * b,
                                   const size_t offset,
                                   const size_t n1,
                                   const size_t n2,
                                   const size_t d2);

gsl_matrix_short *
gsl_matrix_short_alloc_from_matrix (gsl_matrix_short * m,
                                    const size_t k1,
                                    const size_t k2,
                                    const size_t n1,
                                    const size_t n2);

gsl_vector_short *
gsl_vector_short_alloc_row_from_matrix (gsl_matrix_short * m,
                                        const size_t i);

gsl_vector_short *
gsl_vector_short_alloc_col_from_matrix (gsl_matrix_short * m,
                                        const size_t j);

void gsl_matrix_short_free (gsl_matrix_short * m);

/* Views */

_gsl_matrix_short_view
gsl_matrix_short_submatrix (gsl_matrix_short * m,
                            const size_t i, const size_t j,
                            const size_t n1, const size_t n2);

_gsl_vector_short_view
gsl_matrix_short_row (gsl_matrix_short * m, const size_t i);

_gsl_vector_short_view
gsl_matrix_short_column (gsl_matrix_short * m, const size_t j);

_gsl_vector_short_view
gsl_matrix_short_diagonal (gsl_matrix_short * m);

_gsl_vector_short_view
gsl_matrix_short_subdiagonal (gsl_matrix_short * m, const size_t k);

_gsl_vector_short_view
gsl_matrix_short_superdiagonal (gsl_matrix_short * m, const size_t k);

_gsl_matrix_short_view
gsl_matrix_short_view_array (short * base,
                             const size_t n1,
                             const size_t n2);

_gsl_matrix_short_view
gsl_matrix_short_view_array_with_tda (short * base,
                                      const size_t n1,
                                      const size_t n2,
                                      const size_t tda);

_gsl_matrix_short_view
gsl_matrix_short_view_vector (gsl_vector_short * v,
                              const size_t n1,
                              const size_t n2);

_gsl_matrix_short_view
gsl_matrix_short_view_vector_with_tda (gsl_vector_short * v,
                                       const size_t n1,
                                       const size_t n2,
                                       const size_t tda);

_gsl_matrix_short_const_view
gsl_matrix_short_const_submatrix (const gsl_matrix_short * m,
                                  const size_t i, const size_t j,
                                  const size_t n1, const size_t n2);

_gsl_vector_short_const_view
gsl_matrix_short_const_row (const gsl_matrix_short * m,
                            const size_t i);

_gsl_vector_short_const_view
gsl_matrix_short_const_column (const gsl_matrix_short * m,
                               const size_t j);

_gsl_vector_short_const_view
gsl_matrix_short_const_diagonal (const gsl_matrix_short * m);

_gsl_vector_short_const_view
gsl_matrix_short_const_subdiagonal (const gsl_matrix_short * m,
                                    const size_t k);

_gsl_vector_short_const_view
gsl_matrix_short_const_superdiagonal (const gsl_matrix_short * m,
                                      const size_t k);

_gsl_matrix_short_const_view
gsl_matrix_short_const_view_array (const short * base,
                                   const size_t n1,
                                   const size_t n2);

_gsl_matrix_short_const_view
gsl_matrix_short_const_view_array_with_tda (const short * base,
                                            const size_t n1,
                                            const size_t n2,
                                            const size_t tda);

_gsl_matrix_short_const_view
gsl_matrix_short_const_view_vector (const gsl_vector_short * v,
                                    const size_t n1,
                                    const size_t n2);

_gsl_matrix_short_const_view
gsl_matrix_short_const_view_vector_with_tda (const gsl_vector_short * v,
                                             const size_t n1,
                                             const size_t n2,
                                             const size_t tda);

/* Operations */

short   gsl_matrix_short_get(const gsl_matrix_short * m, const size_t i, const size_t j);
void    gsl_matrix_short_set(gsl_matrix_short * m, const size_t i, const size_t j, const short x);

short * gsl_matrix_short_ptr(gsl_matrix_short * m, const size_t i, const size_t j);
const short * gsl_matrix_short_const_ptr(const gsl_matrix_short * m, const size_t i, const size_t j);

void gsl_matrix_short_set_zero (gsl_matrix_short * m);
void gsl_matrix_short_set_identity (gsl_matrix_short * m);
void gsl_matrix_short_set_all (gsl_matrix_short * m, short x);

int gsl_matrix_short_fread (FILE * stream, gsl_matrix_short * m) ;
int gsl_matrix_short_fwrite (FILE * stream, const gsl_matrix_short * m) ;
int gsl_matrix_short_fscanf (FILE * stream, gsl_matrix_short * m);
int gsl_matrix_short_fprintf (FILE * stream, const gsl_matrix_short * m, const char * format);

int gsl_matrix_short_memcpy(gsl_matrix_short * dest, const gsl_matrix_short * src);
int gsl_matrix_short_swap(gsl_matrix_short * m1, const gsl_matrix_short * m2);

int gsl_matrix_short_swap_rows(gsl_matrix_short * m, const size_t i, const size_t j);
int gsl_matrix_short_swap_columns(gsl_matrix_short * m, const size_t i, const size_t j);
int gsl_matrix_short_swap_rowcol(gsl_matrix_short * m, const size_t i, const size_t j);
int gsl_matrix_short_transpose (gsl_matrix_short * m);
int gsl_matrix_short_transpose_memcpy (gsl_matrix_short * dest, const gsl_matrix_short * src);

short gsl_matrix_short_max (const gsl_matrix_short * m);
short gsl_matrix_short_min (const gsl_matrix_short * m);
void gsl_matrix_short_minmax (const gsl_matrix_short * m, short * min_out, short * max_out);

void gsl_matrix_short_max_index (const gsl_matrix_short * m, size_t * imax, size_t *jmax);
void gsl_matrix_short_min_index (const gsl_matrix_short * m, size_t * imin, size_t *jmin);
void gsl_matrix_short_minmax_index (const gsl_matrix_short * m, size_t * imin, size_t * jmin, size_t * imax, size_t * jmax);

int gsl_matrix_short_isnull (const gsl_matrix_short * m);

int gsl_matrix_short_add (gsl_matrix_short * a, const gsl_matrix_short * b);
int gsl_matrix_short_sub (gsl_matrix_short * a, const gsl_matrix_short * b);
int gsl_matrix_short_mul_elements (gsl_matrix_short * a, const gsl_matrix_short * b);
int gsl_matrix_short_div_elements (gsl_matrix_short * a, const gsl_matrix_short * b);
int gsl_matrix_short_scale (gsl_matrix_short * a, const double x);
int gsl_matrix_short_add_constant (gsl_matrix_short * a, const double x);

/***********************************************************************/
/* The functions below are obsolete                                    */
/***********************************************************************/
int gsl_matrix_short_get_row(gsl_vector_short * v, const gsl_matrix_short * m, const size_t i);
int gsl_matrix_short_get_col(gsl_vector_short * v, const gsl_matrix_short * m, const size_t j);
int gsl_matrix_short_set_row(gsl_matrix_short * m, const size_t i, const gsl_vector_short * v);
int gsl_matrix_short_set_col(gsl_matrix_short * m, const size_t j, const gsl_vector_short * v);

extern int gsl_check_range ;

/* inline functions if you are using GCC */

#ifdef HAVE_INLINE
extern inline
short
gsl_matrix_short_get(const gsl_matrix_short * m, const size_t i, const size_t j)
{
#ifndef GSL_RANGE_CHECK_OFF
  if (i >= m->size1)
    {
      GSL_ERROR_VAL("first index out of range", GSL_EINVAL, 0) ;
    }
  else if (j >= m->size2)
    {
      GSL_ERROR_VAL("second index out of range", GSL_EINVAL, 0) ;
    }
#endif
  return m->data[i * m->tda + j] ;
}

extern inline
void
gsl_matrix_short_set(gsl_matrix_short * m, const size_t i, const size_t j, const short x)
{
#ifndef GSL_RANGE_CHECK_OFF
  if (i >= m->size1)
    {
      GSL_ERROR_VOID("first index out of range", GSL_EINVAL) ;
    }
  else if (j >= m->size2)
    {
      GSL_ERROR_VOID("second index out of range", GSL_EINVAL) ;
    }
#endif
  m->data[i * m->tda + j] = x ;
}

extern inline
short *
gsl_matrix_short_ptr(gsl_matrix_short * m, const size_t i, const size_t j)
{
#ifndef GSL_RANGE_CHECK_OFF
  if (i >= m->size1)
    {
      GSL_ERROR_NULL("first index out of range", GSL_EINVAL) ;
    }
  else if (j >= m->size2)
    {
      GSL_ERROR_NULL("second index out of range", GSL_EINVAL) ;
    }
#endif
  return (short *) (m->data + (i * m->tda + j)) ;
}

extern inline
const short *
gsl_matrix_short_const_ptr(const gsl_matrix_short * m, const size_t i, const size_t j)
{
#ifndef GSL_RANGE_CHECK_OFF
  if (i >= m->size1)
    {
      GSL_ERROR_NULL("first index out of range", GSL_EINVAL) ;
    }
  else if (j >= m->size2)
    {
      GSL_ERROR_NULL("second index out of range", GSL_EINVAL) ;
    }
#endif
  return (const short *) (m->data + (i * m->tda + j)) ;
}

#endif

__END_DECLS

#endif /* __GSL_MATRIX_SHORT_H__ */


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#pragma once

#include "Map.h"

#include "MapPoint.h"
#include "MappingKeyframe.h"
#include "InitializationData.h"

#include "Tracking\KeyframeBuilder.h"

#include "Mapping\MapPointKeyframeAssociations.h"

#include "BoW\OnlineBow.h"
#include "BoW\BaseFeatureMatcher.h"

#include "BundleAdjustment\BundleAdjust.h"

#include "Data\Types.h"
#include "Data\Pose.h"
#include "Data\Data.h"
#include "Data\MapState.h"

#include "Utils\historical_queue.h"
#include "Utils\collection.h"

#include <arcana\threading\blocking_concurrent_queue.h>

#include <gsl\gsl>

#include <shared_mutex>
#include <vector>
#include <set>

// forward-declare the friend class method for accessing the root map in unit tests
namespace UnitTests
{
    class TestHelperFunctions;
}

namespace mage
{
    class ThreadSafeMap
    {
        friend class ::UnitTests::TestHelperFunctions;

    public:
        ThreadSafeMap(
            const MageSlamSettings& settings, BaseBow& bagOfWords);

        ~ThreadSafeMap();

        void InitializeMap(const InitializationData& initializationData, thread_memory memory);

        void GetConnectedMapPoints(
            const collection<Proxy<MapPoint>>& points,
            thread_memory memory,
            loop::vector<KeyframeReprojection>& repro) const;

        // returns a sampling of mappoints
        void GetSampleOfMapPoints(temp::vector<MapPointProxy>& mapPoints, uint32_t max, uint32_t offset = 0) const;

        /*
        Inserts a new keyframe and returns all the keyframes that
        are connected to this one by the covisibility graph.
        */
        Proxy<Keyframe, proxy::Image> InsertKeyframe(
            const std::shared_ptr<const KeyframeBuilder>& kb,
            thread_memory memory);

        /*
        Inserts a new keyframe and returns all the keyframes that
        are connected to this one by the covisibility graph.
        */
        Proxy<Keyframe, proxy::Image> InsertKeyframe(
            const std::shared_ptr<const KeyframeBuilder>& keyframe,
            const gsl::span<MapPointAssociations<MapPointTrackingProxy>::Association>& extraAssociation,
            thread_memory memory);

        std::unique_ptr<KeyframeProxy> GetKeyFrameProxy(const Id<Keyframe>& kId) const;

        /*
        Merge information from a set of posited keyframes into the actual keyframes.
        */
        void UpdateKeyframesFromProxies(
            gsl::span<const KeyframeProxy> proxies,
            const OrbMatcherSettings& mergeSettings,
            std::unordered_map<Id<MapPoint>, MapPointTrackingProxy>& mapPointMerges,
            thread_memory memory);

        /*
        Updates the state of the map points in the KeyframeProxy based on the current
        information in the map.
        */
        void UpdateMapPoints(KeyframeProxy& proxy) const;

        /*
        Builds a global bundle adjust dataset of everything in the map.
        NOTE: Fixes the position of fixed and distance-tethered keyframes to preserve some semblance
        of consistent scale.
        */
        void BuildGlobalBundleAdjustData(AdjustableData& data) const;

        /*
        Retrieves keyframes similar to the image provided.
        */
        void FindSimilarKeyframes(const std::shared_ptr<const AnalyzedImage>& image, std::vector<KeyframeProxy>& output) const;

        void FindNonCovisibleSimilarKeyframeClusters(const KeyframeProxy& proxy, thread_memory memory, std::vector<std::vector<KeyframeProxy>>& output) const;

        /*
        Culls map points based on requirements related to number of keyframes they are visibile in and the results found in tracking
        Takes a list of points that have failed to appear where predicted during tracking to remove as well
        */
        void CullRecentMapPoints(
            const Id<Keyframe>& ki,
            const mira::unique_vector<Id<MapPoint>>& recentFailedMapPoints,
            thread_memory memory);

        /*
        Return the collection of all the map points
        */
        void GetMapPointsAsPositions(std::vector<Position>& positions) const;

        /*
        Return the number of map points in the map
        */
        size_t GetMapPointsCount() const;

        /*
        Return the number of keyframes in the map
        */
        size_t GetKeyframesCount() const;

        /*
        Return the collection of all the keyframes
        */
        void GetKeyframeViewMatrices(std::vector<Matrix>& viewMatrices) const;

        /*
        Creates the map points passed in and returns the keyframes that
        were added to the frames connected to Ki through the covis graph
        */
        std::vector<MappingKeyframe> CreateMapPoints(
            const Id<Keyframe>& keyframeId,
            const gsl::span<const MapPointKeyframeAssociations >& toCreate,
            thread_memory memory);

        /*
        Returns all the map points seen by this keyframe and the ones
        seen by it's neighbours in the covisibility graph. It then returns
        all the keyframes that see those points and aren't in Ki's covis graph.
        */
        unsigned int GetMapPointsAndDistantKeyframes(
            const Id<Keyframe>& Ki,
            unsigned int coVisTheta,
            thread_memory memory,
            std::vector<MapPointTrackingProxy>& mapPoints,
            std::vector<Proxy<Keyframe, proxy::Pose, proxy::Intrinsics, proxy::PoseConstraints>>& keyframes,
            std::vector<MapPointAssociation>& mapPointAssociations,
            std::vector<Id<Keyframe>>& externallyTetheredKeyframes) const;

        /*
        Updates all the map point positions and keyframe poses and
        destroys all the MapPoint outliers (Not sure if we actually destroy them or not).
        */
        void AdjustPosesAndMapPoints(
            const AdjustableData& adjusted,
            gsl::span<const std::pair<Id<MapPoint>, Id<Keyframe>>> outlierAssociations,
            thread_memory memory);

        /*
        Discards the duplicate keyframes that are
        connected to Ki in the covisibility graph.

        Returns the KeyframeIds that were removed as well as the covisability which lead to the removal decision if argument provided
        */
        void CullLocalKeyframes(
            Id<Keyframe> Ki,
            thread_memory memory,
            std::vector<std::pair<const Id<Keyframe>, const std::vector<Id<Keyframe>>>>* cullingDecisions = nullptr);

        /*
        gets keyframes connected to KId in the covisibility graph (more than the minimum required
        map points in common to be entered as a link in the covisibility graph)
        */
        void GetCovisibilityConnectedKeyframes(
            const Id<Keyframe>& kId,
            thread_memory memory,
            std::vector<MappingKeyframe>& kc) const;

        /*
        gets the list of recent points the mapping thread is still carefully evaluating. track local map
        will use this to collect more information on these points for use by the mapping thread, and the
        age of each of these map points
        */
        std::map<Id<MapPoint>, unsigned int> GetRecentlyCreatedMapPoints() const;

        /*
        * Will attempt to match the set of map points into the set of keyframes if matching criterea are met
        */
        void TryConnectMapPoints(gsl::span<const MapPointAssociations<MapPointTrackingProxy>::Association> mapPointAssociations,
            const Id<Keyframe>& insertedKeyframe,
            const TrackLocalMapSettings& trackLocalMapSettings,
            const OrbMatcherSettings& orbMatcherSettings,
            float searchRadius,
            thread_memory memory);

        /*
        Delete the state of the entire map
        */
        void Clear(thread_memory memory);

        /*
        Returns all the information pertinent when saving the map
        */
        MapState GetMapData() const;

        float GetMedianTetherDistance() const;

        float GetMapScale() const;

        /*
        Returns all the keyframes in the map
        */
        std::vector<KeyframeProxy> GetAllKeyframes() const;

        /*
        Destroys this instance of the thread safe map and returns it's
        internal map for use in another context.
        */
        static std::unique_ptr<Map> Release(std::unique_ptr<ThreadSafeMap> tsm);

    private:
        void UnSafeGetCovisibilityConnectedKeyframes(
            const Id<Keyframe>& kId,
            thread_memory memory,
            std::vector<MappingKeyframe>& kc) const;

        // the int value is the number of Keyframes this map point has existed for
        std::map<Id<MapPoint>, unsigned int> UnSafeGetRecentlyCreatedMapPoints() const;

        /*
        Performs all the logic for AdjustPosesAndMapPoints, but does not take its own
        lock and so can be used by other methods as well.
        */
        void UnSafeAdjustPosesAndMapPoints(
            const AdjustableData& adjusted,
            gsl::span<const std::pair<Id<MapPoint>, Id<Keyframe>>> outlierAssociations,
            thread_memory memory);

        // vector to record map point creations for map point culling
        historical_queue<std::vector<Proxy<MapPoint>>, 3> m_pointHistory;

        const KeyframeSettings& m_keyframeSettings;
        const MappingSettings& m_mappingSettings;
        const CovisibilitySettings& m_covisibilitySettings;
        const BundleAdjustSettings& m_bundleAdjustSettings;

        mutable std::shared_mutex m_mutex;

        std::unique_ptr<Map> m_map;

        BaseBow& m_bow;

        float m_mapScale;
    };
}


// The MIT License (MIT)
//
// Copyright (c) 2018 Mateusz Pusz
//
// 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.

// Formatting library for C++ - the core API for char/UTF-8
//
// Copyright (c) 2012 - present, Victor Zverovich
// All rights reserved.
//
// For the license information refer to format.h.

#include <gsl/gsl-lite.hpp>
#include <units/bits/fmt_hacks.h>
#include <concepts>
#include <limits>
#include <string_view>

// most of the below code is based on/copied from libfmt

namespace units::detail {

struct auto_id {};

enum class fmt_align { none, left, right, center };
enum class fmt_sign { none, minus, plus, space };
enum class arg_id_kind { none, index, name };

template<typename Char>
struct fill_t {
private:
  static constexpr size_t max_size = 4 / sizeof(Char);
  // At most one codepoint (so one char32_t or four utf-8 char8_t)
  Char data_[max_size] = {Char{' '}};
  unsigned char size_ = 1;

public:
  constexpr void operator=(std::basic_string_view<Char> s)
  {
    auto size = s.size();
    if (size > max_size) return throw STD_FMT::format_error("invalid fill");
    for (size_t i = 0; i < size; ++i) data_[i] = s[i];
    size_ = static_cast<unsigned char>(size);
  }

  [[nodiscard]] constexpr size_t size() const { return size_; }
  [[nodiscard]] constexpr const Char* data() const { return data_; }

  [[nodiscard]] constexpr Char& operator[](size_t index) { return data_[index]; }
  [[nodiscard]] constexpr const Char& operator[](size_t index) const { return data_[index]; }
};

template<typename T>
inline constexpr bool is_integer = std::is_integral<T>::value && !std::is_same<T, bool>::value &&
                                   !std::is_same<T, char>::value && !std::is_same<T, wchar_t>::value;

template<typename Char>
[[nodiscard]] constexpr bool is_ascii_letter(Char c)
{
  return (c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z');
}

// Converts a character to ASCII. Returns a number > 127 on conversion failure.
template<std::integral Char>
[[nodiscard]] constexpr Char to_ascii(Char value)
{
  return value;
}

template<typename Char>
  requires std::is_enum_v<Char>
[[nodiscard]] constexpr auto to_ascii(Char value) -> std::underlying_type_t<Char> { return value; }

struct width_checker {
  template<typename T>
  [[nodiscard]] constexpr unsigned long long operator()(T value) const
  {
    if constexpr (is_integer<T>) {
      if constexpr (std::numeric_limits<T>::is_signed) {
        if (value < 0) throw STD_FMT::format_error("negative width");
      }
      return static_cast<unsigned long long>(value);
    } else {
      throw STD_FMT::format_error("width is not integer");
    }
  }
};

struct precision_checker {
  template<typename T>
  [[nodiscard]] constexpr unsigned long long operator()(T value) const
  {
    if constexpr (is_integer<T>) {
      if constexpr (std::numeric_limits<T>::is_signed) {
        if (value < 0) throw STD_FMT::format_error("negative precision");
      }
      return static_cast<unsigned long long>(value);
    } else {
      throw STD_FMT::format_error("precision is not integer");
    }
  }
};

// Format specifiers for built-in and string types.
template<typename Char>
struct basic_format_specs {
  int width = 0;
  int precision = -1;
  char type = '\0';
  fmt_align align : 4 = fmt_align::none;
  fmt_sign sign : 3 = fmt_sign::none;
  bool alt : 1 = false;  // Alternate form ('#').
  bool localized : 1 = false;
  fill_t<Char> fill;
};

// Format specifiers with width and precision resolved at formatting rather
// than parsing time to allow re-using the same parsed specifiers with
// different sets of arguments (precompilation of format strings).
template<typename Char>
struct dynamic_format_specs : basic_format_specs<Char> {
  int dynamic_width_index = -1;
  int dynamic_precision_index = -1;
};

[[nodiscard]] constexpr int verify_dynamic_arg_index_in_range(size_t idx)
{
  if (idx > static_cast<size_t>(std::numeric_limits<int>::max())) {
    throw STD_FMT::format_error("Dynamic width or precision index too large.");
  }
  return static_cast<int>(idx);
}

template<typename CharT>
[[nodiscard]] constexpr int on_dynamic_arg(size_t arg_id, STD_FMT::basic_format_parse_context<CharT>& context)
{
  context.check_arg_id(FMT_TO_ARG_ID(arg_id));
  return verify_dynamic_arg_index_in_range(arg_id);
}

template<typename CharT>
[[nodiscard]] constexpr int on_dynamic_arg(auto_id, STD_FMT::basic_format_parse_context<CharT>& context)
{
  return verify_dynamic_arg_index_in_range(FMT_FROM_ARG_ID(context.next_arg_id()));
}

template<class Handler, typename FormatContext>
[[nodiscard]] constexpr int get_dynamic_spec(int index, FormatContext& ctx)
{
  const unsigned long long value =
    STD_FMT::visit_format_arg(Handler{}, ctx.arg(FMT_TO_ARG_ID(static_cast<size_t>(index))));
  if (value > static_cast<unsigned long long>(std::numeric_limits<int>::max())) {
    throw STD_FMT::format_error("number is too big");
  }
  return static_cast<int>(value);
}

// Parses the range [begin, end) as an unsigned integer. This function assumes
// that the range is non-empty and the first character is a digit.
template<std::input_iterator It, std::sentinel_for<It> S>
[[nodiscard]] constexpr It parse_nonnegative_int(It begin, S end, size_t& value)
{
  gsl_Expects(begin != end && '0' <= *begin && *begin <= '9');
  constexpr auto max_int = static_cast<unsigned>(std::numeric_limits<int>::max());
  constexpr auto big_int = max_int / 10u;
  value = 0;

  do {
    if (value > big_int) {
      value = max_int + 1;
      break;
    }
    value = value * 10 + static_cast<unsigned int>(*begin - '0');
    ++begin;
  } while (begin != end && '0' <= *begin && *begin <= '9');

  if (value > max_int) throw STD_FMT::format_error("Number is too big");

  return begin;
}

template<std::input_iterator It, std::sentinel_for<It> S>
[[nodiscard]] constexpr It parse_nonnegative_int(It begin, S end, int& value)
{
  size_t val_unsigned = 0;
  begin = parse_nonnegative_int(begin, end, val_unsigned);
  // Never invalid because parse_nonnegative_integer throws an error for values that don't fit in signed integers
  value = static_cast<int>(val_unsigned);
  return begin;
}

template<std::input_iterator It, std::sentinel_for<It> S, typename IDHandler>
[[nodiscard]] constexpr It do_parse_arg_id(It begin, S end, IDHandler&& handler)
{
  gsl_Expects(begin != end);
  auto c = *begin;
  if (c >= '0' && c <= '9') {
    size_t index = 0;
    if (c != '0')
      begin = parse_nonnegative_int(begin, end, index);
    else
      ++begin;
    if (begin == end || (*begin != '}' && *begin != ':'))
      throw STD_FMT::format_error("invalid format string");
    else
      handler(index);
    return begin;
  }
  throw STD_FMT::format_error("invalid format string");
}

template<std::input_iterator It, std::sentinel_for<It> S, typename IDHandler>
[[nodiscard]] constexpr It parse_arg_id(It begin, S end, IDHandler&& handler)
{
  auto c = *begin;
  if (c != '}' && c != ':') return do_parse_arg_id(begin, end, handler);
  handler();
  return begin;
}

template<std::input_iterator It, std::sentinel_for<It> S, typename Handler>
[[nodiscard]] constexpr It parse_sign(It begin, S end, Handler&& handler)
{
  gsl_Expects(begin != end);
  switch (to_ascii(*begin)) {
    case '+':
      handler.on_sign(fmt_sign::plus);
      ++begin;
      break;
    case '-':
      handler.on_sign(fmt_sign::minus);
      ++begin;
      break;
    case ' ':
      handler.on_sign(fmt_sign::space);
      ++begin;
      break;
    default:
      break;
  }
  return begin;
}

template<std::input_iterator It, std::sentinel_for<It> S, typename Handler>
[[nodiscard]] constexpr It parse_width(It begin, S end, Handler&& handler)
{
  struct width_adapter {
    Handler& handler;
    constexpr void operator()() { handler.on_dynamic_width(auto_id{}); }
    constexpr void operator()(size_t id) { handler.on_dynamic_width(id); }
  };

  gsl_Expects(begin != end);
  if ('0' <= *begin && *begin <= '9') {
    int width = 0;
    begin = parse_nonnegative_int(begin, end, width);
    if (width != -1)
      handler.on_width(width);
    else
      throw STD_FMT::format_error("number is too big");
  } else if (*begin == '{') {
    ++begin;
    if (begin != end) begin = parse_arg_id(begin, end, width_adapter{handler});
    if (begin == end || *begin != '}') throw STD_FMT::format_error("invalid format string");
    ++begin;
  }
  return begin;
}

template<std::input_iterator It, std::sentinel_for<It> S, typename Handler>
[[nodiscard]] constexpr It parse_precision(It begin, S end, Handler&& handler)
{
  struct precision_adapter {
    Handler& handler;
    constexpr void operator()() { handler.on_dynamic_precision(auto_id{}); }
    constexpr void operator()(size_t id) { handler.on_dynamic_precision(id); }
  };

  ++begin;
  auto c = begin != end ? *begin : std::iter_value_t<It>();
  if ('0' <= c && c <= '9') {
    auto precision = 0;
    begin = parse_nonnegative_int(begin, end, precision);
    if (precision != -1)
      handler.on_precision(precision);
    else
      throw STD_FMT::format_error("number is too big");
  } else if (c == '{') {
    ++begin;
    if (begin != end) begin = parse_arg_id(begin, end, precision_adapter{handler});
    if (begin == end || *begin++ != '}') throw STD_FMT::format_error("invalid format string");
  } else {
    throw STD_FMT::format_error("missing precision specifier");
  }
  return begin;
}

template<std::input_iterator It>
constexpr int code_point_length(It begin)
{
  if constexpr (sizeof(std::iter_value_t<It>) != 1) return 1;
  constexpr char lengths[] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
                              0, 0, 0, 0, 0, 0, 0, 0, 2, 2, 2, 2, 3, 3, 4, 0};
  int len = lengths[static_cast<unsigned char>(*begin) >> 3];

  // Compute the pointer to the next character early so that the next
  // iteration can start working on the next character. Neither Clang
  // nor GCC figure out this reordering on their own.
  return len + !len;
}

// Parses fill and alignment.
template<std::input_iterator It, std::sentinel_for<It> S, typename Handler>
[[nodiscard]] constexpr It parse_align(It begin, S end, Handler&& handler)
{
  gsl_Expects(begin != end);
  auto align = fmt_align::none;
  auto p = begin + code_point_length(begin);
  if (p >= end) p = begin;
  for (;;) {
    switch (to_ascii(*p)) {
      case '<':
        align = fmt_align::left;
        break;
      case '>':
        align = fmt_align::right;
        break;
      case '^':
        align = fmt_align::center;
        break;
      default:
        break;
    }
    if (align != fmt_align::none) {
      if (p != begin) {
        auto c = *begin;
        if (c == '{') throw STD_FMT::format_error("invalid fill character '{'");
        handler.on_fill(std::basic_string_view<std::iter_value_t<It>>(begin, static_cast<size_t>(p - begin)));
        begin = p + 1;
      } else
        ++begin;
      handler.on_align(align);
      break;
    } else if (p == begin) {
      break;
    }
    p = begin;
  }
  return begin;
}

// Parses standard format specifiers and sends notifications about parsed
// components to handler.
template<std::input_iterator It, std::sentinel_for<It> S, typename SpecHandler>
[[nodiscard]] constexpr It parse_format_specs(It begin, S end, SpecHandler&& handler)
{
  if (begin + 1 < end && begin[1] == '}' && is_ascii_letter(*begin) && *begin != 'L') {
    handler.on_type(*begin++);
    return begin;
  }

  if (begin == end) return begin;

  begin = ::units::detail::parse_align(begin, end, handler);
  if (begin == end) return begin;

  // Parse sign.
  begin = ::units::detail::parse_sign(begin, end, handler);
  if (begin == end) return begin;

  if (*begin == '#') {
    handler.on_hash();
    if (++begin == end) return begin;
  }

  // Parse zero flag.
  if (*begin == '0') {
    handler.on_zero();
    if (++begin == end) return begin;
  }

  begin = ::units::detail::parse_width(begin, end, handler);
  if (begin == end) return begin;

  // Parse precision.
  if (*begin == '.') {
    begin = ::units::detail::parse_precision(begin, end, handler);
    if (begin == end) return begin;
  }

  if (*begin == 'L') {
    handler.on_localized();
    ++begin;
  }

  // Parse type.
  if (begin != end && *begin != '}') handler.on_type(*begin++);
  return begin;
}

// A format specifier handler that sets fields in basic_format_specs.
template<typename Char>
class specs_setter {
protected:
  basic_format_specs<Char>& specs_;
public:
  constexpr explicit specs_setter(basic_format_specs<Char>& specs) : specs_(specs) {}
  constexpr void on_align(fmt_align align) { specs_.align = align; }
  constexpr void on_fill(std::basic_string_view<Char> fill) { specs_.fill = fill; }
  constexpr void on_sign(fmt_sign s) { specs_.sign = s; }
  constexpr void on_hash() { specs_.alt = true; }
  constexpr void on_localized() { specs_.localized = true; }
  constexpr void on_zero() { specs_.fill[0] = Char('0'); }
  constexpr void on_width(int width) { specs_.width = width; }
  constexpr void on_precision(int precision) { specs_.precision = precision; }
  constexpr void on_type(Char type) { specs_.type = static_cast<char>(type); }
};

// Format spec handler that saves references to arguments representing dynamic
// width and precision to be resolved at formatting time.
template<typename ParseContext>
class dynamic_specs_handler : public specs_setter<typename ParseContext::char_type> {
public:
  using char_type = TYPENAME ParseContext::char_type;

  constexpr dynamic_specs_handler(dynamic_format_specs<char_type>& specs, ParseContext& ctx) :
      specs_setter<char_type>(specs), specs_(specs), context_(ctx)
  {}

  template<typename T>
  constexpr void on_dynamic_width(T t)
  {
    specs_.dynamic_width_index = on_dynamic_arg(t, context_);
  }

  template<typename T>
  constexpr void on_dynamic_precision(T t)
  {
    specs_.dynamic_precision_index = on_dynamic_arg(t, context_);
  }
private:
  dynamic_format_specs<char_type>& specs_;
  ParseContext& context_;
};

}  // namespace units::detail


/**
 */
#ifndef _INOUT_APER_H

#define _INOUT_APER_H

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <math.h>
#include <gsl/gsl_vector.h>
#include "aXe_grism.h"
#include "aXe_utils.h"
#include "aXe_errors.h"
#include "spc_cfg.h"
#include "spc_trace_functions.h"

#define APER_MAXLINE 14

extern int
nbeams_from_char_array2 (char **apers, int num);

extern gsl_vector_int *
nbeams_from_char_array (char **apers, int num);

extern int
object_list_to_file (object * const *oblist, char *filename,
                     int leaveout_ignored);

extern int
get_beam_from_aper_file (char *filename, int aperID, int beamID,beam * b);

extern gsl_vector_int *
aper_file_aperlist (char *filename);

extern int
aper_file_apernum (char *filename);

extern object *
get_aperture_from_aper_file (char *filename, int aperID);

extern object **
file_to_object_list (char filename[], observation * obs);

extern char **
return_next_aperture(FILE *input);

extern object **
file_to_object_list_seq (char filename[], observation * obs);

extern int
find_object_in_object_list(object **oblist, const int ID);

extern beam
find_beam_in_object_list(object **oblist, const int objID,
                         const int beamID);

extern beam *
find_beamptr_in_object_list(object **oblist, const int objID,
                            const int beamID);

extern void
refurbish_object_list(object **oblist, const int new_default,
                      const int old_value, const int new_value);

extern int
object_list_size(object **oblist);

extern int
get_beamspec_size(object **oblist);
#endif


#pragma once

#include <gsl\gsl>
#include <winrt\Windows.Foundation.h>
#include <d3d11.h>
#include <map>
#include "DrawableGameComponent.h"
#include "FullScreenRenderTarget.h"
#include "FullScreenQuad.h"
#include "MatrixHelper.h"

namespace Library
{
	class PixelShader;
}

namespace Rendering
{
	class DiffuseLightingDemo;

	enum class ColorFilters
	{
		GrayScale = 0,
		Inverse,
		Sepia,
		Generic,
		End
	};

	class ColorFilteringDemo final : public Library::DrawableGameComponent
	{
	public:
		ColorFilteringDemo(Library::Game& game, const std::shared_ptr<Library::Camera>& camera);
		ColorFilteringDemo(const ColorFilteringDemo&) = delete;
		ColorFilteringDemo(ColorFilteringDemo&&) = default;
		ColorFilteringDemo& operator=(const ColorFilteringDemo&) = default;
		ColorFilteringDemo& operator=(ColorFilteringDemo&&) = default;
		~ColorFilteringDemo();

		std::shared_ptr<DiffuseLightingDemo> DiffuseLighting() const;

		ColorFilters ActiveColorFilter() const;
		void SetActiveColorFilter(ColorFilters colorFilter);

		float GenericFilterBrightness() const;
		void SetGenericFilterBrightness(float brightness);

		static const std::map<ColorFilters, std::string> ColorFilterNames;

		virtual void Initialize() override;
		virtual void Update(const Library::GameTime& gameTime) override;
		virtual void Draw(const Library::GameTime& gameTime) override;

	private:
		struct GenericColorFilterPSConstantBuffer
		{
			DirectX::XMFLOAT4X4 ColorFilter{ Library::MatrixHelper::Identity };
		};

		std::shared_ptr<DiffuseLightingDemo> mDiffuseLightingDemo;
		Library::FullScreenRenderTarget mRenderTarget;
		Library::FullScreenQuad mFullScreenQuad;
		std::map<ColorFilters, std::shared_ptr<Library::PixelShader>> mPixelShadersByColorFilter;
		ColorFilters mActiveColorFilter;
		winrt::com_ptr<ID3D11Buffer> mGenericColorFilterPSConstantBuffer;
		GenericColorFilterPSConstantBuffer mGenericColorFilterPSConstantBufferData;
	};
}


#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sum.h>

gsl_sum_levin_u_workspace *
gsl_sum_levin_u_alloc (size_t n)
{
  gsl_sum_levin_u_workspace * w;

  if (n == 0)
    {
      GSL_ERROR_VAL ("length n must be positive integer", GSL_EDOM, 0);
    }

  w = (gsl_sum_levin_u_workspace *) malloc(sizeof(gsl_sum_levin_u_workspace));

  if (w == NULL)
    {
      GSL_ERROR_VAL ("failed to allocate struct", GSL_ENOMEM, 0);
    }

  w->q_num = (double *) malloc (n * sizeof (double));

  if (w->q_num == NULL)
    {
      free(w) ; /* error in constructor, prevent memory leak */

      GSL_ERROR_VAL ("failed to allocate space for q_num", GSL_ENOMEM, 0);
    }

  w->q_den = (double *) malloc (n * sizeof (double));

  if (w->q_den == NULL)
    {
      free (w->q_num);
      free (w) ; /* error in constructor, prevent memory leak */

      GSL_ERROR_VAL ("failed to allocate space for q_den", GSL_ENOMEM, 0);
    }

  w->dq_num = (double *) malloc (n * n * sizeof (double));

  if (w->dq_num == NULL)
    {
      free (w->q_den);
      free (w->q_num);
      free(w) ; /* error in constructor, prevent memory leak */

      GSL_ERROR_VAL ("failed to allocate space for dq_num", GSL_ENOMEM, 0);
    }

  w->dq_den = (double *) malloc (n * n * sizeof (double));

  if (w->dq_den == NULL)
    {
      free (w->dq_num);
      free (w->q_den);
      free (w->q_num);
      free (w) ; /* error in constructor, prevent memory leak */

      GSL_ERROR_VAL ("failed to allocate space for dq_den", GSL_ENOMEM, 0);
    }

  w->dsum = (double *) malloc (n * sizeof (double));

  if (w->dsum == NULL)
    {
      free (w->dq_den);
      free (w->dq_num);
      free (w->q_den);
      free (w->q_num);
      free (w) ; /* error in constructor, prevent memory leak */

      GSL_ERROR_VAL ("failed to allocate space for dsum", GSL_ENOMEM, 0);
    }

  w->size = n;
  w->terms_used = 0;
  w->sum_plain = 0;

  return w;
}

void
gsl_sum_levin_u_free (gsl_sum_levin_u_workspace * w)
{
  free (w->dsum);
  free (w->dq_den);
  free (w->dq_num);
  free (w->q_den);
  free (w->q_num);
  free (w);
}


#ifndef PRECISION_RECALL_H_G2FJDYVV
#define PRECISION_RECALL_H_G2FJDYVV

#include <gsl/gsl>
#include <opencv2/core/base.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/imgproc.hpp>
#include <optional>
#include <string_view>
#include <util/common_structures.h>

namespace sens_loc::apps {

/// Defines the color and line strength used for backprojection.
struct backproject_style {
    backproject_style() = default;
    backproject_style(int r, int g, int b, int strength)
        : color{CV_RGB(r, g, b)}
        , strength{strength} {
        Expects(strength > 0);
        Expects(r >= 0);
        Expects(g >= 0);
        Expects(b >= 0);
        Expects(r < 256);
        Expects(g < 256);
        Expects(b < 256);
    }

    cv::Scalar color    = CV_RGB(0, 0, 0);
    int        strength = 1;
};

/// Composition of styles on how the backprojections are plotted.
/// \sa backproject_style
struct backproject_config {
    backproject_style true_positive;
    backproject_style false_negative;
    backproject_style false_positive;
};

struct recognition_analysis_input {
    std::string_view                depth_image_pattern;
    std::string_view                pose_file_pattern;
    std::string_view                intrinsic_file;
    std::optional<std::string_view> mask_file;
    cv::NormTypes                   matching_norm;
    float                           keypoint_distance_threshold;

    /// Unit-Conversion for the ICP of kinect images. No other depth images
    /// are treated with ICP, so this is dirty set to a constant.
    const float unit_factor = 0.001F;
};

struct recognition_analysis_output_options {
    std::optional<std::string> backproject_pattern;
    std::optional<std::string> original_files;
    std::optional<std::string> stat_file;
    std::optional<std::string> backprojection_selected_histo;
    std::optional<std::string> relevant_histo;
    std::optional<std::string> true_positive_histo;
    std::optional<std::string> false_positive_histo;
    std::optional<std::string> true_positive_distance_histo;
    std::optional<std::string> false_positive_distance_histo;
};

int analyze_recognition_performance(
    util::processing_input                     in,
    const recognition_analysis_input&          required_data,
    const recognition_analysis_output_options& output_options,
    const backproject_config&                  backproject_config);

}  // namespace sens_loc::apps

#endif /* end of include guard: PRECISION_RECALL_H_G2FJDYVV */


#pragma once

#include "datastructures/PointBuffer.h"
#include "io/PointsPersistence.h"
#include "math/AABB.h"
#include "point_source/PointSource.h"
#include "pointcloud/FileStats.h"
#include "pointcloud/PointAttributes.h"
#include "tiling/Sampling.h"
#include "util/Definitions.h"
#include "util/Transformation.h"
#include <reflection/StaticReflection.h>
#include <threading/Semaphore.h>
#include <threading/TaskSystem.h>

#include <atomic>
#include <deque>
#include <gsl/gsl>
#include <string>
#include <thread>

#include <taskflow/taskflow.hpp>

struct ProgressReporter;
struct TilingAlgorithmBase;
struct ThroughputSampler;

/**
 * Which tiling strategy to use?
 */
enum class TilingStrategy
{
  /**
   * Use the accurate strategy that samples from the root node. This will be
   * slower than the 'Fast' strategy, that skips the first couple of levels and
   * reconstructs them afterwards
   */
  Accurate,
  /**
   * Use the fast strategy that skips the first couple of levels and starts
   * tiling deeper in the octree to enable increased parallelism. The skipped
   * levels are reconstructed and thus contain duplicated data
   */
  Fast
};

struct FixedThreadCount
{
  uint32_t num_threads_for_reading;
  uint32_t num_threads_for_indexing;
};

struct AdaptiveThreadCount
{
  uint32_t num_threads;
};

/**
 * Thread number configuration. Either a fixed number of read and index threads, or
 * a total fixed number of threads that are scheduled adaptively
 */
using ThreadConfig = std::variant<FixedThreadCount, AdaptiveThreadCount>;

struct TilerMetaParameters
{
  float spacing_at_root;
  uint32_t max_depth;
  size_t max_points_per_node;
  size_t batch_read_size;
  size_t internal_cache_size;
  bool shift_points_to_origin;
  bool create_journal;
  TilingStrategy tiling_strategy;
  std::variant<FixedThreadCount, AdaptiveThreadCount> thread_count;
};

/**
 * Abstract command for reading points from a file.
 */
struct ReadCommand
{
  const fs::path* file_path;
  size_t to_read_count;
};

struct Tiler
{
  Tiler(DatasetMetadata dataset_metadata,
        TilerMetaParameters meta_parameters,
        SamplingStrategy sampling_strategy,
        ProgressReporter* progress_reporter,
        MultiReaderPointSource point_source,
        PointsPersistence& persistence,
        const PointAttributes& input_attributes,
        fs::path output_directory);
  ~Tiler();

  /**
   * Run the tiler. Returns the total number of points that were processed
   */
  size_t run();

private:
  void swap_point_buffers(size_t produced_points_count);

  bool build_execution_graph_for_reading(tf::Taskflow& tf,
                                         uint32_t num_read_threads,
                                         ThroughputSampler& throughput_sampler);
  void execute_read_commands(const std::vector<ReadCommand>& read_commands,
                             util::Range<PointBuffer::PointIterator> read_destination);

  void build_execution_graph_for_indexing(tf::Taskflow& tf,
                                          uint32_t num_indexing_threads,
                                          ThroughputSampler& throughput_sampler);

  void create_read_commands();
  void adjust_read_thread_count(size_t num_read_threads);
  uint32_t max_read_parallelism() const;

  void estimate_read_throughput(ThroughputSampler& sampler, size_t num_points_in_last_cycle) const;
  void estimate_index_throughput(ThroughputSampler& sampler, size_t num_points_in_last_cycle) const;

  DatasetMetadata _dataset_metadata;
  TilerMetaParameters _meta_parameters;
  SamplingStrategy _sampling_strategy;
  ProgressReporter* _progress_reporter;
  MultiReaderPointSource _point_source;
  PointsPersistence& _persistence;

  AABB _bounds;

  const PointAttributes& _input_attributes;
  fs::path _output_directory;

  PointBuffer _points_cache_for_producers, _points_cache_for_consumers;
  size_t _produced_points_count;

  std::deque<ReadCommand> _remaining_read_commands;
  std::vector<ReadCommand> _next_read_commands_per_thread;

  std::unique_ptr<TilingAlgorithmBase> _tiling_algorithm;

  Semaphore _producers, _consumers;

  std::chrono::high_resolution_clock::time_point _begin_read_cycle_time;
  std::chrono::high_resolution_clock::time_point _begin_index_cycle_time;
};


/* Author:  G. Jungman */

/* Convenience header */
#ifndef __GSL_SPECFUNC_H__
#define __GSL_SPECFUNC_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif

#include <gsl/gsl_sf.h>

#endif /* __GSL_SPECFUNC_H__ */


#ifndef __SAMPLE_H__
#define __SAMPLE_H__

#include <gsl/gsl_rng.h>
#include <pthread.h>
#include <stdint.h>

#include "parse_mmaps.h"

#define NUM_USER_LOCKS 2000

struct sample_thread_info;
struct index_update;
struct sample_threads;

/* Thread utility functions */

void initialize_threads(struct sample_threads* sample_threads, int num_threads,
			const struct mmap_info* mmap_info);
void destroy_threads(struct sample_threads* sample_threads);

/* Resampling functions */

/* Set all topic and POV assignments to -1, not doing any index
   updates. Used before initializing topics and POVs.*/
void resample_null(struct sample_threads* sample_threads);
/* Initialize topic and POV assignments by sampling them uniformly at
   random and performing index updates. */
void resample_uniform(struct sample_threads* sample_threads);

/* Re-sample topics and POVs. One iteration of Gibbs sampling. */
void resample(struct sample_threads* sample_threads);
/* Re-sample assignments, but (approximately) in the opposite order as
   resample(). Used during model selection. */
void resample_reverse(struct sample_threads* sample_threads);

/* Rather than re-sampling randomly, always choose the maximum
   probability assignment. Useful for finding a high probability
   assignment of topics and POVs after random sampling. */
void resample_maximize(struct sample_threads* sample_threads);
/* Restore a set of assignments, updating indexes as necessary */
void resample_restore(struct sample_threads* sample_threads,
		      const struct revision_assignment* revision_assignments);
/* Set all topic and POV assignments to -1, subtracting the
   assignments from indexes. Primarily useful for verifying that
   indexes were correct. */
void resample_zero(struct sample_threads* sample_threads);

/* Parallel probability computations */

/* Compute the log probability of transitioning to the specified
   assignments from the current assignments (don't actually transition
   to them). */
double transition_probability(struct sample_threads* sample_threads,
			      const struct revision_assignment* revision_assignments);
/* Compute the likelihood of the current model and topic/POV
   assignments */
double parallel_log_likelihood(struct sample_threads* sample_threads);

/* Other miscellaneous parallel utility routines */

/* Set all of the user topic/POV distributions to alpha. Used during
   initialization. */
void parallel_initialize_user_topics(struct sample_threads* sample_threads);

/* Structs to hold synchronization and thread information. */

struct sample_thread_info {
  /* Each thread gets its own random number generator, initialized
     with a different seed. */
  gsl_rng* rand_gen;

  /* A num_topics * num_povs_per_topic array which threads use to
     sample new topic and POV assignments. */
  double* sampling_array;

  /* Each thread gets its own copy of mmap_info, allowing us to
     replace topic_index_mmap with a private copy which is manually
     synchronized. */
  struct mmap_info mmap_info;

  // Sample only pages numbered (page# % mod_n) == sample_pages
  int sample_pages;
  int mod_n;

  // Information about the current thread.
  pthread_t thread;

  /* An array of locks to synchronize access and updates to user
     topic/POV distributions. For a given user's topic and POV
     distribution, the lock to use is userid % NUM_USER_LOCKS. */
  pthread_rwlock_t* user_locks;

  /* Memory accounting for the topic_index_mmap copy, if any. One
     thread updates the original, and so does not have a specially
     allocated topic index, in which case allocated_topic_dist will be
     NULL. */
  char* allocated_topic_dist;

  /* The index of the first position in the topic index update queue
     that we have not read. */
  int64_t last_queue_position;

  /* Pointers to the global topic index update queue, and the global
     position of the first empty slot in the queue. Access to the
     queue location is synchronized with queue_lock. */
  struct index_update* index_update_queue;
  int64_t* queue_location;
  pthread_mutex_t* queue_lock;

  /* Generally +1 or -1, indicating whether sampling is forward or
     backward. */
  int increment;

  /* Helper functions to specify sampling behavior, replacing small or
     large parts of the sampling routine. */
  void (*revision_callback) (struct sample_thread_info*, int64_t);
  void (*index_update_function) (const struct mmap_info* mmap_info,
				 const struct index_update* patch);
  void (*sample_function) (const double* sampling_array,
			   double probability_sum,
			   int num_topics, int pov_per_topic,
			   int* chosen_topic, int* chosen_pov,
			   gsl_rng* rand_gen);

  /* When computing transition probabilities, these assignments
     specify which assignments the transition is to. */
  const struct revision_assignment* reference_assignments;

  double output;
};

struct sample_threads {
  struct sample_thread_info* thread_info;
  pthread_mutex_t queue_lock;
  pthread_rwlock_t user_locks[NUM_USER_LOCKS];
  int num_threads;

  struct index_update* index_update_queue;
  // One past the last value in the queue
  int64_t queue_location;
};

#endif


//
//  editor.hpp
//  PretendToWork
//
//  Created by tqtifnypmb on 14/12/2017.
//  Copyright © 2017 tqtifnypmb. All rights reserved.
//

#pragma once

#include "../crdt/Engine.h"
#include "../types.h"

#include <memory>
#include <gsl/gsl>
#include <map>

namespace brick
{

class Rope;
class Editor {
public:

    using DeltaList = std::vector<std::tuple<Revision, size_t, size_t>>;     // [(delta, begRow, endRow)]
    using SyncCb = std::function<void(const Editor::DeltaList& deltaList)>;

    Editor(size_t viewId, const detail::CodePointList& cplist, SyncCb syncCb);
    Editor(size_t viewId, SyncCb syncCb);

    template <class Converter>
    void insert(gsl::span<const char> bytes, size_t pos);
    void insert(const detail::CodePointList& cplist, size_t pos);
    void erase(Range range);
    void undo();

    void sync(Editor& editor);

    std::map<size_t, detail::CodePointList> region(size_t begLine, size_t endLine);

    void clearRevisions() {
        engine_.revisions().clear();
    }

private:
    Editor::DeltaList convertEngineDelta(const Engine::DeltaList& deltas);

    void updateLines(size_t pos, const detail::CodePointList& cplist);
    void updateLines(Range r);

    std::unique_ptr<Rope> rope_;
    Engine engine_;
    std::vector<size_t> linesIndex_;
    SyncCb sync_cb_;
};

template <class Converter>
void Editor::insert(gsl::span<const char> bytes, size_t pos) {
    insert(Converter::encode(bytes), pos);
}

}   // namespace brick


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

#ifndef _MCAS_COMMON_BYTE_SPAN_
#define _MCAS_COMMON_BYTE_SPAN_

#include <common/pointer_cast.h>
#include <cstddef>
#include <gsl/gsl_byte>
#include <gsl/span>
#include <sys/uio.h>

#ifndef MCAS_SPAN_USES_GSL
/* For minimal difference with older code; implement span as iovec (not gsl::span) */
#define MCAS_SPAN_USES_GSL 0
#endif
#ifndef MCAS_BYTE_USES_STD
/* For compilcation with C++14, use gsl::byte, not C++17 std::byte */
#define MCAS_BYTE_USES_STD 0
#endif

namespace common
{
#if MCAS_BYTE_USES_STD
	using byte = std::byte;
#else
	using byte = gsl::byte; /* can be std::byte in C++17 */
#endif
	template <typename T> using span = gsl::span<T>; /* can be std::span in C++20 */
	/* span of a const area. No equivalent in ::iovec, so always use span */
	using const_byte_span = span<const byte>;
	inline const_byte_span make_const_byte_span(const void *base, std::size_t len)
    {
      return const_byte_span(static_cast<const_byte_span::pointer>(base), len);
    }
}

namespace
{
	/* Accessors: Non-member in order to match ::iovec accessors */
	/* Start of area as a void *, for use with %p format and for conversion to an arbitrary type */
	inline const void *base(const common::const_byte_span &r) { return r.data(); }
    /* Length of area in bytes, for use in byte-based address calculations and comparisons */
	inline std::size_t size(const common::const_byte_span &r) { return r.size(); }
	/* Start of area as byte *, for use in byte-based address calculations and comparisons */
	inline const common::byte *data(const common::const_byte_span &r) { return r.data(); }
	/* End of area as byte *, for use in byte-based address calculations and comparisons */
	inline const common::byte *data_end(const common::const_byte_span &r) { return r.data() + r.size(); }
	/* End of are as a void *, for use with %p format */
	inline const void *end(const common::const_byte_span &r) { return ::data_end(r); }
}

namespace
{
	/* Same accessors as above, for ::iovec */
	inline void *base(const ::iovec &r) { return r.iov_base; }
	inline std::size_t size(const ::iovec &r) { return r.iov_len; }
	inline common::byte *data(const ::iovec &r) { return static_cast<common::byte *>(::base(r)); }
	inline common::byte *data_end(const ::iovec &r) { return ::data(r) + ::size(r); }
	inline void *end(const ::iovec &r) { return ::data_end(r); }
}

namespace common
{
	inline constexpr ::iovec make_iovec(void *base, std::size_t len) { return ::iovec{base, len}; }
}

#if MCAS_SPAN_USES_GSL
#include <common/pointer_cast.h>
namespace common
{
	using byte_span = span<byte>;
}

namespace
{
	/* Same accessors as above, for non-const byte span */
	inline void *base(const common::byte_span &r) { return r.data(); }
	inline std::size_t size(const common::byte_span &r) { return r.size(); }
	inline common::byte *data(const common::byte_span &r) { return r.data(); }
	inline common::byte *data_end(const common::byte_span &r) { return ::data(r) + ::size(r); }
	inline void *end(const common::byte_span &r) { return ::data_end(r); }
}

namespace common
{
	/* Construct a byte_span in syntax compatible with iovec (function, not constructor) */
	inline byte_span make_byte_span(void *base, std::size_t len) { return byte_span(common::pointer_cast<byte_span::value_type>(base), len); }
}
#else /* ! MCAS_SPAN_USES_GSL */
namespace common
{
	using byte_span = ::iovec;
	/* Construct an iovec in syntax compatible with span (no braces) */
	inline byte_span make_byte_span(void *base, std::size_t len) { return byte_span{base, len}; }
}
#endif

namespace common
{
	/* Converion from span of non-const to span of const */
	inline const_byte_span make_const_byte_span(const byte_span s) { return const_byte_span(::data(s), ::size(s)); }
}

#endif


/* vector/gsl_vector_long_double.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#ifndef __GSL_VECTOR_LONG_DOUBLE_H__
#define __GSL_VECTOR_LONG_DOUBLE_H__

#include <stdlib.h>
#include <gsl/gsl_types.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_check_range.h>
#include <gsl/gsl_block_long_double.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct
{
  size_t size;
  size_t stride;
  long double *data;
  gsl_block_long_double *block;
  int owner;
}
gsl_vector_long_double;

typedef struct
{
  gsl_vector_long_double vector;
} _gsl_vector_long_double_view;

typedef _gsl_vector_long_double_view gsl_vector_long_double_view;

typedef struct
{
  gsl_vector_long_double vector;
} _gsl_vector_long_double_const_view;

typedef const _gsl_vector_long_double_const_view gsl_vector_long_double_const_view;

/* Allocation */

GSL_EXPORT gsl_vector_long_double *gsl_vector_long_double_alloc (const size_t n);
GSL_EXPORT gsl_vector_long_double *gsl_vector_long_double_calloc (const size_t n);

GSL_EXPORT gsl_vector_long_double *gsl_vector_long_double_alloc_from_block (gsl_block_long_double * b,
                                                                           const size_t offset,
                                                                           const size_t n,
                                                                           const size_t stride);

GSL_EXPORT gsl_vector_long_double *gsl_vector_long_double_alloc_from_vector (gsl_vector_long_double * v,
                                                                            const size_t offset,
                                                                            const size_t n,
                                                                            const size_t stride);

GSL_EXPORT void gsl_vector_long_double_free (gsl_vector_long_double * v);

/* Views */

GSL_EXPORT
_gsl_vector_long_double_view
gsl_vector_long_double_view_array (long double *v, size_t n);

GSL_EXPORT
_gsl_vector_long_double_view
gsl_vector_long_double_view_array_with_stride (long double *base,
                                         size_t stride,
                                         size_t n);

GSL_EXPORT
_gsl_vector_long_double_const_view
gsl_vector_long_double_const_view_array (const long double *v, size_t n);

GSL_EXPORT
_gsl_vector_long_double_const_view
gsl_vector_long_double_const_view_array_with_stride (const long double *base,
                                               size_t stride,
                                               size_t n);

GSL_EXPORT
_gsl_vector_long_double_view
gsl_vector_long_double_subvector (gsl_vector_long_double *v,
                            size_t i,
                            size_t n);

GSL_EXPORT
_gsl_vector_long_double_view
gsl_vector_long_double_subvector_with_stride (gsl_vector_long_double *v,
                                        size_t i,
                                        size_t stride,
                                        size_t n);

GSL_EXPORT
_gsl_vector_long_double_const_view
gsl_vector_long_double_const_subvector (const gsl_vector_long_double *v,
                                  size_t i,
                                  size_t n);

GSL_EXPORT
_gsl_vector_long_double_const_view
gsl_vector_long_double_const_subvector_with_stride (const gsl_vector_long_double *v,
                                              size_t i,
                                              size_t stride,
                                              size_t n);

/* Operations */

GSL_EXPORT long double gsl_vector_long_double_get (const gsl_vector_long_double * v, const size_t i);
GSL_EXPORT void gsl_vector_long_double_set (gsl_vector_long_double * v, const size_t i, long double x);

GSL_EXPORT long double *gsl_vector_long_double_ptr (gsl_vector_long_double * v, const size_t i);
GSL_EXPORT const long double *gsl_vector_long_double_const_ptr (const gsl_vector_long_double * v, const size_t i);

GSL_EXPORT void gsl_vector_long_double_set_zero (gsl_vector_long_double * v);
GSL_EXPORT void gsl_vector_long_double_set_all (gsl_vector_long_double * v, long double x);
GSL_EXPORT int gsl_vector_long_double_set_basis (gsl_vector_long_double * v, size_t i);

GSL_EXPORT int gsl_vector_long_double_fread (FILE * stream, gsl_vector_long_double * v);
GSL_EXPORT int gsl_vector_long_double_fwrite (FILE * stream, const gsl_vector_long_double * v);
GSL_EXPORT int gsl_vector_long_double_fscanf (FILE * stream, gsl_vector_long_double * v);
GSL_EXPORT int gsl_vector_long_double_fprintf (FILE * stream, const gsl_vector_long_double * v,
                                               const char *format);

GSL_EXPORT int gsl_vector_long_double_memcpy (gsl_vector_long_double * dest, const gsl_vector_long_double * src);

GSL_EXPORT int gsl_vector_long_double_reverse (gsl_vector_long_double * v);

GSL_EXPORT int gsl_vector_long_double_swap (gsl_vector_long_double * v, gsl_vector_long_double * w);
GSL_EXPORT int gsl_vector_long_double_swap_elements (gsl_vector_long_double * v, const size_t i, const size_t j);

GSL_EXPORT long double gsl_vector_long_double_max (const gsl_vector_long_double * v);
GSL_EXPORT long double gsl_vector_long_double_min (const gsl_vector_long_double * v);
GSL_EXPORT void gsl_vector_long_double_minmax (const gsl_vector_long_double * v, long double * min_out, long double * max_out);

GSL_EXPORT size_t gsl_vector_long_double_max_index (const gsl_vector_long_double * v);
GSL_EXPORT size_t gsl_vector_long_double_min_index (const gsl_vector_long_double * v);
GSL_EXPORT void gsl_vector_long_double_minmax_index (const gsl_vector_long_double * v, size_t * imin, size_t * imax);

GSL_EXPORT int gsl_vector_long_double_add (gsl_vector_long_double * a, const gsl_vector_long_double * b);
GSL_EXPORT int gsl_vector_long_double_sub (gsl_vector_long_double * a, const gsl_vector_long_double * b);
GSL_EXPORT int gsl_vector_long_double_mul (gsl_vector_long_double * a, const gsl_vector_long_double * b);
GSL_EXPORT int gsl_vector_long_double_div (gsl_vector_long_double * a, const gsl_vector_long_double * b);
GSL_EXPORT int gsl_vector_long_double_scale (gsl_vector_long_double * a, const double x);
GSL_EXPORT int gsl_vector_long_double_add_constant (gsl_vector_long_double * a, const double x);

GSL_EXPORT int gsl_vector_long_double_isnull (const gsl_vector_long_double * v);

#ifdef HAVE_INLINE

extern inline
long double
gsl_vector_long_double_get (const gsl_vector_long_double * v, const size_t i)
{
#if GSL_RANGE_CHECK
  if (i >= v->size)
    {
      GSL_ERROR_VAL ("index out of range", GSL_EINVAL, 0);
    }
#endif
  return v->data[i * v->stride];
}

extern inline
void
gsl_vector_long_double_set (gsl_vector_long_double * v, const size_t i, long double x)
{
#if GSL_RANGE_CHECK
  if (i >= v->size)
    {
      GSL_ERROR_VOID ("index out of range", GSL_EINVAL);
    }
#endif
  v->data[i * v->stride] = x;
}

extern inline
long double *
gsl_vector_long_double_ptr (gsl_vector_long_double * v, const size_t i)
{
#if GSL_RANGE_CHECK
  if (i >= v->size)
    {
      GSL_ERROR_NULL ("index out of range", GSL_EINVAL);
    }
#endif
  return (long double *) (v->data + i * v->stride);
}

extern inline
const long double *
gsl_vector_long_double_const_ptr (const gsl_vector_long_double * v, const size_t i)
{
#if GSL_RANGE_CHECK
  if (i >= v->size)
    {
      GSL_ERROR_NULL ("index out of range", GSL_EINVAL);
    }
#endif
  return (const long double *) (v->data + i * v->stride);
}

#endif /* HAVE_INLINE */

__END_DECLS

#endif /* __GSL_VECTOR_LONG_DOUBLE_H__ */