#include <fstream>
#include <iostream>
#include <filesystem>
#include <string>
#include <unordered_map>
#include <algorithm>
#include <chrono>
#include <future>
#include <utility>
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <xtensor/xarray.hpp>
#include <xtensor/xmath.hpp>
#include "csv.hpp"
#include "../assembly/assembly.cpp"
#include "../probability/probability.cpp"
#include "../probability/probability_util.cpp"
#include "../domain/domain.cpp"
#include "./context.cpp"

using namespace std;
using namespace std::chrono;
using namespace csv;

bool task_compute_domain_probabilities_per_assembly(
	const int task_nb,
	const DomainProbabilityContext ctx,
	const vector<string>& assembly_ids
) {
	auto start = system_clock::now();

	cerr << "Thread " << task_nb << " started." << endl;

	const string kind = ctx.kind;
	const string query = ctx.query;
	const string tail = ctx.tail;

	string dataFolder = "../data/";
	string sequencesFolder = dataFolder + "sequences/";
	auto n_assemblies = assembly_ids.size();

	string metadata_path = dataFolder + query + "_master.csv";
	auto metadata = LoadDomainMetadata(metadata_path);

	int i = 0;
	for (auto& accession : assembly_ids) {
		if (i == 0 || (i+1) % 100 == 0) {
			auto tp = system_clock::now();
			auto elapsed = duration_cast<seconds>(tp - start).count();
			cerr << "Thread " << task_nb << ": ";
			cerr << "Processing assembly " << i + 1 << " / " << n_assemblies;
			cerr << " (elapsed: " << elapsed << " seconds)" << endl;
		}
		++i;

		string gene_probs_path = (
			sequencesFolder + accession + "/" +
			accession + ctx.distance_to_mean_suffix
		);

		try {
			ifstream gene_probs_file(gene_probs_path);
			GeneProbabilies gene_probs(gene_probs_file, tail);

			string protein_domains_path = (
				sequencesFolder + accession + "/" +
				accession + "_" + query + ".csv.gz"
			);
			ifstream protein_domains_file(protein_domains_path);
			boost::iostreams::filtering_streambuf<boost::iostreams::input> inbuf;
			inbuf.push(boost::iostreams::gzip_decompressor());
			inbuf.push(protein_domains_file);
			istream instream(&inbuf);
			ProteinDomains domains(instream);

			string outputFolder;
			if (ctx.assembly_output_folder.empty()) {
				outputFolder = sequencesFolder + accession;
			} else {
				outputFolder = ctx.assembly_output_folder;
			}

			string assembly_domain_prob_out_path;
			if (kind == "tri-nucleotide") {
				assembly_domain_prob_out_path = (
					outputFolder + "/" +
					accession + "_" + query + "_probability_" + tail + ".csv"
				);
			} else {
				assembly_domain_prob_out_path = (
					outputFolder + "/" +
					accession + "_" + query + "_aa_probability_" + tail + ".csv"
				);
			}

			ofstream of(assembly_domain_prob_out_path);
			auto writer = make_csv_writer(of);
			writer << DomainProbability::RecordHeader();

			vector<DomainProbability> records;
			for (ProteinDomain& domain : domains.Keys()) {
				if (metadata.find(domain.id) != metadata.end()) {
					auto& [domain_query, domain_description] = metadata[domain.id];
					domain.query = domain_query;
					domain.description = domain_description;
				}

				xt::xarray<double> probs = domains.Probabilities(domain, gene_probs);
				xt::xarray<double> probs_random = domains.Probabilities(domain, gene_probs, true);

				xt::xarray<double> log_probabilities = xt::eval(xt::log(probs));
				xt::xarray<double> log_probabilities_random = xt::eval(xt::log(probs_random));

				double log_prob = product_rule_log(log_probabilities);
				double log_prob_random = product_rule_log(log_probabilities_random);

				DomainProbability record(
					domain,
					log_prob,
					log_prob_random,
					log_probabilities.size()
				);
				records.push_back(record);
			}

			sort(records.begin(), records.end(), greater<DomainProbability>());

			for (auto& record : records) {
				writer << record.Record();
			}
		}
		catch (exception& e) {
			cerr << "Thread " << task_nb << " | Assembly: " << accession << " | ";
			cerr << "Exception: " << e.what() << endl;
			throw;
		}
	}

	auto tp = system_clock::now();
	auto elapsed = duration_cast<seconds>(tp - start).count();
	cerr << "Thread " << task_nb << ": DONE";
	cerr << " (elapsed: " << elapsed << " seconds)" << endl;
	return true;
}

bool task_compute_domain_probabilities_per_phylum(
	const int task_nb,
	const DomainProbabilityContext ctx,
	const vector<string>& phyla,
	const unordered_map<string, vector<string>>& assemblies_per_phylum
) {
	cerr << "Thread " << task_nb << " started." << endl;

	const string kind = ctx.kind;
	const string query = ctx.query;
	const string tail = ctx.tail;

	string dataFolder = "../data/";
	string sequencesFolder = dataFolder + "sequences/";
	string phylumFolder = dataFolder + "phylum/";
	auto n_phyla = phyla.size();

	// Create phylum directory if it does not exist.
	filesystem::create_directory(phylumFolder);

	int i = 0;
	for (auto& phylum : phyla) {
		auto& assembly_ids = assemblies_per_phylum.at(phylum);
		auto n_assemblies = assembly_ids.size();

		cerr << "Thread " << task_nb << ": ";
		cerr << "Processing phylum " << i + 1 << " / " << n_phyla;
		cerr << ": " << phylum << " (" << n_assemblies << " assemblies)";
		cerr << endl;
		++i;

		set<ProteinDomain> protein_domains;
		unordered_map<ProteinDomain, vector<DomainProbability>> protein_domain_probs;
		for (auto& accession : assembly_ids) {
			string assemblyFolder;
			if (ctx.assembly_output_folder.empty()) {
				assemblyFolder = sequencesFolder + accession;
			} else {
				assemblyFolder = ctx.assembly_output_folder;
			}

			string path;
			if (kind == "tri-nucleotide") {
				path = (
					assemblyFolder + "/" +
					accession + "_" + query + "_probability_" + tail + ".csv"
				);
			} else {
				path = (
					assemblyFolder + "/" +
					accession + "_" + query + "_aa_probability_" + tail + ".csv"
				);
			}

			vector<DomainProbability> domains = LoadDomainProbabilities(path);
			for (auto& domain_prob : domains) {
				auto& domain = domain_prob.domain;
				protein_domains.insert(domain);

				if (protein_domain_probs.find(domain) == protein_domain_probs.end()) {
					protein_domain_probs[domain] = vector<DomainProbability>{domain_prob};
				} else {
					protein_domain_probs[domain].push_back(domain_prob);
				}
			}
		}

		string phylum_lower = phylum;
		transform(phylum_lower.begin(), phylum_lower.end(), phylum_lower.begin(), ::tolower);
		transform(phylum_lower.begin(), phylum_lower.end(), phylum_lower.begin(), [](char ch) {
		    return ch == ' ' ? '_' : ch;
		});

		string phylumDir = phylumFolder + phylum_lower + "/";
		if (!ctx.phylum_output_folder.empty()) {
			phylumDir = ctx.phylum_output_folder + "/";
		}
		filesystem::create_directory(phylumDir);

		string phylum_domain_prob_out_path;
		if (kind == "tri-nucleotide") {
			phylum_domain_prob_out_path = (
				phylumDir +
				phylum_lower + "_" + query + "_probability_" + tail + ".csv"
			);
		} else {
			phylum_domain_prob_out_path = (
				phylumDir +
				phylum_lower + "_" + query + "_aa_probability_" + tail + ".csv"
			);
		}

		ofstream of(phylum_domain_prob_out_path);
		auto writer = make_csv_writer(of);
		writer << DomainProbability::RecordHeader();

		vector<DomainProbability> records;
		for (auto& domain : protein_domains) {
			auto& domain_probs = protein_domain_probs[domain];
			auto n_probs = domain_probs.size();
			xt::xarray<double> log_probs = xt::zeros<double>({n_probs});
			xt::xarray<double> log_probs_random = xt::zeros<double>({n_probs});
			for (int ix = 0; ix < n_probs; ++ix) {
				log_probs[ix] = domain_probs[ix].log_probability;
				log_probs_random[ix] = domain_probs[ix].log_probability_random;
			}

			double log_prob = product_rule_log(log_probs);
			double log_prob_random = product_rule_log(log_probs_random);

			try {
				DomainProbability record(
					domain,
					log_prob,
					log_prob_random,
					n_probs
				);
				records.push_back(record);
			}
			catch (exception& e) {
				cerr << "Thread " << task_nb << " | Phylum: " << phylum << " | ";
				cerr << "Exception: " << e.what() << endl;
				throw;
			}
		}

		sort(records.begin(), records.end(), greater<DomainProbability>());

		for (auto& record : records) {
			writer << record.Record();
		}
	}
	cerr << "Thread " << task_nb << ": DONE" << endl;
	return true;
}

void compute_domain_probabilities(const DomainProbabilityContext ctx) {
	auto start = system_clock::now();

	const string kind = ctx.kind;
	const string query = ctx.query;
	const string tail = ctx.tail;
	const int n_threads = ctx.n_threads;

	string dataFolder = "../data/";
	string assembliesPath = dataFolder + "assemblies.csv";

	Assemblies assemblies(assembliesPath, ctx.complete_genome_only);
	auto assembly_ids = assemblies.GetIds();
	auto n_per_thread = ceil((double) assembly_ids.size() / (double) n_threads);

	//
	// 1) Compute probability of domains for each assembly individually.
	//
	cerr << "Processing of domain probabilities per assembly" << endl;
	cerr << "Processing " << assemblies.Size() << " assemblies" << endl;
	cerr << "Starting " << n_threads << " threads" << endl;

	vector<future<bool>> futures;
	for (int i = 0; i < n_threads; ++i) {
		auto start = assembly_ids.begin() + i * n_per_thread;
		auto end = assembly_ids.end();
		int endInt = i * n_per_thread + n_per_thread;
		if (endInt < assembly_ids.size()) {
			end = assembly_ids.begin() + endInt;
		}
		auto ids = vector<string>(start, end);
		futures.push_back(async(
			task_compute_domain_probabilities_per_assembly,
			i+1,
			ctx,
			ids
		));
	}
	for (auto& f : futures) {
		if(!f.get()) {
			throw runtime_error("Unexpected error while processing assembly output");
		}
	}

	auto tp = system_clock::now();
	auto elapsed = duration_cast<seconds>(tp - start).count();
	cerr << "Processing of domain probabilities per assembly is complete" << endl;
	cerr << "Elapsed: " << elapsed << " seconds" << endl;

	//
	// 2) Compute probability of domains for each phylum
	//    with at least 10 assemblies within it.
	//
	size_t min_n_phyla = 10;
	cerr << "Processing of domain probabilities per phylum" << endl;

	unordered_map<string, vector<string>> assemblies_per_phylum;
	for (auto& assembly_id : assembly_ids) {
		Assembly& assembly = assemblies.Get(assembly_id);
		string phylum = assembly.phylum;
		if (phylum.empty()) {
			continue;
		}
		if (assemblies_per_phylum.find(phylum) == assemblies_per_phylum.end()) {
			assemblies_per_phylum[phylum] = vector<string>{assembly_id};
		} else {
			assemblies_per_phylum[phylum].push_back(assembly_id);
		}
	}

	set<string> phyla_set;
	for (auto& assembly_id : assembly_ids) {
		Assembly& assembly = assemblies.Get(assembly_id);
		string phylum = assembly.phylum;
		if (!phylum.empty() && assemblies_per_phylum[phylum].size() >= min_n_phyla) {
			phyla_set.insert(phylum);
		}
	}

	vector<string> phyla;
	phyla.assign(phyla_set.begin(), phyla_set.end());
	auto n_phyla = phyla.size();

	cerr << "Processing " << n_phyla << " phyla" << endl;
	cerr << "Starting " << n_threads << " threads" << endl;

	n_per_thread = ceil((double) n_phyla / (double) n_threads);

	vector<future<bool>> futuresP;
	for (int i = 0; i < n_threads; ++i) {
		auto start = phyla.begin() + i * n_per_thread;
		auto end = phyla.end();
		int endInt = i * n_per_thread + n_per_thread;
		if (endInt < n_phyla) {
			end = phyla.begin() + endInt;
		}
		futuresP.push_back(async(
			task_compute_domain_probabilities_per_phylum,
			i+1,
			ctx,
			vector<string>(start, end),
			assemblies_per_phylum
		));
	}
	for (auto& f : futuresP) {
		if(!f.get()) {
			throw runtime_error("Unexpected error while processing phylum output");
		}
	}

	tp = system_clock::now();
	elapsed = duration_cast<seconds>(tp - start).count();
	cerr << "Processing of domain probabilities per phylum is complete" << endl;
	cerr << "Elapsed: " << elapsed << " seconds" << endl;

	//
	// 3) Compute probability of domains per superkingdom.
	//
	cerr << "Processing of domain probabilities per superkingdom" << endl;
	vector<string> superkingdoms;
	unordered_map<string, vector<string>> phyla_per_superkingdom;
	for (auto& phylum : phyla) {
		auto assembly_id = assemblies_per_phylum[phylum][0];
		Assembly& assembly = assemblies.Get(assembly_id);
		const string superkingdom_raw = assembly.domain;
		if (superkingdom_raw.empty()) {
			continue;
		}
		string superkingdom = superkingdom_raw;
		transform(
			superkingdom.begin(),
			superkingdom.end(),
			superkingdom.begin(),
			::tolower
		);
		superkingdom[0] = toupper(superkingdom[0]);

		if (phyla_per_superkingdom.find(superkingdom) == phyla_per_superkingdom.end()) {
			phyla_per_superkingdom[superkingdom] = vector<string>{phylum};
			superkingdoms.push_back(superkingdom);
		} else {
			phyla_per_superkingdom[superkingdom].push_back(phylum);
		}
	}

	string superkingdom_folder = dataFolder + "superkingdom/";
	filesystem::create_directory(superkingdom_folder);

	for (auto& superkingdom : superkingdoms) {
		string superkingdom_lower = superkingdom;
		transform(
			superkingdom_lower.begin(),
			superkingdom_lower.end(),
			superkingdom_lower.begin(),
			::tolower
		);

		string superkingdom_inner_folder = (
			superkingdom_folder + "/" + superkingdom_lower + "/"
		);
		if (!ctx.superkingdom_output_folder.empty()) {
			superkingdom_inner_folder = ctx.superkingdom_output_folder + "/";
		}
		filesystem::create_directory(superkingdom_inner_folder);

		auto& superkingdom_phyla = phyla_per_superkingdom[superkingdom];
		auto n_superkingdom_phyla = superkingdom_phyla.size();

		set<ProteinDomain> protein_domains;
		unordered_map<ProteinDomain, vector<DomainProbability>> protein_domain_probs;
		for (auto& phylum : superkingdom_phyla) {
			string phylum_lower = phylum;
			transform(
				phylum_lower.begin(),
				phylum_lower.end(),
				phylum_lower.begin(),
				::tolower
			);
			transform(
				phylum_lower.begin(),
				phylum_lower.end(),
				phylum_lower.begin(),
				[](char ch) {
				    return ch == ' ' ? '_' : ch;
				}
			);
			string phylumDir = dataFolder + "phylum/" + phylum_lower + "/";
			if (!ctx.phylum_output_folder.empty()) {
				phylumDir = ctx.phylum_output_folder + "/";
			}

			string phylum_domain_prob_path;
			if (kind == "tri-nucleotide") {
				phylum_domain_prob_path = (
					phylumDir +
					phylum_lower + "_" + query + "_probability_" + tail + ".csv"
				);
			} else {
				phylum_domain_prob_path = (
					phylumDir +
					phylum_lower + "_" + query + "_aa_probability_" + tail + ".csv"
				);
			}

			vector<DomainProbability> domains = LoadDomainProbabilities(
				phylum_domain_prob_path
			);
			for (auto& domain_prob : domains) {
				auto& domain = domain_prob.domain;
				protein_domains.insert(domain);

				if (protein_domain_probs.find(domain) == protein_domain_probs.end()) {
					protein_domain_probs[domain] = vector<DomainProbability>{domain_prob};
				} else {
					protein_domain_probs[domain].push_back(domain_prob);
				}
			}
		}

		string superkingdom_out_path;
		if (kind == "tri-nucleotide") {
			superkingdom_out_path= (
				superkingdom_inner_folder +
				superkingdom_lower + "_" + query + "_probability_" + tail + ".csv"
			);
		} else {
			superkingdom_out_path= (
				superkingdom_inner_folder +
				superkingdom_lower + "_" + query + "_aa_probability_" + tail + ".csv"
			);
		}

		ofstream superkingdom_of(superkingdom_out_path);
		auto writer = make_csv_writer(superkingdom_of);
		writer << DomainProbability::RecordHeader();

		xt::xarray<double> uniform_log_prior = xt::eval(
			xt::log(make_uniform_prior(n_superkingdom_phyla))
		);

		vector<DomainProbability> records;
		for (auto& domain : protein_domains) {
			auto& domain_probs = protein_domain_probs[domain];
			auto n_probs = domain_probs.size();
			xt::xarray<double> log_probs = xt::zeros<double>({n_superkingdom_phyla});
			xt::xarray<double> log_probs_random = xt::zeros<double>({n_superkingdom_phyla});
			for (int ix = 0; ix < n_probs; ++ix) {
				log_probs[ix] = domain_probs[ix].log_probability;
				log_probs_random[ix] = domain_probs[ix].log_probability_random;
			}

			double log_prob = marginalization_log(
				uniform_log_prior,
				log_probs
			);
			double log_prob_random = marginalization_log(
				uniform_log_prior,
				log_probs_random
			);

			try {
				DomainProbability record(
					domain,
					log_prob,
					log_prob_random,
					n_probs
				);
				records.push_back(record);
			}
			catch (exception& e) {
				cerr << "Global computation | ";
				cerr << "Exception: " << e.what() << endl;
				throw;
			}
		}

		sort(records.begin(), records.end(), greater<DomainProbability>());

		for (auto& record : records) {
			writer << record.Record();
		}
	}
	cerr << "Processing of domain probabilities per superkingdom is complete" << endl;

	//
	// 4) Compute global probability of domains.
	//
	cerr << "Processing of domain probabilities globally" << endl;
	set<ProteinDomain> protein_domains;
	unordered_map<ProteinDomain, vector<DomainProbability>> protein_domain_probs;
	for (auto& phylum : phyla) {
		string phylum_lower = phylum;
		transform(phylum_lower.begin(), phylum_lower.end(), phylum_lower.begin(), ::tolower);
		transform(phylum_lower.begin(), phylum_lower.end(), phylum_lower.begin(), [](char ch) {
		    return ch == ' ' ? '_' : ch;
		});
		string phylumDir = dataFolder + "phylum/" + phylum_lower + "/";
		if (!ctx.phylum_output_folder.empty()) {
			phylumDir = ctx.phylum_output_folder + "/";
		}

		string phylum_domain_prob_path;
		if (kind == "tri-nucleotide") {
			phylum_domain_prob_path = (
				phylumDir +
				phylum_lower + "_" + query + "_probability_" + tail + ".csv"
			);
		} else {
			phylum_domain_prob_path = (
				phylumDir +
				phylum_lower + "_" + query + "_aa_probability_" + tail + ".csv"
			);
		}

		vector<DomainProbability> domains = LoadDomainProbabilities(phylum_domain_prob_path);
		for (auto& domain_prob : domains) {
			auto& domain = domain_prob.domain;
			protein_domains.insert(domain);

			if (protein_domain_probs.find(domain) == protein_domain_probs.end()) {
				protein_domain_probs[domain] = vector<DomainProbability>{domain_prob};
			} else {
				protein_domain_probs[domain].push_back(domain_prob);
			}
		}
	}

	string overallOutputFolder = dataFolder;
	if (!ctx.overall_output_folder.empty()) {
		overallOutputFolder = ctx.overall_output_folder + "/";
	}

	string protein_out_path;
	if (kind == "tri-nucleotide") {
		protein_out_path = overallOutputFolder + query + "_probability_" + tail + ".csv";
	} else {
		protein_out_path = overallOutputFolder + query + "_aa_probability_" + tail + ".csv";
	}

	ofstream output_file(protein_out_path);
	auto writer = make_csv_writer(output_file);
	writer << DomainProbability::RecordHeader();

	xt::xarray<double> uniform_log_prior = xt::eval(
		xt::log(make_uniform_prior(n_phyla))
	);

	vector<DomainProbability> records;
	for (auto& domain : protein_domains) {
		auto& domain_probs = protein_domain_probs[domain];
		auto n_probs = domain_probs.size();
		xt::xarray<double> log_probs = xt::zeros<double>({n_phyla});
		xt::xarray<double> log_probs_random = xt::zeros<double>({n_phyla});
		for (int ix = 0; ix < n_probs; ++ix) {
			log_probs[ix] = domain_probs[ix].log_probability;
			log_probs_random[ix] = domain_probs[ix].log_probability_random;
		}

		double log_prob = marginalization_log(
			uniform_log_prior,
			log_probs
		);
		double log_prob_random = marginalization_log(
			uniform_log_prior,
			log_probs_random
		);

		try {
			DomainProbability record(
				domain,
				log_prob,
				log_prob_random,
				n_probs
			);
			records.push_back(record);
		}
		catch (exception& e) {
			cerr << "Global computation | ";
			cerr << "Exception: " << e.what() << endl;
			throw;
		}
	}

	sort(records.begin(), records.end(), greater<DomainProbability>());

	for (auto& record : records) {
		writer << record.Record();
	}

	tp = system_clock::now();
	elapsed = duration_cast<seconds>(tp - start).count();
	cerr << "Processing of global domain probabilities is complete" << endl;
	cerr << "Elapsed: " << elapsed << " seconds" << endl;
	cerr << "DONE" << endl;
}


#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>
#include <arc_utilities/eigen_typedefs.hpp>
#include <map>
#include <vector>

#ifndef IIWA_7_FK_FAST_HPP
#define IIWA_7_FK_FAST_HPP

namespace IIWA_7_FK_FAST {
const size_t IIWA_7_NUM_ACTIVE_JOINTS = 7;
const size_t IIWA_7_NUM_LINKS = 8;

const std::string IIWA_7_ACTIVE_JOINT_1_NAME = "iiwa_joint_1";
const std::string IIWA_7_ACTIVE_JOINT_2_NAME = "iiwa_joint_2";
const std::string IIWA_7_ACTIVE_JOINT_3_NAME = "iiwa_joint_3";
const std::string IIWA_7_ACTIVE_JOINT_4_NAME = "iiwa_joint_4";
const std::string IIWA_7_ACTIVE_JOINT_5_NAME = "iiwa_joint_5";
const std::string IIWA_7_ACTIVE_JOINT_6_NAME = "iiwa_joint_6";
const std::string IIWA_7_ACTIVE_JOINT_7_NAME = "iiwa_joint_7";

const std::string IIWA_7_LINK_1_NAME = "iiwa_link_0";
const std::string IIWA_7_LINK_2_NAME = "iiwa_link_1";
const std::string IIWA_7_LINK_3_NAME = "iiwa_link_2";
const std::string IIWA_7_LINK_4_NAME = "iiwa_link_3";
const std::string IIWA_7_LINK_5_NAME = "iiwa_link_4";
const std::string IIWA_7_LINK_6_NAME = "iiwa_link_5";
const std::string IIWA_7_LINK_7_NAME = "iiwa_link_6";
const std::string IIWA_7_LINK_8_NAME = "iiwa_link_7";

inline Eigen::Isometry3d Get_link_0_joint_1_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.15);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_1_joint_2_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.19);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, M_PI);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_2_joint_3_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.21, 0.0);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, M_PI);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_3_joint_4_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.19);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_4_joint_5_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.21, 0.0);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(-M_PI_2, M_PI, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_5_joint_6_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.06070, 0.19);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline Eigen::Isometry3d Get_link_6_joint_7_LinkJointTransform(const double joint_val) {
  const Eigen::Translation3d pre_joint_translation(0.0, 0.081, 0.06070);
  const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(-M_PI_2, M_PI, 0.0);
  const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
  const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
  const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
  return (pre_joint_transform * joint_transform);
}

inline EigenHelpers::VectorIsometry3d GetLinkTransforms(
    const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  assert(configuration.size() == IIWA_7_NUM_ACTIVE_JOINTS);
  EigenHelpers::VectorIsometry3d link_transforms(IIWA_7_NUM_LINKS);
  link_transforms[0] = base_transform;
  link_transforms[1] = link_transforms[0] * Get_link_0_joint_1_LinkJointTransform(configuration[0]);
  link_transforms[2] = link_transforms[1] * Get_link_1_joint_2_LinkJointTransform(configuration[1]);
  link_transforms[3] = link_transforms[2] * Get_link_2_joint_3_LinkJointTransform(configuration[2]);
  link_transforms[4] = link_transforms[3] * Get_link_3_joint_4_LinkJointTransform(configuration[3]);
  link_transforms[5] = link_transforms[4] * Get_link_4_joint_5_LinkJointTransform(configuration[4]);
  link_transforms[6] = link_transforms[5] * Get_link_5_joint_6_LinkJointTransform(configuration[5]);
  link_transforms[7] = link_transforms[6] * Get_link_6_joint_7_LinkJointTransform(configuration[6]);
  return link_transforms;
}

inline EigenHelpers::VectorIsometry3d GetLinkTransforms(
    const std::map<std::string, double>& configuration,
    const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  std::vector<double> configuration_vector(IIWA_7_NUM_ACTIVE_JOINTS);
  configuration_vector[0] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_1_NAME, 0.0);
  configuration_vector[1] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_2_NAME, 0.0);
  configuration_vector[2] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_3_NAME, 0.0);
  configuration_vector[3] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_4_NAME, 0.0);
  configuration_vector[4] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_5_NAME, 0.0);
  configuration_vector[5] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_6_NAME, 0.0);
  configuration_vector[6] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_7_NAME, 0.0);
  return GetLinkTransforms(configuration_vector, base_transform);
}

inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(
    const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
  EigenHelpers::MapStringIsometry3d link_transforms_map;
  link_transforms_map[IIWA_7_LINK_1_NAME] = link_transforms[0];
  link_transforms_map[IIWA_7_LINK_2_NAME] = link_transforms[1];
  link_transforms_map[IIWA_7_LINK_3_NAME] = link_transforms[2];
  link_transforms_map[IIWA_7_LINK_4_NAME] = link_transforms[3];
  link_transforms_map[IIWA_7_LINK_5_NAME] = link_transforms[4];
  link_transforms_map[IIWA_7_LINK_6_NAME] = link_transforms[5];
  link_transforms_map[IIWA_7_LINK_7_NAME] = link_transforms[6];
  link_transforms_map[IIWA_7_LINK_8_NAME] = link_transforms[7];
  return link_transforms_map;
}

inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(
    const std::map<std::string, double>& configuration,
    const Eigen::Isometry3d& base_transform = Eigen::Isometry3d::Identity()) {
  const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
  EigenHelpers::MapStringIsometry3d link_transforms_map;
  link_transforms_map[IIWA_7_LINK_1_NAME] = link_transforms[0];
  link_transforms_map[IIWA_7_LINK_2_NAME] = link_transforms[1];
  link_transforms_map[IIWA_7_LINK_3_NAME] = link_transforms[2];
  link_transforms_map[IIWA_7_LINK_4_NAME] = link_transforms[3];
  link_transforms_map[IIWA_7_LINK_5_NAME] = link_transforms[4];
  link_transforms_map[IIWA_7_LINK_6_NAME] = link_transforms[5];
  link_transforms_map[IIWA_7_LINK_7_NAME] = link_transforms[6];
  link_transforms_map[IIWA_7_LINK_8_NAME] = link_transforms[7];
  return link_transforms_map;
}
}  // namespace IIWA_7_FK_FAST

#endif  // IIWA_7_FK_FAST_HPP


#pragma once

#ifdef USE_FIBONACCI_HEAP
#include <boost/heap/fibonacci_heap.hpp>
#endif

#include <boost/heap/d_ary_heap.hpp>
#include <unordered_map>
#include <unordered_set>

#include "neighbor.hpp"
#include "planresult.hpp"

// #define REBUILT_FOCAL_LIST
// #define CHECK_FOCAL_LIST

namespace libMultiRobotPlanning {

/*!
  \example a_star_epsilon.cpp Simple example using a 2D grid world and
  up/down/left/right
  actions
*/

/*! \brief A*_epsilon Algorithm to find the shortest path with a given
suboptimality bound (also known as focal search)
This class implements the A*_epsilon algorithm, an informed search
algorithm
that finds the shortest path for a given map up to a suboptimality factor.
It uses an admissible heuristic (to keep track of the optimum) and an
inadmissible heuristic (
to guide the search within a suboptimal bound w.)
Details of the algorithm can be found in the following paper:\n
Judea Pearl, Jin H. Kim:\n
"Studies in Semi-Admissible Heuristics."" IEEE Trans. Pattern Anal. Mach.
Intell. 4(4): 392-399 (1982)\n
https://doi.org/10.1109/TPAMI.1982.4767270
This class can either use a fibonacci heap, or a d-ary heap. The latter is the
default. Define "USE_FIBONACCI_HEAP" to use the fibonacci heap instead.
\tparam State Custom state for the search. Needs to be copy'able
\tparam Action Custom action for the search. Needs to be copy'able
\tparam Cost Custom Cost type (integer or floating point types)
\tparam Environment This class needs to provide the custom logic. In
    particular, it needs to support the following functions:
  - `Cost admissibleHeuristic(const State& s)`\n
    This function can return 0 if no suitable heuristic is available.
  - `Cost focalStateHeuristic(const State& s, Cost gScore)`\n
    This function computes a (potentially inadmissible) heuristic for the given
state.
  - `Cost focalTransitionHeuristic(const State& s1, const State& s2, Cost
gScoreS1, Cost gScoreS2)`\n
    This function computes a (potentially inadmissible) heuristic for the given
state transition.
  - `bool isSolution(const State& s)`\n
    Return true if the given state is a goal state.
  - `void getNeighbors(const State& s, std::vector<Neighbor<State, Action,
   int> >& neighbors)`\n
    Fill the list of neighboring state for the given state s.
  - `void onExpandNode(const State& s, int fScore, int gScore)`\n
    This function is called on every expansion and can be used for statistical
purposes.
  - `void onDiscover(const State& s, int fScore, int gScore)`\n
    This function is called on every node discovery and can be used for
   statistical purposes.
    \tparam StateHasher A class to convert a state to a hash value. Default:
   std::hash<State>
*/
template <typename State, typename Action, typename Cost, typename Environment,
          typename StateHasher = std::hash<State> >
class IOS {
 public:
  IOS(Environment& environment, float w)
      : m_env(environment), m_w(w) {}

  bool search(const State& startState,
              PlanResult<State, Action, Cost>& solution) {
    solution.states.clear();
    solution.states.push_back(std::make_pair<>(startState, 0));
    solution.actions.clear();
    solution.cost = INT32_MAX;

    openSet_t openSet;
    focalSet_t
        focalSet;  // subset of open nodes that are within suboptimality bound
    anopenset_t anopen;
    std::unordered_map<State, fibHeapHandle_t, StateHasher> stateToHeap;
    std::unordered_map<State, fibHeapHandle_t, StateHasher> stateToFocal;
    std::unordered_set<State, StateHasher> closedSet;
    std::unordered_map<State, std::tuple<State, Action, Cost, Cost>,
                       StateHasher>
        cameFrom;

    auto handle = openSet.push(
        Node(startState, m_env.admissibleHeuristic(startState), 0, 0,0));
    stateToHeap.insert(std::make_pair<>(startState, handle));
    (*handle).handle = handle;

    anopen.push(handle);
    focalSet.push(handle);

    stateToFocal.insert(std::make_pair<>(startState, handle));

    std::vector<Neighbor<State, Action, Cost> > neighbors;
    neighbors.reserve(10);

    Cost bestFScore = (*handle).fScore;

    std::cout << "new search" << std::endl;

//my work
while(!openSet.empty()){

  //printf("In Open\n");

  const auto& nopen = openSet.top();
  //std:: cout << m_w <<std::endl;
  if(solution.cost <= m_w * nopen.fScore ){
        //printf("Found\n");
        return true;
  }
  //Node nopen = *nopen_handle;
  if(focalSet.empty()){
    printf("Empty\n");
    return false;
  }
  auto currentHandle = focalSet.top();
  Node current = *currentHandle;

  auto currentHandle_anop = anopen.top();
  Node current_anop = *currentHandle_anop;

  if(current.focalHeuristic < solution.cost){
    // Do greedy step

    //printf("greedy\n");

    focalSet.pop();
    // openSet.erase(currentHandle);
    stateToFocal.erase(current.state);
    closedSet.insert(current.state);

    m_env.onExpandNode(current.state, current.fScore, current.gScore);

    if (m_env.isSolution(current.state)) {
        printf("Solution\n");
        solution.states.clear();
        solution.actions.clear();
        auto iter = cameFrom.find(current.state);
        while (iter != cameFrom.end()) {
          solution.states.push_back(
              std::make_pair<>(iter->first, std::get<3>(iter->second)));
          solution.actions.push_back(std::make_pair<>(
              std::get<1>(iter->second), std::get<2>(iter->second)));
          iter = cameFrom.find(std::get<0>(iter->second));
        }
        solution.states.push_back(std::make_pair<>(startState, 0));
        std::reverse(solution.states.begin(), solution.states.end());
        std::reverse(solution.actions.begin(), solution.actions.end());
        solution.cost = current.gScore;
        if(openSet.empty())
          solution.fmin = current.fScore;
        else
          solution.fmin = openSet.top().fScore;
        if(current.focalHeuristic <= (m_w*current_anop.fopenmax)){
          return true;
        }

      }
    else{
      neighbors.clear();
      m_env.getNeighbors(current.state, neighbors);
      for (const Neighbor<State, Action, Cost>& neighbor : neighbors) {
        //printf("Neighbour\n");
        if (closedSet.find(neighbor.state) == closedSet.end()) {
          Cost tentative_gScore = current.gScore + neighbor.cost;
          auto iter = stateToHeap.find(neighbor.state);
          auto iterFocal = stateToFocal.find(neighbor.state);
          if (iter == stateToHeap.end() && iterFocal == stateToFocal.end()) {  // Discover a new node
            //std::cout << "  this is a new node" << std::endl;

            // I didn't add Improved termination condition

            Cost fScore =
                tentative_gScore + m_env.admissibleHeuristic(neighbor.state);

            // std::cout << tentative_gScore << " " << current.focalHeuristic << " " << m_env.focalStateHeuristic(neighbor.state, tentative_gScore) << " " << m_env.focalTransitionHeuristic(current.state, neighbor.state,
            //                                    current.gScore,
            //                                    tentative_gScore) << std::endl;

            Cost focalHeuristic = tentative_gScore + ((2*m_w-1) * m_env.admissibleHeuristic(neighbor.state)) ;

            Cost fopenmax = (tentative_gScore/m_w) +  m_env.admissibleHeuristic(neighbor.state);

            /*
            ((2*m_w - 1)*
                (m_env.focalStateHeuristic(neighbor.state, tentative_gScore) +
                 m_env.focalTransitionHeuristic(current.state, neighbor.state,
                                               current.gScore,
                                               tentative_gScore)));
                                               */

            auto handle = openSet.push(
                Node(neighbor.state, fScore, tentative_gScore, focalHeuristic, fopenmax));
            (*handle).handle = handle;
            focalSet.push(handle);
            anopen.push(handle);
            // if (fScore <= bestFScore * m_w) {
            //   // std::cout << "focalAdd: " << *handle << std::endl;
            //   focalSet.push(handle);
            // }
            stateToHeap.insert(std::make_pair<>(neighbor.state, handle));
            stateToFocal.insert(std::make_pair<>(neighbor.state, handle));
            m_env.onDiscover(neighbor.state, fScore, tentative_gScore);
            // std::cout << "  this is a new node " << fScore << "," <<
            // tentative_gScore << std::endl;
            cameFrom.erase(neighbor.state);
            cameFrom.insert(std::make_pair<>(
              neighbor.state,
              std::make_tuple<>(current.state, neighbor.action, neighbor.cost,
                                tentative_gScore)));
          }
        }
      }
    }

  }
  else{

    //Do optimal step

    std::cout<< "In Optimal" <<std::endl;

    const auto& nopen2 = openSet.top();
    std:: cout<< anopen.size() << std:: endl;
    openSet.pop();
    std:: cout<< anopen.size() << std:: endl;
    auto iter_for_open = stateToHeap.find(nopen2.state);
    currentHandle = iter_for_open->second;
    current = *currentHandle;
    // openSet.pop();
    // openSet.erase(currentHandle);
    //anopen.erase(current);
    stateToHeap.erase(current.state);
    if(closedSet.find(current.state)==closedSet.end()){
      std::cout<< "New Low level node" << std::endl;
      //m_env.onExpandNode(current.state, current.fScore, current.gScore);
      closedSet.insert(current.state);
    }

    //Do i need to add this node to closed set

    m_env.onExpandNode(current.state, current.fScore, current.gScore);

    if (m_env.isSolution(current.state)) {
        solution.states.clear();
        solution.actions.clear();
        auto iter = cameFrom.find(current.state);
        while (iter != cameFrom.end()) {
          solution.states.push_back(
              std::make_pair<>(iter->first, std::get<3>(iter->second)));
          solution.actions.push_back(std::make_pair<>(
              std::get<1>(iter->second), std::get<2>(iter->second)));
          iter = cameFrom.find(std::get<0>(iter->second));
        }
        solution.states.push_back(std::make_pair<>(startState, 0));
        std::reverse(solution.states.begin(), solution.states.end());
        std::reverse(solution.actions.begin(), solution.actions.end());
        solution.cost = current.gScore;
        solution.fmin = current.fScore;
        return true;
      }
    else{
      neighbors.clear();
      m_env.getNeighbors(current.state, neighbors);
      for (const Neighbor<State, Action, Cost>& neighbor : neighbors) {

        Cost tentative_gScore = current.gScore + neighbor.cost;
        auto iter = stateToHeap.find(neighbor.state);
        auto iterFocal = stateToFocal.find(neighbor.state);

        if (iter != stateToHeap.end()) {
          std::cout<< "In open" <<std::endl;
          auto handle = iter->second;
            // We found this node with a better path than previous path
            if (tentative_gScore < (*handle).gScore) {
              std:: cout << "Update Open" <<std::endl;
              (*handle).gScore = tentative_gScore;
              (*handle).fScore = tentative_gScore + m_env.admissibleHeuristic(neighbor.state);
              (*handle).focalHeuristic = tentative_gScore + ((2*m_w-1) * m_env.admissibleHeuristic(neighbor.state)) ;
              (*handle).fopenmax = (tentative_gScore/m_w) +  m_env.admissibleHeuristic(neighbor.state);
              // (*handle).focalHeuristic = tentative_gScore + ((2*m_w - 1)*
              //   (m_env.focalStateHeuristic(neighbor.state, tentative_gScore) +
              //    m_env.focalTransitionHeuristic(current.state, neighbor.state,
              //                                  current.gScore,
              //                                  tentative_gScore)));
                openSet.update(handle);
                // m_env.onDiscover(neighbor.state, (*handle).fScore,
                //              (*handle).gScore);

                //Do i need to include this node to focal
                if(iterFocal != stateToFocal.end()){
                  std:: cout << "Update Focal" <<std::endl;
                  // handle = iterFocal->second;
                  // focalSet.update(handle);

                }
              cameFrom.erase(neighbor.state);
                  cameFrom.insert(std::make_pair<>(
                  neighbor.state,
                  std::make_tuple<>(current.state, neighbor.action, neighbor.cost,
                                    tentative_gScore)));
            }
        }
        else if(closedSet.find(neighbor.state) == closedSet.end()){

          if (iter == stateToHeap.end()) {  // Discover a new node
            // std::cout << "  this is a new node" << std::endl;
            Cost fScore =
                tentative_gScore + m_env.admissibleHeuristic(neighbor.state);

            Cost focalHeuristic = tentative_gScore + ((2*m_w-1) * m_env.admissibleHeuristic(neighbor.state)) ;

            Cost fopenmax = (tentative_gScore/m_w) +  m_env.admissibleHeuristic(neighbor.state);

            // Cost focalHeuristic = tentative_gScore + ((2*m_w - 1)*
            //     (m_env.focalStateHeuristic(neighbor.state, tentative_gScore) +
            //      m_env.focalTransitionHeuristic(current.state, neighbor.state,
            //                                    current.gScore,
            //                                    tentative_gScore)));
            auto handle = openSet.push(
                Node(neighbor.state, fScore, tentative_gScore, focalHeuristic,fopenmax));
            (*handle).handle = handle;

            focalSet.push(handle);
            anopen.push(handle);

            stateToHeap.insert(std::make_pair<>(neighbor.state, handle));
            stateToFocal.insert(std::make_pair<>(neighbor.state, handle));
            m_env.onDiscover(neighbor.state, fScore, tentative_gScore);
            // std::cout << "  this is a new node " << fScore << "," <<
            // tentative_gScore << std::endl;

            cameFrom.erase(neighbor.state);
            cameFrom.insert(std::make_pair<>(
              neighbor.state,
              std::make_tuple<>(current.state, neighbor.action, neighbor.cost,
                                tentative_gScore)));
        }
        }
      }

    }

  }
}

    return false;
  }

 private:
  struct Node;

#ifdef USE_FIBONACCI_HEAP
  typedef typename boost::heap::fibonacci_heap<Node> openSet_t;
  typedef typename openSet_t::handle_type fibHeapHandle_t;
// typedef typename boost::heap::fibonacci_heap<fibHeapHandle_t,
// boost::heap::compare<compareFocalHeuristic> > focalSet_t;
#else
  typedef typename boost::heap::d_ary_heap<Node, boost::heap::arity<2>,
                                           boost::heap::mutable_<true> >
      openSet_t;
  typedef typename openSet_t::handle_type fibHeapHandle_t;
// typedef typename boost::heap::d_ary_heap<fibHeapHandle_t,
// boost::heap::arity<2>, boost::heap::mutable_<true>,
// boost::heap::compare<compareFocalHeuristic> > focalSet_t;
#endif

  struct Node {
    Node(const State& state, Cost fScore, Cost gScore, Cost focalHeuristic, Cost fopenmax)
        : state(state),
          fScore(fScore),
          gScore(gScore),
          focalHeuristic(focalHeuristic),
          fopenmax(fopenmax) {}

    bool operator<(const Node& other) const {
      //printf("InOpenCompare\n");
      // Sort order
      // 1. lowest fScore
      // 2. highest gScore

      // Our heap is a maximum heap, so we invert the comperator function here
      if (fScore != other.fScore) {
        return fScore > other.fScore;
      } else {
        return gScore < other.gScore;
      }
    }

    friend std::ostream& operator<<(std::ostream& os, const Node& node) {
      os << "state: " << node.state << " fScore: " << node.fScore
         << " gScore: " << node.gScore << " focal: " << node.focalHeuristic;
      return os;
    }

    State state;

    Cost fScore;
    Cost gScore;
    Cost focalHeuristic;
    Cost fopenmax;

    fibHeapHandle_t handle;
    // #ifdef USE_FIBONACCI_HEAP
    //   typename boost::heap::fibonacci_heap<Node>::handle_type handle;
    // #else
    //   typename boost::heap::d_ary_heap<Node, boost::heap::arity<2>,
    //   boost::heap::mutable_<true> >::handle_type handle;
    // #endif
  };

  struct compareFocalHeuristic {
    bool operator()(const fibHeapHandle_t& h1,
                    const fibHeapHandle_t& h2) const {
      // Sort order (see "Improved Solvers for Bounded-Suboptimal Multi-Agent
      // Path Finding" by Cohen et. al.)
      // 1. lowest focalHeuristic
      // 2. lowest fScore
      // 3. highest gScore

      //printf("InfocalCompare\n");

      // Our heap is a maximum heap, so we invert the comperator function here

      //compare fun() for A*eps
      if ((*h1).focalHeuristic != (*h2).focalHeuristic) {
        return (*h1).focalHeuristic > (*h2).focalHeuristic;
      }
      else if ((*h1).fScore != (*h2).fScore) {
          return (*h1).fScore > (*h2).fScore;
      }
      else {
        return (*h1).gScore < (*h2).gScore;
      }

      //compare fun() for IOS
      // if ((*h1).focalHeuristic != (*h2).focalHeuristic) {
      //   return (*h1).focalHeuristic > (*h2).focalHeuristic;
      //   // } else if ((*h1).fScore != (*h2).fScore) {
      //   //   return (*h1).fScore > (*h2).fScore;
      // }
      // // else if ((*h1).fScore != (*h2).fScore) {
      // //   return (*h1).fScore > (*h2).fScore;
      // //}
      // else {
      //   return (*h1).gScore < (*h2).gScore;
      // }

    }
  };

#ifdef USE_FIBONACCI_HEAP
  // typedef typename boost::heap::fibonacci_heap<Node> openSet_t;
  // typedef typename openSet_t::handle_type fibHeapHandle_t;
  typedef typename boost::heap::fibonacci_heap<
      fibHeapHandle_t, boost::heap::compare<compareFocalHeuristic> >
      focalSet_t;
#else
  // typedef typename boost::heap::d_ary_heap<Node, boost::heap::arity<2>,
  // boost::heap::mutable_<true> > openSet_t;
  // typedef typename openSet_t::handle_type fibHeapHandle_t;
  typedef typename boost::heap::d_ary_heap<
      fibHeapHandle_t, boost::heap::arity<2>, boost::heap::mutable_<true>,
      boost::heap::compare<compareFocalHeuristic> >
      focalSet_t;
#endif

struct compareOpenMax {
    bool operator()(const fibHeapHandle_t& h1,
                    const fibHeapHandle_t& h2) const {
      // Sort order (see "Improved termination condition")

      // Our heap is a maximum heap, so we invert the comperator function here

      //compare fun() for A*eps
      return (*h1).fopenmax < (*h2).fopenmax;

    }
  };

  typedef typename boost::heap::d_ary_heap<
      fibHeapHandle_t, boost::heap::arity<2>, boost::heap::mutable_<true>,
      boost::heap::compare<compareOpenMax> >
      anopenset_t;

 private:
  Environment& m_env;
  float m_w;
};

}  // namespace libMultiRobotPlanning


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

#ifndef ITL_PC_SUB_MATRIX_PC_INCLUDE
#define ITL_PC_SUB_MATRIX_PC_INCLUDE

#include <boost/static_assert.hpp>
#include <boost/mpl/if.hpp>

#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/property_map.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/numeric/itl/pc/solver.hpp>

namespace itl { namespace pc {

/// Class for applying \tparam Preconditioner only on a sub-matrix
/** Other entries are just copied.
    Optionally preconditioner can be referred from outside instead of storing it by setting
    \tparam Store to true.
**/
template <typename Preconditioner, typename Matrix, bool Store= true>
class sub_matrix_pc
{
    typedef mtl::dense_vector<bool>                                               tag_type;
    typedef typename boost::mpl::if_c<Store, Preconditioner, const Preconditioner&>::type pc_type;

    struct matrix_container
    {
	matrix_container() : Ap(0) {}

	matrix_container(const tag_type& tags, const Matrix& src)
	{
	    using std::size_t;
	    using namespace mtl;

	    mtl::dense_vector<size_t> perm(size(tags));
	    size_t n= 0;
	    for (size_t i= 0; i < size(tags); ++i) {
		perm[i]= n;
		if (tags[i])
		    ++n;
	    }

	    Ap= new Matrix(n, n);

	    typename traits::row<Matrix>::type             row(src);
	    typename traits::col<Matrix>::type             col(src);
	    typename traits::const_value<Matrix>::type     value(src);
	    typedef typename traits::range_generator<tag::major, Matrix>::type  cursor_type;

	    mat::inserter<Matrix> ins(*Ap, Ap->nnz() / Ap->dim1());

	    for (cursor_type cursor = mtl::begin<tag::major>(src), cend = mtl::end<tag::major>(src);
		 cursor != cend; ++cursor) {
		// std::cout << dest << '\n';

		typedef typename traits::range_generator<tag::nz, cursor_type>::type icursor_type;
		for (icursor_type icursor = mtl::begin<tag::nz>(cursor), icend = mtl::end<tag::nz>(cursor);
		     icursor != icend; ++icursor)
		    if (tags[row(*icursor)] && tags[col(*icursor)])
			ins(perm[row(*icursor)], perm[col(*icursor)]) << value(*icursor);
	    }
	}

	~matrix_container() { delete Ap; }

	Matrix* Ap;
    };

    size_t count_entries() const
    {
	using mtl::size;
	size_t n= 0;
	for (size_t i= 0; i < size(tags); ++i)
	    if (tags[i]) ++n;
	return n;
    }

  public:

    sub_matrix_pc(const tag_type& tags, const Matrix& A)
      : tags(tags), n(count_entries()), mc(tags, A), P(*mc.Ap)
    {
	BOOST_STATIC_ASSERT((Store));
	delete mc.Ap;
	mc.Ap= 0;
    }

#if 0 // to do later
    sub_matrix_pc(const tag_type& tags, const Preconditioner& P)
      : tags(tags), P(P)
    {
	// check sizes
    }
#endif

  private:
    template <typename VectorIn>
    VectorIn& create_x0(VectorIn) const
    {
	static VectorIn  x0(n);
	return x0;
    }

    template <typename VectorOut>
    VectorOut& create_y0(VectorOut) const
    {
	static VectorOut  y0(n);
	return y0;
    }

    template <typename VectorIn>
    void restrict(const VectorIn& x, VectorIn& x0) const
    {
	for (size_t i= 0, j= 0; i < size(tags); ++i)
	    if (tags[i])
		x0[j++]= x[i];
    }

    template <typename VectorIn, typename VectorOut>
    void prolongate(const VectorIn& x, const VectorOut& y0, VectorOut& y) const
    {
	for (size_t i= 0, j= 0; i < size(tags); ++i)
	    if (tags[i])
		y[i]= y0[j++];
	    else
		y[i]= x[i];
    }

  public:
    /// Solve Px = y approximately on according sub-system; remaining entries are copied
    template <typename VectorIn, typename VectorOut>
    void solve(const VectorIn& x, VectorOut& y) const
    {
	mtl::vampir_trace<5056> tracer;
	y.checked_change_resource(x);

	VectorIn&  x0= create_x0(x);
	VectorOut& y0= create_y0(y);

	restrict(x, x0);
	P.solve(x0, y0);
	// y0= solve(P, x0); // doesn't compile yet for unknown reasons
	prolongate(x, y0, y);
    }

    /// Solve Px = y approximately on according sub-system; remaining entries are copied
    template <typename VectorIn, typename VectorOut>
    void adjoint_solve(const VectorIn& x, VectorOut& y) const
    {
	mtl::vampir_trace<5057> tracer;
	y.checked_change_resource(x);

	VectorIn&  x0= create_x0(x);
	VectorOut& y0= create_y0(y);

	restrict(x, x0);
	P.adjoint_solve(x0, y0);
	// y0= adjoint_solve(P, x0); // doesn't compile yet for unknown reasons
	prolongate(x, y0, y);
    }

  private:
    tag_type          tags;
    size_t            n;
    matrix_container  mc;
    pc_type           P;
};

template <typename Preconditioner, typename Matrix, bool Store, typename Vector>
solver<sub_matrix_pc<Preconditioner, Matrix, Store>, Vector, false>
inline solve(const sub_matrix_pc<Preconditioner, Matrix, Store>& P, const Vector& x)
{
    return solver<sub_matrix_pc<Preconditioner, Matrix, Store>, Vector, false>(P, x);
}

template <typename Preconditioner, typename Matrix, bool Store, typename Vector>
solver<sub_matrix_pc<Preconditioner, Matrix, Store>, Vector, true>
inline adjoint_solve(const sub_matrix_pc<Preconditioner, Matrix, Store>& P, const Vector& x)
{
    return solver<sub_matrix_pc<Preconditioner, Matrix, Store>, Vector, true>(P, x);
}

}} // namespace itl::pc

#endif // ITL_PC_SUB_MATRIX_PC_INCLUDE


/*
 Copyright (C) 2016-2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <orea/aggregation/postprocess.hpp>
#include <orea/aggregation/exposureallocator.hpp>
#include <orea/aggregation/dimcalculator.hpp>
#include <orea/aggregation/dimregressioncalculator.hpp>
#include <orea/aggregation/dynamiccreditxvacalculator.hpp>
#include <orea/aggregation/xvacalculator.hpp>
#include <orea/aggregation/staticcreditxvacalculator.hpp>
#include <orea/aggregation/cvaspreadsensitivitycalculator.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/vectorutils.hpp>
#include <ql/errors.hpp>
#include <ql/time/calendars/weekendsonly.hpp>
#include <ql/version.hpp>

#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/generallinearleastsquares.hpp>
#include <ql/math/kernelfunctions.hpp>
#include <ql/methods/montecarlo/lsmbasissystem.hpp>
#include <ql/time/daycounters/actualactual.hpp>

#include <qle/math/nadarayawatson.hpp>
#include <qle/math/stabilisedglls.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/error_of_mean.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>

using namespace std;
using namespace QuantLib;

using namespace boost::accumulators;

namespace ore {
namespace analytics {

PostProcess::PostProcess(
    const boost::shared_ptr<Portfolio>& portfolio, const boost::shared_ptr<NettingSetManager>& nettingSetManager,
    const boost::shared_ptr<Market>& market, const std::string& configuration, const boost::shared_ptr<NPVCube>& cube,
    const boost::shared_ptr<AggregationScenarioData>& scenarioData, const map<string, bool>& analytics,
    const string& baseCurrency, const string& allocMethod, Real marginalAllocationLimit, Real quantile,
    const string& calculationType, const string& dvaName, const string& fvaBorrowingCurve,
    const string& fvaLendingCurve,const boost::shared_ptr<DynamicInitialMarginCalculator>& dimCalculator,
    const boost::shared_ptr<CubeInterpretation>& cubeInterpretation, bool fullInitialCollateralisation,
    vector<Period> cvaSensiGrid, Real cvaSensiShiftSize,
    Real kvaCapitalDiscountRate, Real kvaAlpha, Real kvaRegAdjustment, Real kvaCapitalHurdle, Real kvaOurPdFloor,
    Real kvaTheirPdFloor, Real kvaOurCvaRiskWeight, Real kvaTheirCvaRiskWeight, const boost::shared_ptr<NPVCube>& cptyCube,
    const string& flipViewBorrowingCurvePostfix, const string& flipViewLendingCurvePostfix)
    : portfolio_(portfolio), nettingSetManager_(nettingSetManager), market_(market), configuration_(configuration),
      cube_(cube), cptyCube_(cptyCube), scenarioData_(scenarioData), analytics_(analytics), baseCurrency_(baseCurrency), quantile_(quantile),
      calcType_(parseCollateralCalculationType(calculationType)), dvaName_(dvaName),
      fvaBorrowingCurve_(fvaBorrowingCurve), fvaLendingCurve_(fvaLendingCurve), dimCalculator_(dimCalculator),
      cubeInterpretation_(cubeInterpretation), fullInitialCollateralisation_(fullInitialCollateralisation),
      cvaSpreadSensiGrid_(cvaSensiGrid), cvaSpreadSensiShiftSize_(cvaSensiShiftSize),
      kvaCapitalDiscountRate_(kvaCapitalDiscountRate), kvaAlpha_(kvaAlpha), kvaRegAdjustment_(kvaRegAdjustment),
      kvaCapitalHurdle_(kvaCapitalHurdle), kvaOurPdFloor_(kvaOurPdFloor), kvaTheirPdFloor_(kvaTheirPdFloor),
      kvaOurCvaRiskWeight_(kvaOurCvaRiskWeight), kvaTheirCvaRiskWeight_(kvaTheirCvaRiskWeight) {

    // set a default value for the cube interpretation object if it is NULL
    if (!cubeInterpretation_) {
        WLOG("cube interpretation is not set, use regular");
        cubeInterpretation_ = boost::make_shared<RegularCubeInterpretation>();
    }
    boost::shared_ptr<RegularCubeInterpretation> regularCubeInterpretation =
        boost::dynamic_pointer_cast<RegularCubeInterpretation>(cubeInterpretation_);
    bool isRegularCubeStorage = (regularCubeInterpretation != NULL);

    LOG("cube storage is regular: " << isRegularCubeStorage);
    LOG("cube dates: " << cube->dates().size());

    QL_REQUIRE(marginalAllocationLimit > 0.0, "positive allocationLimit expected");

    // check portfolio and cube have the same trade ids, in the same order
    QL_REQUIRE(portfolio->size() == cube_->ids().size(),
               "PostProcess::PostProcess(): portfolio size ("
                   << portfolio->size() << ") does not match cube trade size (" << cube_->ids().size() << ")");
    for (Size i = 0; i < portfolio->size(); ++i) {
        QL_REQUIRE(portfolio->trades()[i]->id() == cube_->ids()[i], "PostProcess::PostProcess(): portfolio trade #"
                                                                        << i << " (id=" << portfolio->trades()[i]->id()
                                                                        << ") does not match cube trade id ("
                                                                        << cube_->ids()[i]);
    }

    if (analytics_["dynamicCredit"]) {
        QL_REQUIRE(cptyCube_, "cptyCube cannot be null when dynamicCredit is ON");
        // check portfolio and cptyCube have the same counterparties, in the same order
        QL_REQUIRE(portfolio->counterparties().size() + 1 == cptyCube_->ids().size(),
                   "PostProcess::PostProcess(): portfolio counterparty size ("
                   << portfolio->counterparties().size() << ") does not match cpty cube trade size ("
                   << cptyCube_->ids().size() << ")");
        for (Size i = 0; i < portfolio->counterparties().size(); ++i) {
            QL_REQUIRE(portfolio->counterparties()[i] == cptyCube_->ids()[i],
                       "PostProcess::PostProcess(): portfolio counterparty #"
                       << i << " (id=" << portfolio->counterparties()[i]
                       << ") does not match cube name id ("
                       << cptyCube_->ids()[i]);
        }
        QL_REQUIRE(dvaName == cptyCube_->ids().back(),
                       "PostProcess::PostProcess(): dvaName (" << dvaName
                       << ") does not match cube name id ("
                       << cptyCube_->ids().back());
    }

    ExposureAllocator::AllocationMethod allocationMethod = parseAllocationMethod(allocMethod);

    /***********************************************
     * Step 0: Netting as of today
     * a) Compute the netting set NPV as of today
     * b) Find the final maturity of the netting set
     */
    LOG("Compute netting set NPVs as of today and netting set maturity");
    // Don't use Settings::instance().evaluationDate() here, this has moved to simulation end date.
    Date today = market->asofDate();
    LOG("AsOfDate = " << QuantLib::io::iso_date(today));

    /***************************************************************
     * Step 1: Dynamic Initial Margin calculation
     * Fills DIM cube per netting set that can be
     * - returned to be further analysed
     * - used in collateral calculation
     * - used in MVA calculation
     */
    if (analytics_["dim"] || analytics_["mva"]) {
        QL_REQUIRE(dimCalculator_, "DIM calculator not set");
        dimCalculator_->build();
    }

    /************************************************************
     * Step 2: Trade Exposure and Netting
     * a) Aggregation across scenarios per trade and date
     *    This yields single trade exposure profiles, EPE and ENE
     * b) Aggregation of NPVs within netting sets per date
     *    and scenario. This prepares the netting set exposure
     *    calculation below
     */
    exposureCalculator_ =
        boost::make_shared<ExposureCalculator>(
            portfolio, cube_, cubeInterpretation_,
            market_, analytics_["exerciseNextBreak"], baseCurrency_, configuration_,
            quantile_, calcType_, analytics_["dynamicCredit"], analytics_["flipViewXVA"]
        );
    exposureCalculator_->build();

    /******************************************************************
     * Step 3: Netting set exposure and allocation to trades
     *
     * a) Compute all netting set exposure profiles EPE and ENE using
     *    collateral if CSAs are given and active.
     * b) Compute the expected collateral balance for each netting set.
     * c) Allocate each netting set's exposure profile to the trade
     *    level such that the trade exposures add up to the netting
     *    set exposure.
     *    Reference:
     *    Michael Pykhtin & Dan Rosen, Pricing Counterparty Risk
     *    at the Trade Level and CVA Allocations, October 2010
     */
    nettedExposureCalculator_ =
        boost::make_shared<NettedExposureCalculator>(
            portfolio_, market_, cube_, baseCurrency, configuration_, quantile_,
            calcType_, analytics_["dynamicCredit"], nettingSetManager_,
	        exposureCalculator_->nettingSetDefaultValue(),
	        exposureCalculator_->nettingSetCloseOutValue(),
            scenarioData_, cubeInterpretation_, analytics_["dim"],
            dimCalculator_, fullInitialCollateralisation_,
            allocationMethod == ExposureAllocator::AllocationMethod::Marginal, marginalAllocationLimit,
            exposureCalculator_->exposureCube(), ExposureCalculator::allocatedEPE, ExposureCalculator::allocatedENE,
            analytics_["flipViewXVA"]
        );
    nettedExposureCalculator_->build();

    /********************************************************
     * Update Stand Alone XVAs
     * needed for some of the simple allocation methods below
     */
    if (analytics_["dynamicCredit"]) {
        cvaCalculator_ = boost::make_shared<DynamicCreditXvaCalculator>(
            portfolio_, market_, configuration_,baseCurrency_, dvaName_,
            fvaBorrowingCurve_, fvaLendingCurve_, analytics_["dim"],
            dimCalculator, exposureCalculator_->exposureCube(),
            nettedExposureCalculator_->exposureCube(), cptyCube_,
            ExposureCalculator::ExposureIndex::EPE,
            ExposureCalculator::ExposureIndex::ENE,
            NettedExposureCalculator::ExposureIndex::EPE,
            NettedExposureCalculator::ExposureIndex::ENE, 0, analytics_["flipViewXVA"],
            flipViewBorrowingCurvePostfix, flipViewLendingCurvePostfix);
    } else {
        cvaCalculator_ = boost::make_shared<StaticCreditXvaCalculator>(
            portfolio_, market_, configuration_,baseCurrency_, dvaName_,
            fvaBorrowingCurve_, fvaLendingCurve_, analytics_["dim"],
            dimCalculator, exposureCalculator_->exposureCube(),
            nettedExposureCalculator_->exposureCube(),
            ExposureCalculator::ExposureIndex::EPE,
            ExposureCalculator::ExposureIndex::ENE,
            NettedExposureCalculator::ExposureIndex::EPE,
            NettedExposureCalculator::ExposureIndex::ENE, analytics_["flipViewXVA"],
            flipViewBorrowingCurvePostfix, flipViewLendingCurvePostfix);
    }
    cvaCalculator_->build();

    /***************************
     * Simple allocation methods
     */
    boost::shared_ptr<ExposureAllocator> exposureAllocator;
    if (allocationMethod == ExposureAllocator::AllocationMethod::Marginal) {
        DLOG("Marginal Calculation handled in NettedExposureCalculator");
    }
    else if (allocationMethod == ExposureAllocator::AllocationMethod::RelativeFairValueNet)
        exposureAllocator = boost::make_shared<RelativeFairValueNetExposureAllocator>(
            portfolio, exposureCalculator_->exposureCube(),
            nettedExposureCalculator_->exposureCube(), cube_,
            ExposureCalculator::allocatedEPE, ExposureCalculator::allocatedENE,
            ExposureCalculator::EPE, ExposureCalculator::ENE,
            NettedExposureCalculator::EPE, NettedExposureCalculator::ENE);
    else if (allocationMethod == ExposureAllocator::AllocationMethod::RelativeFairValueGross)
        exposureAllocator = boost::make_shared<RelativeFairValueGrossExposureAllocator>(
            portfolio, exposureCalculator_->exposureCube(),
            nettedExposureCalculator_->exposureCube(), cube_,
            ExposureCalculator::allocatedEPE, ExposureCalculator::allocatedENE,
            ExposureCalculator::EPE, ExposureCalculator::ENE,
            NettedExposureCalculator::EPE, NettedExposureCalculator::ENE);
    else if (allocationMethod == ExposureAllocator::AllocationMethod::RelativeXVA)
        exposureAllocator = boost::make_shared<RelativeXvaExposureAllocator>(
            portfolio, exposureCalculator_->exposureCube(),
            nettedExposureCalculator_->exposureCube(), cube_,
            cvaCalculator_->tradeCva(), cvaCalculator_->tradeDva(),
            cvaCalculator_->nettingSetSumCva(), cvaCalculator_->nettingSetSumDva(),
            ExposureCalculator::allocatedEPE, ExposureCalculator::allocatedENE,
            ExposureCalculator::EPE, ExposureCalculator::ENE,
            NettedExposureCalculator::EPE, NettedExposureCalculator::ENE);
    else if (allocationMethod == ExposureAllocator::AllocationMethod::None)
        exposureAllocator = boost::make_shared<NoneExposureAllocator>(
            portfolio, exposureCalculator_->exposureCube(),
            nettedExposureCalculator_->exposureCube());
    else
        QL_FAIL("allocationMethod " << allocationMethod << " not available");
    if(exposureAllocator)
        exposureAllocator->build();

    /********************************************************
     * Update Allocated XVAs
     */
    if (analytics_["dynamicCredit"]) {
        allocatedCvaCalculator_ = boost::make_shared<DynamicCreditXvaCalculator>(
            portfolio_, market_, configuration_, baseCurrency_, dvaName_, fvaBorrowingCurve_, fvaLendingCurve_,
            analytics_["dim"], dimCalculator, exposureCalculator_->exposureCube(),
            nettedExposureCalculator_->exposureCube(), cptyCube_, ExposureCalculator::ExposureIndex::allocatedEPE,
            ExposureCalculator::ExposureIndex::allocatedENE, NettedExposureCalculator::ExposureIndex::EPE,
            NettedExposureCalculator::ExposureIndex::ENE, 0, analytics_["flipViewXVA"], flipViewBorrowingCurvePostfix,
            flipViewLendingCurvePostfix);
    } else {
        allocatedCvaCalculator_ = boost::make_shared<StaticCreditXvaCalculator>(
            portfolio_, market_, configuration_, baseCurrency_, dvaName_, fvaBorrowingCurve_, fvaLendingCurve_,
            analytics_["dim"], dimCalculator, exposureCalculator_->exposureCube(),
            nettedExposureCalculator_->exposureCube(), ExposureCalculator::ExposureIndex::allocatedEPE,
            ExposureCalculator::ExposureIndex::allocatedENE, NettedExposureCalculator::ExposureIndex::EPE,
            NettedExposureCalculator::ExposureIndex::ENE, analytics_["flipViewXVA"], flipViewBorrowingCurvePostfix,
            flipViewLendingCurvePostfix);
    }
    allocatedCvaCalculator_->build();

    /********************************************************
     * Cache average EPE and ENE
     */
    for (auto tradeId : tradeIds()) {
        tradeEPE_[tradeId] = exposureCalculator_->epe(tradeId);
        tradeENE_[tradeId] = exposureCalculator_->ene(tradeId);
        allocatedTradeEPE_[tradeId] = exposureCalculator_->allocatedEpe(tradeId);
        allocatedTradeENE_[tradeId] = exposureCalculator_->allocatedEne(tradeId);
    }
    for (auto nettingSetId : nettingSetIds()) {
        netEPE_[nettingSetId] = nettedExposureCalculator_->epe(nettingSetId);
        netENE_[nettingSetId] = nettedExposureCalculator_->ene(nettingSetId);
    }

    /********************************************************
     * Calculate netting set KVA-CCR and KVA-CVA
     */
    updateNettingSetKVA();

    /***************************************
     * Calculate netting set CVA sensitivity
     */
    updateNettingSetCvaSensitivity();
}

void PostProcess::updateNettingSetKVA() {

    // Loop over all netting sets
    for (auto nettingSetId : nettingSetIds()) {
        // Init results
        ourNettingSetKVACCR_[nettingSetId] = 0.0;
        theirNettingSetKVACCR_[nettingSetId] = 0.0;
        ourNettingSetKVACVA_[nettingSetId] = 0.0;
        theirNettingSetKVACVA_[nettingSetId] = 0.0;
    }

    if (!analytics_["kva"])
        return;

    LOG("Update netting set KVA");

    vector<Date> dateVector = cube_->dates();
    Size dates = dateVector.size();
    Date today = market_->asofDate();
    Handle<YieldTermStructure> discountCurve = market_->discountCurve(baseCurrency_, configuration_);
    DayCounter dc = ActualActual();

    // Loop over all netting sets
    for (auto nettingSetId : nettingSetIds()) {
        string cid;
        if (analytics_["flipViewXVA"]) {
            cid = dvaName_;
        } else {
            cid = nettedExposureCalculator_->counterparty(nettingSetId);
        }
        LOG("KVA for netting set " << nettingSetId);

        // Main input are the EPE and ENE profiles, previously computed
        vector<Real> epe = netEPE_[nettingSetId];
        vector<Real> ene = netENE_[nettingSetId];

        // PD from counterparty Dts, floored to avoid 0 ...
        // Today changed to today+1Y to get the one-year PD
        Handle<DefaultProbabilityTermStructure> cvaDts = market_->defaultCurve(cid, configuration_);
        QL_REQUIRE(!cvaDts.empty(), "Default curve missing for counterparty " << cid);
        Real cvaRR = market_->recoveryRate(cid, configuration_)->value();
        Real PD1 = std::max(cvaDts->defaultProbability(today + 1 * Years), 0.000000000001);
        Real LGD1 = (1 - cvaRR);

        // FIXME: if flipViewXVA is sufficient, then all code for their KVA-CCR could be discarded here...
        Handle<DefaultProbabilityTermStructure> dvaDts;
        Real dvaRR = 0.0;
        Real PD2 = 0;
        if (analytics_["flipViewXVA"]) {
            dvaName_ = nettedExposureCalculator_->counterparty(nettingSetId);
        }
        if (dvaName_ != "") {
            dvaDts = market_->defaultCurve(dvaName_, configuration_);
            dvaRR = market_->recoveryRate(dvaName_, configuration_)->value();
            PD2 = std::max(dvaDts->defaultProbability(today + 1 * Years), 0.000000000001);
        } else {
            ALOG("dvaName not specified, own PD set to zero for their KVA calculation");
        }
        Real LGD2 = (1 - dvaRR);

        // Granularity adjustment, Gordy (2004):
        Real rho1 = 0.12 * (1 - std::exp(-50 * PD1)) / (1 - std::exp(-50)) +
                    0.24 * (1 - (1 - std::exp(-50 * PD1)) / (1 - std::exp(-50)));
        Real rho2 = 0.12 * (1 - std::exp(-50 * PD2)) / (1 - std::exp(-50)) +
                    0.24 * (1 - (1 - std::exp(-50 * PD2)) / (1 - std::exp(-50)));

        // Basel II internal rating based (IRB) estimate of worst case PD:
        // Large homogeneous pool (LHP) approximation of Vasicek (1997)
        InverseCumulativeNormal icn;
        CumulativeNormalDistribution cnd;
        Real PD99_1 = cnd((icn(PD1) + std::sqrt(rho1) * icn(0.999)) / (std::sqrt(1 - rho1))) - PD1;
        Real PD99_2 = cnd((icn(PD2) + std::sqrt(rho2) * icn(0.999)) / (std::sqrt(1 - rho2))) - PD2;

        // KVA regulatory PD, worst case PD, floored at 0.03 for corporates and banks, not floored for sovereigns
        Real kva99PD1 = std::max(PD99_1, kvaTheirPdFloor_);
        Real kva99PD2 = std::max(PD99_2, kvaOurPdFloor_);

        // Factor B(PD) for the maturity adjustment factor, B(PD) = (0.11852 - 0.05478 * ln(PD)) ^ 2
        Real kvaMatAdjB1 = std::pow((0.11852 - 0.05478 * std::log(PD1)), 2.0);
        Real kvaMatAdjB2 = std::pow((0.11852 - 0.05478 * std::log(PD2)), 2.0);

        DLOG("Our KVA-CCR " << nettingSetId << ": PD=" << PD1);
        DLOG("Our KVA-CCR " << nettingSetId << ": LGD=" << LGD1);
        DLOG("Our KVA-CCR " << nettingSetId << ": rho=" << rho1);
        DLOG("Our KVA-CCR " << nettingSetId << ": PD99=" << PD99_1);
        DLOG("Our KVA-CCR " << nettingSetId << ": PD Floor=" << kvaTheirPdFloor_);
        DLOG("Our KVA-CCR " << nettingSetId << ": Floored PD99=" << kva99PD1);
        DLOG("Our KVA-CCR " << nettingSetId << ": B(PD)=" << kvaMatAdjB1);

        DLOG("Their KVA-CCR " << nettingSetId << ": PD=" << PD2);
        DLOG("Their KVA-CCR " << nettingSetId << ": LGD=" << LGD2);
        DLOG("Their KVA-CCR " << nettingSetId << ": rho=" << rho2);
        DLOG("Their KVA-CCR " << nettingSetId << ": PD99=" << PD99_2);
        DLOG("Their KVA-CCR " << nettingSetId << ": PD Floor=" << kvaOurPdFloor_);
        DLOG("Their KVA-CCR " << nettingSetId << ": Floored PD99=" << kva99PD2);
        DLOG("Their KVA-CCR " << nettingSetId << ": B(PD)=" << kvaMatAdjB2);

        for (Size j = 0; j < dates; ++j) {
            Date d0 = j == 0 ? today : cube_->dates()[j - 1];
            Date d1 = cube_->dates()[j];

            // Preprocess:
            // 1) Effective maturity from effective expected exposure as of time j
            //    Index _1 corresponds to our perspective, index _2 to their perspective.
            // 2) Basel EEPE as of time j, i.e. as time averge over EEE, starting at time j
            // More accuracy may be achieved here by using a Longstaff-Schwartz method / regression
            Real eee_kva_1 = 0.0, eee_kva_2 = 0.0;
            Real effMatNumer1 = 0.0, effMatNumer2 = 0.0;
            Real effMatDenom1 = 0.0, effMatDenom2 = 0.0;
            Real eepe_kva_1 = 0, eepe_kva_2 = 0.0;
            Size kmax = j, count = 0;
            // Cut off index for EEPE/EENE calculation: One year ahead
            while (dateVector[kmax] < dateVector[j] + 1 * Years + 4 * Days && kmax < dates - 1)
                kmax++;
            Real sumdt = 0.0, eee1_b = 0.0, eee2_b = 0.0;
            for (Size k = j; k < dates; ++k) {
                Date d2 = cube_->dates()[k];
                Date prevDate = k == 0 ? today : dateVector[k - 1];

                eee_kva_1 = std::max(eee_kva_1, epe[k + 1]);
                eee_kva_2 = std::max(eee_kva_2, ene[k + 1]);

                // Components of the KVA maturity adjustment MA as of time j
                if (dc.yearFraction(d1, d2) > 1.0) {
                    effMatNumer1 += epe[k + 1] * dc.yearFraction(prevDate, d2);
                    effMatNumer2 += ene[k + 1] * dc.yearFraction(prevDate, d2);
                }
                if (dc.yearFraction(d1, d2) <= 1.0) {
                    effMatDenom1 += eee_kva_1 * dc.yearFraction(prevDate, d2);
                    effMatDenom2 += eee_kva_2 * dc.yearFraction(prevDate, d2);
                }

                if (k < kmax) {
                    Real dt = dc.yearFraction(cube_->dates()[k], cube_->dates()[k + 1]);
                    sumdt += dt;
                    Real epe_b = epe[k + 1] / discountCurve->discount(dateVector[k]);
                    Real ene_b = ene[k + 1] / discountCurve->discount(dateVector[k]);
                    eee1_b = std::max(epe_b, eee1_b);
                    eee2_b = std::max(ene_b, eee2_b);
                    eepe_kva_1 += eee1_b * dt;
                    eepe_kva_2 += eee2_b * dt;
                    count++;
                }
            }

            // Normalize EEPE/EENE calculation
            eepe_kva_1 = count > 0 ? eepe_kva_1 / sumdt : 0.0;
            eepe_kva_2 = count > 0 ? eepe_kva_2 / sumdt : 0.0;

            // KVA CCR using the IRB risk weighted asset method and IMM:
            // KVA effective maturity of the nettingSet, capped at 5
            Real kvaNWMaturity1 = std::min(1.0 + (effMatDenom1 == 0.0 ? 0.0 : effMatNumer1 / effMatDenom1), 5.0);
            Real kvaNWMaturity2 = std::min(1.0 + (effMatDenom2 == 0.0 ? 0.0 : effMatNumer2 / effMatDenom2), 5.0);

            // Maturity adjustment factor for the RWA method:
            // MA(PD, M) = (1 + (M - 2.5) * B(PD)) / (1 - 1.5 * B(PD)), capped at 5, floored at 1, M = effective
            // maturity
            Real kvaMatAdj1 =
                std::max(std::min((1.0 + (kvaNWMaturity1 - 2.5) * kvaMatAdjB1) / (1.0 - 1.5 * kvaMatAdjB1), 5.0), 1.0);
            Real kvaMatAdj2 =
                std::max(std::min((1.0 + (kvaNWMaturity2 - 2.5) * kvaMatAdjB2) / (1.0 - 1.5 * kvaMatAdjB2), 5.0), 1.0);

            // CCR Capital: RC = EAD x LGD x PD99.9 x MA(PD, M); EAD = alpha x EEPE(t) (approximated by EPE here);
            Real kvaRC1 = kvaAlpha_ * eepe_kva_1 * LGD1 * kva99PD1 * kvaMatAdj1;
            Real kvaRC2 = kvaAlpha_ * eepe_kva_2 * LGD2 * kva99PD2 * kvaMatAdj2;

            // Expected risk capital discounted at capital discount rate
            Real kvaCapitalDiscount = 1 / std::pow(1 + kvaCapitalDiscountRate_, dc.yearFraction(today, d0));
            Real kvaCCRIncrement1 =
                kvaRC1 * kvaCapitalDiscount * dc.yearFraction(d0, d1) * kvaCapitalHurdle_ * kvaRegAdjustment_;
            Real kvaCCRIncrement2 =
                kvaRC2 * kvaCapitalDiscount * dc.yearFraction(d0, d1) * kvaCapitalHurdle_ * kvaRegAdjustment_;

            ourNettingSetKVACCR_[nettingSetId] += kvaCCRIncrement1;
            theirNettingSetKVACCR_[nettingSetId] += kvaCCRIncrement2;

            DLOG("Our KVA-CCR for " << nettingSetId << ": " << j << " EEPE=" << setprecision(2) << eepe_kva_1
                                    << " EPE=" << epe[j] << " RC=" << kvaRC1 << " M=" << setprecision(6)
                                    << kvaNWMaturity1 << " MA=" << kvaMatAdj1 << " Cost=" << setprecision(2)
                                    << kvaCCRIncrement1 << " KVA=" << ourNettingSetKVACCR_[nettingSetId]);
            DLOG("Their KVA-CCR for " << nettingSetId << ": " << j << " EENE=" << eepe_kva_2 << " ENE=" << ene[j]
                                      << " RC=" << kvaRC2 << " M=" << setprecision(6) << kvaNWMaturity2
                                      << " MA=" << kvaMatAdj2 << " Cost=" << setprecision(2) << kvaCCRIncrement2
                                      << " KVA=" << theirNettingSetKVACCR_[nettingSetId]);

            // CVA Capital
            // effective maturity without cap at 5, DF set to 1 for IMM banks
            // TODO: Set MA in CCR capital calculation to 1
            Real kvaCvaMaturity1 = 1.0 + (effMatDenom1 == 0.0 ? 0.0 : effMatNumer1 / effMatDenom1);
            Real kvaCvaMaturity2 = 1.0 + (effMatDenom2 == 0.0 ? 0.0 : effMatNumer2 / effMatDenom2);
            Real scva1 = kvaTheirCvaRiskWeight_ * kvaCvaMaturity1 * eepe_kva_1;
            Real scva2 = kvaOurCvaRiskWeight_ * kvaCvaMaturity2 * eepe_kva_2;
            Real kvaCVAIncrement1 =
                scva1 * kvaCapitalDiscount * dc.yearFraction(d0, d1) * kvaCapitalHurdle_ * kvaRegAdjustment_;
            Real kvaCVAIncrement2 =
                scva2 * kvaCapitalDiscount * dc.yearFraction(d0, d1) * kvaCapitalHurdle_ * kvaRegAdjustment_;

            DLOG("Our KVA-CVA for " << nettingSetId << ": " << j << " EEPE=" << eepe_kva_1 << " SCVA=" << scva1
                                    << " Cost=" << kvaCVAIncrement1);
            DLOG("Their KVA-CVA for " << nettingSetId << ": " << j << " EENE=" << eepe_kva_2 << " SCVA=" << scva2
                                      << " Cost=" << kvaCVAIncrement2);

            ourNettingSetKVACVA_[nettingSetId] += kvaCVAIncrement1;
            theirNettingSetKVACVA_[nettingSetId] += kvaCVAIncrement2;
        }
    }

    LOG("Update netting set KVA done");
}

void PostProcess::updateNettingSetCvaSensitivity() {

    if (!analytics_["cvaSensi"])
        return;

    LOG("Update netting set CVA sensitvities");

    Handle<YieldTermStructure> discountCurve = market_->discountCurve(baseCurrency_, configuration_);

    for (auto n : netEPE_) {
        string nettingSetId = n.first;
        vector<Real> epe = netEPE_[nettingSetId];
        string cid;
        Handle<DefaultProbabilityTermStructure> cvaDts;
        Real cvaRR;
        if (analytics_["flipViewXVA"]) {
            cid = dvaName_;
        } else {
            cid = nettedExposureCalculator_->counterparty(nettingSetId);
        }
        cvaDts = market_->defaultCurve(cid);
        QL_REQUIRE(!cvaDts.empty(), "Default curve missing for counterparty " << cid);
        cvaRR = market_->recoveryRate(cid, configuration_)->value();

	    bool cvaSensi = analytics_["cvaSensi"];
	    LOG("CVA Sensitivity: " << cvaSensi);
	    if (cvaSensi) {
	        boost::shared_ptr<CVASpreadSensitivityCalculator> cvaSensiCalculator = boost::make_shared<CVASpreadSensitivityCalculator>(
	             nettingSetId, market_->asofDate(), epe, cube_->dates(), cvaDts, cvaRR, discountCurve, cvaSpreadSensiGrid_, cvaSpreadSensiShiftSize_);

	        for (Size i = 0; i < cvaSensiCalculator->shiftTimes().size(); ++i) {
	            DLOG("CVA Sensi Calculator: t=" << cvaSensiCalculator->shiftTimes()[i]
		         << " h=" << cvaSensiCalculator->hazardRateSensitivities()[i]
		         << " s=" << cvaSensiCalculator->cdsSpreadSensitivities()[i]);
	        }

	        netCvaHazardRateSensi_[nettingSetId] = cvaSensiCalculator->hazardRateSensitivities();
	        netCvaSpreadSensi_[nettingSetId] = cvaSensiCalculator->cdsSpreadSensitivities();
	        cvaSpreadSensiTimes_ = cvaSensiCalculator->shiftTimes();
	    } else {
	        cvaSpreadSensiTimes_ = vector<Real>();
	        netCvaHazardRateSensi_[nettingSetId] = vector<Real>();
	        netCvaSpreadSensi_[nettingSetId] = vector<Real>();
	    }
    }

    LOG("Update netting set CVA sensitivities done");
}

const vector<Real>& PostProcess::tradeEPE(const string& tradeId) {
    QL_REQUIRE(tradeEPE_.find(tradeId) != tradeEPE_.end(), "Trade " << tradeId << " not found in exposure map");
    return tradeEPE_[tradeId];
}

const vector<Real>& PostProcess::tradeENE(const string& tradeId) {
    QL_REQUIRE(tradeENE_.find(tradeId) != tradeENE_.end(), "Trade " << tradeId << " not found in exposure map");
    return tradeENE_[tradeId];
}

const vector<Real>& PostProcess::tradeEE_B(const string& tradeId) {
    return exposureCalculator_->ee_b(tradeId);
}

const Real& PostProcess::tradeEPE_B(const string& tradeId) {
    return exposureCalculator_->epe_b(tradeId);
}

const vector<Real>& PostProcess::tradeEEE_B(const string& tradeId) {
    return exposureCalculator_->eee_b(tradeId);
}

const Real& PostProcess::tradeEEPE_B(const string& tradeId) {
    return exposureCalculator_->eepe_b(tradeId);
}

const vector<Real>& PostProcess::tradePFE(const string& tradeId) {
    return exposureCalculator_->pfe(tradeId);
}

const vector<Real>& PostProcess::netEPE(const string& nettingSetId) {
    QL_REQUIRE(netEPE_.find(nettingSetId) != netEPE_.end(),
               "Netting set " << nettingSetId << " not found in exposure map");
    return netEPE_[nettingSetId];
}

const vector<Real>& PostProcess::netENE(const string& nettingSetId) {
    QL_REQUIRE(netENE_.find(nettingSetId) != netENE_.end(),
               "Netting set " << nettingSetId << " not found in exposure map");
    return netENE_[nettingSetId];
}

vector<Real> PostProcess::netCvaHazardRateSensitivity(const string& nettingSetId) {
    if (netCvaHazardRateSensi_.find(nettingSetId) != netCvaHazardRateSensi_.end())
        return netCvaHazardRateSensi_[nettingSetId];
    else
        return vector<Real>();
}

vector<Real> PostProcess::netCvaSpreadSensitivity(const string& nettingSetId) {
    if (netCvaSpreadSensi_.find(nettingSetId) != netCvaSpreadSensi_.end())
        return netCvaSpreadSensi_[nettingSetId];
    else
        return vector<Real>();
}

const vector<Real>& PostProcess::netEE_B(const string& nettingSetId) {
    return nettedExposureCalculator_->ee_b(nettingSetId);
}

const Real& PostProcess::netEPE_B(const string& nettingSetId) {
    return nettedExposureCalculator_->epe_b(nettingSetId);
}

const vector<Real>& PostProcess::netEEE_B(const string& nettingSetId) {
    return nettedExposureCalculator_->eee_b(nettingSetId);
}

const Real& PostProcess::netEEPE_B(const string& nettingSetId) {
    return nettedExposureCalculator_->eepe_b(nettingSetId);
}

const vector<Real>& PostProcess::netPFE(const string& nettingSetId) {
    return nettedExposureCalculator_->pfe(nettingSetId);
}

const vector<Real>& PostProcess::expectedCollateral(const string& nettingSetId) {
    return nettedExposureCalculator_->expectedCollateral(nettingSetId);
}

const vector<Real>& PostProcess::colvaIncrements(const string& nettingSetId) {
    return nettedExposureCalculator_->colvaIncrements(nettingSetId);
}

const vector<Real>& PostProcess::collateralFloorIncrements(const string& nettingSetId) {
    return nettedExposureCalculator_->collateralFloorIncrements(nettingSetId);
}

const vector<Real>& PostProcess::allocatedTradeEPE(const string& tradeId) {
    QL_REQUIRE(allocatedTradeEPE_.find(tradeId) != allocatedTradeEPE_.end(),
               "Trade " << tradeId << " not found in exposure map");
    return allocatedTradeEPE_[tradeId];
}

const vector<Real>& PostProcess::allocatedTradeENE(const string& tradeId) {
    QL_REQUIRE(allocatedTradeENE_.find(tradeId) != allocatedTradeENE_.end(),
               "Trade " << tradeId << " not found in exposure map");
    return allocatedTradeENE_[tradeId];
}

Real PostProcess::tradeCVA(const string& tradeId) {
    return cvaCalculator_->tradeCva(tradeId);
}

Real PostProcess::tradeDVA(const string& tradeId) {
    return cvaCalculator_->tradeDva(tradeId);
}

Real PostProcess::tradeMVA(const string& tradeId) {
    return cvaCalculator_->tradeMva(tradeId);
}

Real PostProcess::tradeFBA(const string& tradeId) {
    return cvaCalculator_->tradeFba(tradeId);
}

Real PostProcess::tradeFCA(const string& tradeId) {
    return cvaCalculator_->tradeFca(tradeId);
}

Real PostProcess::tradeFBA_exOwnSP(const string& tradeId) {
    return cvaCalculator_->tradeFba_exOwnSp(tradeId);
}

Real PostProcess::tradeFCA_exOwnSP(const string& tradeId) {
    return cvaCalculator_->tradeFca_exOwnSp(tradeId);
}

Real PostProcess::tradeFBA_exAllSP(const string& tradeId) {
    return cvaCalculator_->tradeFba_exAllSp(tradeId);
}

Real PostProcess::tradeFCA_exAllSP(const string& tradeId) {
    return cvaCalculator_->tradeFca_exAllSp(tradeId);
}

Real PostProcess::nettingSetCVA(const string& nettingSetId) {
    return cvaCalculator_->nettingSetCva(nettingSetId);
}

Real PostProcess::nettingSetDVA(const string& nettingSetId) {
    return cvaCalculator_->nettingSetDva(nettingSetId);
}

Real PostProcess::nettingSetMVA(const string& nettingSetId) {
    return cvaCalculator_->nettingSetMva(nettingSetId);

}

Real PostProcess::nettingSetFBA(const string& nettingSetId) {
    return cvaCalculator_->nettingSetFba(nettingSetId);

}

Real PostProcess::nettingSetFCA(const string& nettingSetId) {
    return cvaCalculator_->nettingSetFca(nettingSetId);

}

Real PostProcess::nettingSetOurKVACCR(const string& nettingSetId) {
    QL_REQUIRE(ourNettingSetKVACCR_.find(nettingSetId) != ourNettingSetKVACCR_.end(),
               "NettingSetId " << nettingSetId << " not found in nettingSet KVACCR map");
    return ourNettingSetKVACCR_[nettingSetId];
}

Real PostProcess::nettingSetTheirKVACCR(const string& nettingSetId) {
    QL_REQUIRE(theirNettingSetKVACCR_.find(nettingSetId) != theirNettingSetKVACCR_.end(),
               "NettingSetId " << nettingSetId << " not found in nettingSet KVACCR map");
    return theirNettingSetKVACCR_[nettingSetId];
}

Real PostProcess::nettingSetOurKVACVA(const string& nettingSetId) {
    QL_REQUIRE(ourNettingSetKVACVA_.find(nettingSetId) != ourNettingSetKVACVA_.end(),
               "NettingSetId " << nettingSetId << " not found in nettingSet KVACVA map");
    return ourNettingSetKVACVA_[nettingSetId];
}

Real PostProcess::nettingSetTheirKVACVA(const string& nettingSetId) {
    QL_REQUIRE(theirNettingSetKVACVA_.find(nettingSetId) != theirNettingSetKVACVA_.end(),
               "NettingSetId " << nettingSetId << " not found in nettingSet KVACVA map");
    return theirNettingSetKVACVA_[nettingSetId];
}

Real PostProcess::nettingSetFBA_exOwnSP(const string& nettingSetId) {
    return cvaCalculator_->nettingSetFba_exOwnSp(nettingSetId);
}

Real PostProcess::nettingSetFCA_exOwnSP(const string& nettingSetId) {
    return cvaCalculator_->nettingSetFca_exOwnSp(nettingSetId);
}

Real PostProcess::nettingSetFBA_exAllSP(const string& nettingSetId) {
    return cvaCalculator_->nettingSetFba_exAllSp(nettingSetId);
}

Real PostProcess::nettingSetFCA_exAllSP(const string& nettingSetId) {
    return cvaCalculator_->nettingSetFca_exAllSp(nettingSetId);
}

Real PostProcess::allocatedTradeCVA(const string& allocatedTradeId) {
    return allocatedCvaCalculator_->tradeCva(allocatedTradeId);
}

Real PostProcess::allocatedTradeDVA(const string& allocatedTradeId) {
    return allocatedCvaCalculator_->tradeDva(allocatedTradeId);
}

Real PostProcess::nettingSetCOLVA(const string& nettingSetId) {
    return nettedExposureCalculator_->colva(nettingSetId);
}

Real PostProcess::nettingSetCollateralFloor(const string& nettingSetId) {
    return nettedExposureCalculator_->collateralFloor(nettingSetId);
}

void PostProcess::exportDimEvolution(ore::data::Report& dimEvolutionReport) {
    dimCalculator_->exportDimEvolution(dimEvolutionReport);
}

void PostProcess::exportDimRegression(const std::string& nettingSet, const std::vector<Size>& timeSteps,
                                      const std::vector<boost::shared_ptr<ore::data::Report>>& dimRegReports) {

    boost::shared_ptr<RegressionDynamicInitialMarginCalculator> regCalc =
        boost::dynamic_pointer_cast<RegressionDynamicInitialMarginCalculator>(dimCalculator_);

    if (regCalc)
        regCalc->exportDimRegression(nettingSet, timeSteps, dimRegReports);
}

} // namespace analytics
} // namespace ore


/**
 * Copyright (c) 2011-2013 Andreas Sembrant
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *  - Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  - Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *  - Neither the name of the copyright holders nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * Authors: Andreas Sembrant
 *
 */

#ifndef __SCARPHASE_INTERNAL_UTIL_RANDOM_PROJECTION_HPP
#define __SCARPHASE_INTERNAL_UTIL_RANDOM_PROJECTION_HPP

#include <stdint.h>
#include <boost/array.hpp>

namespace scarphase {
namespace internal {
namespace util {

/**
 * @brief Random projection to reduce the dimentioality of a large vector
 *        space.
 *
 *  V[1,n] * M[n,m] = V[1,m]
 *
 */
template<typename value_type,
         int      OUTPUT_SIZE>
class RandomProjection //-----------------------------------------------------//
{

public: //--------------------------------------------------------------------//

    /**
     * @brief C-tor.
     */
    RandomProjection();

    /**
     * @brief Hash a value using random projection.
     *        y = x * M, where y << x
     *
     * @param[in]  x Value to hash.
     * @returns A hash of x.
     *
     */
    value_type Hash(value_type x);

private: //-------------------------------------------------------------------//

    /**
     * @brief The size of the input variable.
     */
    #define INPUT_SIZE int(sizeof(value_type) * 8)

    /**
     * @brief Random projection matrix.
     */
    boost::array< boost::array< uint8_t, INPUT_SIZE >, OUTPUT_SIZE > m;

};

} /* namespace util */
} /* namespace internal */
} /* namespace scarphase */

//----------------------------------------------------------------------------//
// Inline Definitions                                                         //
//----------------------------------------------------------------------------//

#include <cstdlib>
#include <boost/random.hpp>
#include <boost/static_assert.hpp>

namespace scarphase {
namespace internal {
namespace util {

template<typename value_type,
         int      OUTPUT_SIZE>
RandomProjection<value_type, OUTPUT_SIZE>::RandomProjection()
{
    BOOST_STATIC_ASSERT(OUTPUT_SIZE > 0);

    boost::mt19937 rng;
    boost::uniform_int<> bin(0,1);
    boost::variate_generator< boost::mt19937&,
                              boost::uniform_int<> > die(rng, bin);

    for (int i = 0; i < OUTPUT_SIZE; i++)
    {
        for (int j = 0; j < INPUT_SIZE; j++)
        {
            m[i][j] = die(); //std::rand() % 2;
        }
    }

}

template<typename value_type,
         int      OUTPUT_SIZE>
inline value_type
RandomProjection<value_type, OUTPUT_SIZE>::Hash(value_type x)
{
    value_type result = 0;
    for (int i = 0; i < OUTPUT_SIZE; i++)
    {
        int t = 0;
        for (int j = 0; j < INPUT_SIZE; j++)
        {
            t += m[i][j] & ((x >> j) & 0x01);
        }
        result |= (t % 2) << i;
    }
    return result;
}

} /* namespace util */
} /* namespace internal */
} /* namespace scarphase */

#endif /* __SCARPHASE_INTERNAL_UTIL_RANDOM_PROJECTION_HPP */