#include <complex>
#include <iostream>
#include <fstream>
#include <string>

#include <boost/regex.hpp>
#include <boost/filesystem.hpp>

#include <Eigen/Core>

#include <vtkDataArraySelection.h>
#include <vtkXMLImageDataReader.h>

#include "FullSampleSimulation.h"
#include "DropletSampleSimulation.h"

using json = nlohmann::json;

void print_usage() {
	std::cout <<
		std::endl <<
		"Usage: ./rt-solver [ -h | -c FILE | -x FILE ]" <<
		std::endl << std::endl <<
		"\t-h" << std::endl <<
		"\t\tPrint this message and exit." <<
		std::endl << std::endl <<
		"\t-c FILE" << std::endl <<
		"\t\tCreate a default settings file with the specified name and exit." <<
		std::endl << std::endl <<
		"\t-x FILE" << std::endl <<
		"\t\tRun the code with the specified settings file." <<
		std::endl << std::endl;
}

std::string minify_json(std::string filename) {

	std::ifstream f(filename);
	if(!f.is_open())
		throw std::string(
			"Could not open the specified setting file");

	boost::regex empty_re("\\s*");
	boost::regex comment_re("\\s*#.*");
	boost::smatch match_result;

	std::string line, minified_json;
	while(std::getline(f, line)) {
		if(!boost::regex_match(line, match_result, empty_re) &&
				!boost::regex_match(line, match_result, comment_re))
			minified_json.append(line+"\n");
	}
	return minified_json;
}

void create_default_settings_file(std::string filename) {

	const char *file_content = R"V0G0N(
{
	"Light source": {
		# Widths (µm) of the light source (array of size [spatial_dim-1]). Should be smaller
		# than the director field mesh in the transverse direction if the sample type is
		# "Full".
		"Source widths": [10, 10],

		# Number of rays per dimension for the light source
		"Source N rays per dim": [200, 200],

		# Mean wavelength (µm) for the spectrum of the source
		"Mean wavelength": 0.6,

		# Full width (µm) of the light spectrum
		"Spectral FWHM": 0.2,

		# Number of wavelengths in the spectrum
		"N wavelengths": 1
	},
	"Geometry": {
		# Type of sample: "Droplet" or "Full". If "Droplet" is choosed, the given vti file for
		# the director field should have a cubic mesh (only values inside a sphere of same
		# diameter than the mesh will be considered).
		"Sample type": "Full",

		# Relative path to a VTI file containing a VTK array "n" for the director values.
		# IMPORTANT: the specified director field should have C_1 regularity (which exclude
		# the presence of defects and n->-n jumps).
		"Director field VTI file": "",

		"Droplet sample parameters": {
			# Since the ray mapping is singular near the vertical part of the droplet boundary,
			# the light source for droplet rays is shrinked by the following factor
			"Source shrink factor": 0.92,

			# Distance (µm) between the boundary of the droplet and the sample plates (the
			# droplet is centered inside the sample).
			"Distance from upper sample plate": 10
		}
	},
	"Material properties": {
		# Ordinary refractive index of the liquid crystal
		"Ordinary refractive index": 1.5,

		# Extraordinary refractive index of the liquid crystal
		"Extraordinary refractive index": 1.6,

		# Refractive index of the host fluid in case of a droplet sample
		"Host fluid refractive index": 1.55,

		# Refractive index of the external medium outside the sample
		"External medium refractive index": 1,

		# Refractive indices of the isotropic layers defining the lower part of the sample
		# (light propagates from bottom to top in the z direction, layers should be specified
		# with increasing z)
		"Refractive indices of the lower isotropic layers": [1.5],

		# Refractive indices of the isotropic layers defining the upper part of the sample
		# (light propagates from bottom to top in the z direction, layers should be specified
		# with increasing z)
		"Refractive indices of the upper isotropic layers": [1.5],

		# Thicknesses of the isotropic layers defining the lower part of the sample
		# (light propagates from bottom to top in the z direction, layers should be specified
		# with increasing z)
		"Thicknesses of the lower isotropic layers": [1000],

		# Thicknesses of the isotropic layers defining the upper part of the sample
		# (light propagates from bottom to top in the z direction, layers should be specified
		# with increasing z)
		"Thicknesses of the upper isotropic layers": [1000]
	},
	"Visualisation": {
		# Results will be stored in this folder (automatically created if it does not exists)
		"Results folder name": "results/",

		# Widths (µm) of the target plane on which fields are reconstructed. Will be used to
		# set the transverse size of output data. Should be smaller than the source widths.
		"Target output widths": [8, 8],

		# Number of pixels per dimension for the target plane on which fields are
		# reconstructed. Will be used to set the transverse dims of output data
		"Target N pixels per dim": [100, 100],

		"Bulk output": {
			# Base name for bulk data files
			"Base name": "bulk",

			# Should we run the homotopy continuation algorithm to reconstruct and save optical
			# fields in the bulk of the liquid crystal?
			"Export reconstructed fields": true,

			# Should we save bulk data associated with the rays?
			"Export ray data": true
		},
		"Screen output": {
			# Base name for screen data files
			"Base name": "screen",

			# Should we run the homotopy continuation algorithm to reconstruct and save optical
			# fields on the focal plane?
			"Export reconstructed fields": true,

			# Should we save data associated with the rays on the focal plane?
			"Export ray data": true,

			# Numerical aperture of the focal lens
			"Numerical aperture": 0.4
		}
	},
	"InverseScreenMap parameters": {
		# Number of adaptative refinements when inverting the ray mapping. Should be tweaked to
		# get a good compromise between accuracy and speed.
		"N refinement cycles": 4,

		# Homotopy settings used in the coarse phase of the inversion process
		"Coarse step HC parameters": {
			# Tolerance of the newton algorithm when computing corrector steps
			"Newton tolerance": 1e-8,

			# The homotopy step size will be adjusted so that the newton algorithm for the
			# corrector step runs in at most N iteration, where N is set in this parameter.
			"Optimum newton step number": 4,

			# Maximum size for the homotopy step size (µm)
			"Max arclength step": 0.1,

			# Maximum distance (µm) run by a homotopy path before stopping the search for
			# inverses.
			"Max arclength": 6
		},

		# Homotopy settings used in the refinement phase of the inversion process
		"Refinement step HC parameters": {
			# Tolerance of the newton algorithm when computing corrector steps
			"Newton tolerance": 1e-8,

			# The homotopy step size will be adjusted so that the newton algorithm for the
			# corrector step runs in at most N iteration, where N is set in this parameter.
			"Optimum newton step number": 4,

			# Maximum size for the homotopy step size (µm)
			"Max arclength step": 0.1,

			# Maximum distance (µm) run by a homotopy path before stopping the search for
			# inverses.
			"Max arclength": 6
		}
	}
})V0G0N";

	std::ofstream f(filename);
	if(!f.is_open())
		throw std::string(
			"Could not open the specified setting file, maybe you"
			"prepended a nonexistent directory?");

	f << file_content;
}

int main(int argc, char *argv[]) {

	std::string param_file_name;

	bool failed_parsing = (argc>1) ? false : true;
	int i=1;
	while(i<argc) {
		if(strcmp(argv[i],"-x")==0) {
			if(argv[++i][0]!='-') {
				param_file_name = argv[i];
				break;
			}
			else {
				failed_parsing = true;
				break;
			}
		}
		else if(strcmp(argv[i],"-h")==0) {
			print_usage();
			return 0;
		}
		else if(strcmp(argv[i],"-c")==0) {
			if(argv[++i][0]!='-') {
				create_default_settings_file(argv[i]);
				return 0;
			}
		}
		else {
			failed_parsing = true;
			break;
		}
	}
	if(failed_parsing) {
		std::cout <<
			std::endl << "Syntax error!" << std::endl;
		print_usage();
		return -1;
	}

	Eigen::initParallel();
	json j;

	try {
		j = json::parse(minify_json(param_file_name));

		std::string filename = j.at("Geometry").at("Director field VTI file");
		boost::filesystem::path path(filename);
		if(!boost::filesystem::exists(path))
			throw std::string(
				"The given vti file for the director file does not exists");

		vtkObject::GlobalWarningDisplayOff();

		std::cout <<
			"Loading director field values" << std::endl;
		auto reader = vtkSmartPointer<vtkXMLImageDataReader>::New();
		reader->SetFileName(filename.c_str());
		reader->UpdateInformation();

		bool found_n_data = false;
		for(unsigned int i=0; i<reader->GetNumberOfPointArrays(); i++) {
			auto array_name = reader->GetPointArrayName(i);
			if(!std::strcmp(array_name, "n")) {
				found_n_data = true;
				reader->SetPointArrayStatus(array_name, 1);
			}
			else
				reader->SetPointArrayStatus(array_name, 0);
		}
		if(!found_n_data)
			throw std::string(
				"Could not find a vector field named \"n\" in the given vti file");

		reader->Update();
		auto nfield_vti_data = reader->GetOutput();
		auto nfield_vti_vals = vtkDoubleArray::FastDownCast(
			nfield_vti_data->GetPointData()->GetAbstractArray("n"));

		int dims[3];			nfield_vti_data->GetDimensions(dims);
		double spacings[3];		nfield_vti_data->GetSpacing(spacings);

		unsigned int spatial_dim;
		if(dims[2]<=1)
			throw std::string(
				"Not enough points in the z-direction");
		else if(dims[0]==1 && dims[1]==1)
			throw std::string(
				"1D samples not supported");
		else if(dims[1]==1)
			throw std::string(
				"2D samples should be specified in the YZ plane");
		else if(dims[0]==1)
			spatial_dim = 2;
		else
			spatial_dim = 3;

		double lc_thickness = (dims[2]-1)*spacings[2];
		unsigned int N_lc_steps = dims[2];

		switch(spatial_dim) {
			case 2: {
				auto n_values = std::make_shared<std::vector<Vector<3,double> > >();
				Vector<3,double> n_val;
				for(int iy=-2; iy<dims[1]+2; iy++) {
					for(int iz=-2; iz<dims[2]+2; iz++) {
						if(iy>=0 && iy<dims[1] && iz>=0 && iz<dims[2])
							for(int c=0; c<3; c++)
								n_val(c) = nfield_vti_vals->GetComponent(iy+dims[1]*iz, c);
						n_values->push_back(n_val);
					}
				}

				Vector<2,unsigned long> full_dims({dims[1]+4,dims[2]+4});
				Vector<2,double> full_origin(
					{-(dims[1]+3)*spacings[1]/2.,-(dims[2]+3)*spacings[2]/2.});
				Vector<2,double> full_lengths(
					{(dims[1]+3)*spacings[1],(dims[2]+3)*spacings[2]});
				auto full_mesh = std::make_shared<CartesianMesh<2> >(
					full_origin, full_lengths, full_dims);

				std::shared_ptr<Simulation<2> > sim;
				if(j.at("Geometry").at("Sample type") == "Full") {
					Vector<2,unsigned long> lc_dims({dims[1],dims[2]});
					Vector<2,double> lc_origin(
						{-(dims[1]-1)*spacings[1]/2.,-(dims[2]-1)*spacings[2]/2.});
					Vector<2,double> lc_lengths(
						{(dims[1]-1)*spacings[1],(dims[2]-1)*spacings[2]});
					auto lc_mesh = std::make_shared<CartesianMesh<2> >(
						lc_origin, lc_lengths, lc_dims);
					auto lc_domain = std::make_shared<ParallelotopeDomain<2> >(*lc_mesh);
					auto n_field = std::make_shared<CubicInterpolatedMapping<2,3,double> >(
						n_values, full_mesh, lc_domain);
					n_field->normalize();

					sim = std::make_shared<FullSampleSimulation<2> >(
						j, lc_thickness, N_lc_steps, n_field);
				}
				else
					throw(std::string(
						"Error: \"Sample type\" value should be \"Full\"\n"
						"in dimension 2."));
				sim->run();
				break;
			}
			case 3: {
				auto n_values = std::make_shared<std::vector<Vector<3,double> > >();
				Vector<3,double> n_val;
				for(int ix=-2; ix<dims[0]+2; ix++) {
					for(int iy=-2; iy<dims[1]+2; iy++) {
						for(int iz=-2; iz<dims[2]+2; iz++) {
							if(ix>=0 && ix<dims[0] && iy>=0 && iy<dims[1]
									&& iz>=0 && iz<dims[2])
								for(int c=0; c<3; c++)
									n_val(c) = nfield_vti_vals->GetComponent(
										ix+dims[0]*(iy+dims[1]*iz), c);
							else
								n_val = 0;
							n_values->push_back(n_val);
						}
					}
				}

				Vector<3,unsigned long> full_dims({dims[0]+4,dims[1]+4,dims[2]+4});
				Vector<3,double> full_origin({-(dims[0]+3)*spacings[0]/2.,
					-(dims[1]+3)*spacings[1]/2.,-(dims[2]+3)*spacings[2]/2.});
				Vector<3,double> full_lengths({(dims[0]+3)*spacings[0],
					(dims[1]+3)*spacings[1],(dims[2]+3)*spacings[2]});
				auto full_mesh = std::make_shared<CartesianMesh<3> >(
					full_origin, full_lengths, full_dims);

				std::shared_ptr<Simulation<3> > sim;
				if(j.at("Geometry").at("Sample type") == "Full") {
					Vector<3,unsigned long> lc_dims({dims[0],dims[1],dims[2]});
					Vector<3,double> lc_origin({-(dims[0]-1)*spacings[0]/2.,
						-(dims[1]-1)*spacings[1]/2.,-(dims[2]-1)*spacings[2]/2.});
					Vector<3,double> lc_lengths({(dims[0]-1)*spacings[0],
						(dims[1]-1)*spacings[1],(dims[2]-1)*spacings[2]});
					auto lc_mesh = std::make_shared<CartesianMesh<3> >(
						lc_origin, lc_lengths, lc_dims);
					auto lc_domain = std::make_shared<ParallelotopeDomain<3> >(*lc_mesh);
					auto n_field = std::make_shared<CubicInterpolatedMapping<3,3,double> >(
						n_values, full_mesh, lc_domain);
					n_field->normalize();

					sim = std::make_shared<FullSampleSimulation<3> >(
						j, lc_thickness, N_lc_steps, n_field);
				}
				else if(j.at("Geometry").at("Sample type") == "Droplet") {
					auto lc_domain = std::make_shared<SphericalDomain<3> >(
						Vector<3,double>({0,0,0}), (dims[2]-1)*spacings[2]/2.);
					auto n_field = std::make_shared<CubicInterpolatedMapping<3,3,double> >(
						n_values, full_mesh, lc_domain);
					n_field->extrapolate_data();
					n_field->normalize();
					lc_thickness += 2*j.at("Geometry").at("Droplet sample parameters").at(
						"Distance from upper sample plate").get<double>();

					sim = std::make_shared<DropletSampleSimulation>(
						j, lc_thickness, N_lc_steps, n_field);
				}
				else
					throw(std::string(
						"Error: \"Sample type\" value should be \"Full\"\n"
						"or \"Droplet\" in dimension 3."));
				sim->run();
				break;
			}
		}
	}
	catch(std::string &s) {
		std::cerr <<
			std::endl << std::endl <<
			"----------------------------------------------------------------" <<
			std::endl <<
			"Exception on processing: " << std::endl <<
			s << std::endl <<
			"Aborting!" << std::endl <<
			"----------------------------------------------------------------" <<
			std::endl << std::endl;
		return -1;
	}
	catch(std::exception &exc) {
		std::cerr <<
			std::endl << std::endl <<
			"----------------------------------------------------------------" <<
			std::endl <<
			"Exception on processing: " << std::endl <<
			exc.what() << std::endl <<
			"Aborting!" << std::endl <<
			"----------------------------------------------------------------" <<
			std::endl << std::endl;
		return -1;
	}
	catch(...) {
		std::cerr <<
			std::endl << std::endl <<
			"----------------------------------------------------------------" <<
			std::endl <<
			"Unknown exception!" << std::endl <<
			"Aborting!" << std::endl <<
			"----------------------------------------------------------------" <<
			std::endl << std::endl;
		return -1;
	}

	return 0;
}


#ifndef HDGRAPH_HPP
#define HDGRAPH_HPP

#include <boost/graph/graph_utility.hpp>
#include "globals.hpp"
#include "rbgraph.hpp"

//=============================================================================
// Data structures

/**
  Scoped enumeration type whose underlying size is 1 byte, used for character
  state.

  State is paired with a character in the struct SignedCharacter.
*/
enum class State : bool {
  lose,  ///< The paired character is lost
  gain   ///< The paired character is gained
};

/**
  @brief Struct used to represent a signed character

  Each character c+ and c− is called a signed character.
*/
struct SignedCharacter {
  std::string character{};    ///< Character name
  State state = State::gain;  ///< Character state
};

//=============================================================================
// Bundled properties

/**
  @brief Struct used to represent the properties of an edge (Hasse diagram)

  The character c is gained in the edge (x, y) such that y is a child of x and
  c has state 0 in x and state 1 in y.
  In this case the edge (x, y) is labeled by c+.
  Conversely, c is lost in the edge (x, y) if y is a child of x and character c
  has state 1 in x and state 0 in y.
  In the latter case the edge (x, y) is labeled by c−.
  For each character c, we allow at most one edge labeled by c−.
*/
struct HDEdgeProperties {
  std::list<SignedCharacter> signedcharacters{};  ///< List of SignedCharacters
                                                  ///< that label the edge
};

/**
  @brief Struct used to represent the properties of a vertex (Hasse diagram)

  Given s a species, by C(s) we denote the set of characters of s.
  Let GM be a maximal reducible graph.
  Then the diagram P for GM is the Hasse diagram for the poset (Ps , ≤) of all
  species of GM ordered by the relation ≤, where s1 ≤ s2 if C(s1) ⊆ C(s2).
*/
struct HDVertexProperties {
  std::list<std::string> species{};  ///< List of species that label the vertex
  std::list<std::string> characters{};  ///< List of characters of the species
};

/**
  @brief Struct used to represent the properties of a Hasse diagram
*/
struct HDGraphProperties {
  const RBGraph* g{};   ///< Original red-black graph
  const RBGraph* gm{};  ///< Original maximal reducible graph
  size_t num_v; ///< Number of vertices
};

//=============================================================================
// Typedefs used for readabily

// Graph

/**
  Hasse diagram
*/
typedef boost::adjacency_list<boost::setS,            // OutEdgeList
                              boost::listS,            // VertexList
                              boost::bidirectionalS,  // Directed
                              HDVertexProperties,     // VertexProperties
                              HDEdgeProperties,       // EdgeProperties
                              HDGraphProperties       // GraphProperties
                              >
    HDGraph;

// Descriptors

/**
  Edge of a Hasse diagram
*/
typedef boost::graph_traits<HDGraph>::edge_descriptor HDEdge;

/**
  Vertex of a Hasse diagram
*/
typedef boost::graph_traits<HDGraph>::vertex_descriptor HDVertex;

// Iterators

/**
  Iterator of vertices (Hasse diagram)
*/
typedef boost::graph_traits<HDGraph>::vertex_iterator HDVertexIter;

/**
  Iterator of incoming edges (Hasse diagram)
*/
typedef boost::graph_traits<HDGraph>::in_edge_iterator HDInEdgeIter;

/**
  Iterator of outgoing edges (Hasse diagram)
*/
typedef boost::graph_traits<HDGraph>::out_edge_iterator HDOutEdgeIter;

/**
  Iterator (const) of a list of signed characters
*/
typedef std::list<SignedCharacter>::const_iterator SignedCharacterIter;

// Size types

/**
  Size type of vertices (Hasse diagram)
*/
typedef boost::graph_traits<HDGraph>::vertices_size_type HDVertexSize;

// Maps

/**
  Map of vertex indexes (Hasse diagram)
*/
typedef std::map<HDVertex, HDVertexSize> HDVertexIMap;

/**
  Associative property map of vertex indexes (Hasse diagram)
*/
typedef boost::associative_property_map<HDVertexIMap> HDVertexIAssocMap;

typedef std::map<HDVertex, size_t> HDVertexIndexMap;
typedef boost::associative_property_map<HDVertexIndexMap> HDVertexIndexAssocMap;

//=============================================================================
// Enum / Struct operator overloads

/**
  @brief Overloading of operator<< for State

  @param[in] os Output stream
  @param[in] s  State

  @return Updated output stream
*/
inline std::ostream& operator<<(std::ostream& os, const State s) {
  const bool sign = (s == State::lose);

  return os << (sign ? "-" : "+");
}

/**
  @brief Overloading of operator<< for SignedCharacter

  @param[in] os Output stream
  @param[in] sc SignedCharacter

  @return Updated output stream
*/
inline std::ostream& operator<<(std::ostream& os, const SignedCharacter sc) {
  return os << sc.character << sc.state;
}

/**
  @brief Overloading of operator== for a pair of signed characters

  @param[in] a SignedCharacter
  @param[in] b SignedCharacter

  @return True if a is equal to b
*/
inline bool operator==(const SignedCharacter& a, const SignedCharacter b) {
  return (a.character == b.character && a.state == b.state);
}

//=============================================================================
// Boost functions (overloading)

/**
  @brief Add vertex with \e species and \e characters to \e hasse

  @param[in]     species    List of species names
  @param[in]     characters List of character names
  @param[in,out] hasse      Hasse diagram graph

  @return Vertex descriptor for the new vertex
*/
HDVertex add_vertex(const std::list<std::string>& species,
                    const std::list<std::string>& characters, HDGraph& hasse);

/**
  @brief Add vertex with \e species and \e characters to \e hasse

  @param[in]     species    Species name
  @param[in]     characters List of character names
  @param[in,out] hasse      Hasse diagram graph

  @return Vertex descriptor for the new vertex
*/
inline HDVertex add_vertex(const std::string& species,
                           const std::list<std::string>& characters,
                           HDGraph& hasse) {
  return add_vertex(std::list<std::string>{species}, characters, hasse);
}

/**
  @brief Add edge between \e u and \e v with a list of signed characters to
         \e hasse

  @param[in]     u                Source Vertex
  @param[in]     v                Target Vertex
  @param[in]     signedcharacters List of signed characters
  @param[in,out] hasse            Hasse diagram graph

  @return Edge descriptor for the new edge.
          If the edge is already in the graph then a duplicate will not be
          added and the bool flag will be false.
          When the flag is false, the returned edge descriptor points to the
          already existing edge
*/
std::pair<HDEdge, bool> add_edge(
    const HDVertex u, const HDVertex v,
    const std::list<SignedCharacter>& signedcharacters, HDGraph& hasse);

//=============================================================================
// General functions

/**
  @brief Return a pointer to the original red-black graph of \e hasse

  @param[in] hasse Hasse diagram graph

  @return Pointer to the the original red-black graph of \e hasse
*/
inline const RBGraph* const orig_g(const HDGraph& hasse) {
  return hasse[boost::graph_bundle].g;
}

/**
  @brief Return a pointer to the original maximal reducible graph of \e hasse

  @param[in] hasse Hasse diagram graph

  @return Pointer to the the original maximal reducible graph of \e hasse
*/
inline const RBGraph* const orig_gm(const HDGraph& hasse) {
  return hasse[boost::graph_bundle].gm;
}

/**
  @brief Overloading of operator<< for HDGraph

  @param[in] os    Output stream
  @param[in] hasse Hasse diagram graph

  @return Updated output stream
*/
std::ostream& operator<<(std::ostream& os, const HDGraph& hasse);

//=============================================================================
// Algorithm functions

/**
  @brief Returns True if \e a is included in \e b

  @param[in] a List of character names (strings)
  @param[in] b List of character names (strings)

  @return True if \e a is included in \e b, False otherwise
*/
bool is_included(const std::list<std::string>& a,
                 const std::list<std::string>& b);

/**
  @brief Build the Hasse diagram of \e gm

  Let GM be a maximal reducible graph.
  Then the diagram P for GM is the Hasse diagram for the poset (Ps, ≤) of all
  species of GM ordered by the relation ≤, where s1 ≤ s2 if C(s1) ⊆ C(s2).
  Given (Ps, ≤) the poset of all species of a red-black graph, we
  consider the representation of the poset (Ps, ≤) by its Hasse diagram,
  represented by a directed acyclic graph P.
  More precisely, two species s1 and s2 are connected by the arc (s1, s2) if
  s1 < s2 and there does not exist a species s3 such that s1 < s3 < s2.

  @param[out] hasse Hasse diagram graph
  @param[in]  g     Red-black graph
  @param[in]  gm    Maximal reducible red-black graph
  @param[in]  components Vector of red-black connected subgraphs
  @param[in]  c_assocmap Connected Components map
*/
void hasse_diagram(HDGraph& hasse, const RBGraph& g, const RBGraph& gm, const RBGraphVector& components, const RBVertexIMap& c_map);

/**
  @brief Removes active species from an hasse diagram

  A reduced HDGraph is the one with active species eliminated.
  A specie is active if it has red edges incident to it.

  @param [in] hasse Hasse diagram graph
  @param [in] g     Red-black graph
  @param[in]  components Vector of red-black connected subgraphs
  @param[in]  c_assocmap Connected Components map

  @return Reduced Hasse diagram graph
*/
void reduce_diagram(HDGraph& hasse, const RBGraph& gm);

void transitive_reduction(HDGraph& hasse);
void remove_vertex(HDVertex& v, HDGraph& p);
inline size_t num_vertices(const HDGraph& hasse) {
  return hasse[boost::graph_bundle].num_v;
}

#endif  // HDGRAPH_HPP


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_TOOLS_NEWTON_SOLVER_HPP
#define BOOST_MATH_TOOLS_NEWTON_SOLVER_HPP

#ifdef _MSC_VER
#pragma once
#endif
#include <boost/math/tools/complex.hpp> // test for multiprecision types.

#include <iostream>
#include <utility>
#include <boost/config/no_tr1/cmath.hpp>
#include <stdexcept>

#include <boost/math/tools/config.hpp>
#include <boost/cstdint.hpp>
#include <boost/assert.hpp>
#include <boost/throw_exception.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4512)
#endif
#include <boost/math/tools/tuple.hpp>
#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#include <boost/math/special_functions/sign.hpp>
#include <boost/math/special_functions/next.hpp>
#include <boost/math/tools/toms748_solve.hpp>
#include <boost/math/policies/error_handling.hpp>

namespace boost {
namespace math {
namespace tools {

namespace detail {

namespace dummy {

   template<int n, class T>
   typename T::value_type get(const T&) BOOST_MATH_NOEXCEPT(T);
}

template <class Tuple, class T>
void unpack_tuple(const Tuple& t, T& a, T& b) BOOST_MATH_NOEXCEPT(T)
{
   using dummy::get;
   // Use ADL to find the right overload for get:
   a = get<0>(t);
   b = get<1>(t);
}
template <class Tuple, class T>
void unpack_tuple(const Tuple& t, T& a, T& b, T& c) BOOST_MATH_NOEXCEPT(T)
{
   using dummy::get;
   // Use ADL to find the right overload for get:
   a = get<0>(t);
   b = get<1>(t);
   c = get<2>(t);
}

template <class Tuple, class T>
inline void unpack_0(const Tuple& t, T& val) BOOST_MATH_NOEXCEPT(T)
{
   using dummy::get;
   // Rely on ADL to find the correct overload of get:
   val = get<0>(t);
}

template <class T, class U, class V>
inline void unpack_tuple(const std::pair<T, U>& p, V& a, V& b) BOOST_MATH_NOEXCEPT(T)
{
   a = p.first;
   b = p.second;
}
template <class T, class U, class V>
inline void unpack_0(const std::pair<T, U>& p, V& a) BOOST_MATH_NOEXCEPT(T)
{
   a = p.first;
}

template <class F, class T>
void handle_zero_derivative(F f,
   T& last_f0,
   const T& f0,
   T& delta,
   T& result,
   T& guess,
   const T& min,
   const T& max) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   if (last_f0 == 0)
   {
      // this must be the first iteration, pretend that we had a
      // previous one at either min or max:
      if (result == min)
      {
         guess = max;
      }
      else
      {
         guess = min;
      }
      unpack_0(f(guess), last_f0);
      delta = guess - result;
   }
   if (sign(last_f0) * sign(f0) < 0)
   {
      // we've crossed over so move in opposite direction to last step:
      if (delta < 0)
      {
         delta = (result - min) / 2;
      }
      else
      {
         delta = (result - max) / 2;
      }
   }
   else
   {
      // move in same direction as last step:
      if (delta < 0)
      {
         delta = (result - max) / 2;
      }
      else
      {
         delta = (result - min) / 2;
      }
   }
}

} // namespace

template <class F, class T, class Tol, class Policy>
std::pair<T, T> bisect(F f, T min, T max, Tol tol, boost::uintmax_t& max_iter, const Policy& pol) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<Policy>::value&& BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   T fmin = f(min);
   T fmax = f(max);
   if (fmin == 0)
   {
      max_iter = 2;
      return std::make_pair(min, min);
   }
   if (fmax == 0)
   {
      max_iter = 2;
      return std::make_pair(max, max);
   }

   //
   // Error checking:
   //
   static const char* function = "boost::math::tools::bisect<%1%>";
   if (min >= max)
   {
      return boost::math::detail::pair_from_single(policies::raise_evaluation_error(function,
         "Arguments in wrong order in boost::math::tools::bisect (first arg=%1%)", min, pol));
   }
   if (fmin * fmax >= 0)
   {
      return boost::math::detail::pair_from_single(policies::raise_evaluation_error(function,
         "No change of sign in boost::math::tools::bisect, either there is no root to find, or there are multiple roots in the interval (f(min) = %1%).", fmin, pol));
   }

   //
   // Three function invocations so far:
   //
   boost::uintmax_t count = max_iter;
   if (count < 3)
      count = 0;
   else
      count -= 3;

   while (count && (0 == tol(min, max)))
   {
      T mid = (min + max) / 2;
      T fmid = f(mid);
      if ((mid == max) || (mid == min))
         break;
      if (fmid == 0)
      {
         min = max = mid;
         break;
      }
      else if (sign(fmid) * sign(fmin) < 0)
      {
         max = mid;
      }
      else
      {
         min = mid;
         fmin = fmid;
      }
      --count;
   }

   max_iter -= count;

#ifdef BOOST_MATH_INSTRUMENT
   std::cout << "Bisection iteration, final count = " << max_iter << std::endl;

   static boost::uintmax_t max_count = 0;
   if (max_iter > max_count)
   {
      max_count = max_iter;
      std::cout << "Maximum iterations: " << max_iter << std::endl;
   }
#endif

   return std::make_pair(min, max);
}

template <class F, class T, class Tol>
inline std::pair<T, T> bisect(F f, T min, T max, Tol tol, boost::uintmax_t& max_iter)  BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value&& BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   return bisect(f, min, max, tol, max_iter, policies::policy<>());
}

template <class F, class T, class Tol>
inline std::pair<T, T> bisect(F f, T min, T max, Tol tol) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value&& BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return bisect(f, min, max, tol, m, policies::policy<>());
}

template <class F, class T>
T newton_raphson_iterate(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value&& BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   BOOST_MATH_STD_USING

   static const char* function = "boost::math::tools::newton_raphson_iterate<%1%>";
   if (min >= max)
   {
      return policies::raise_evaluation_error(function, "Range arguments in wrong order in boost::math::tools::newton_raphson_iterate(first arg=%1%)", min, boost::math::policies::policy<>());
   }

   T f0(0), f1, last_f0(0);
   T result = guess;

   T factor = static_cast<T>(ldexp(1.0, 1 - digits));
   T delta = tools::max_value<T>();
   T delta1 = tools::max_value<T>();
   T delta2 = tools::max_value<T>();

   //
   // We use these to sanity check that we do actually bracket a root,
   // we update these to the function value when we update the endpoints
   // of the range.  Then, provided at some point we update both endpoints
   // checking that max_range_f * min_range_f <= 0 verifies there is a root
   // to be found somewhere.  Note that if there is no root, and we approach
   // a local minima, then the derivative will go to zero, and hence the next
   // step will jump out of bounds (or at least past the minima), so this
   // check *should* happen in pathological cases.
   //
   T max_range_f = 0;
   T min_range_f = 0;

   boost::uintmax_t count(max_iter);

#ifdef BOOST_MATH_INSTRUMENT
   std::cout << "Newton_raphson_iterate, guess = " << guess << ", min = " << min << ", max = " << max
      << ", digits = " << digits << ", max_iter = " << max_iter << std::endl;
#endif

   do {
      last_f0 = f0;
      delta2 = delta1;
      delta1 = delta;
      detail::unpack_tuple(f(result), f0, f1);
      --count;
      if (0 == f0)
         break;
      if (f1 == 0)
      {
         // Oops zero derivative!!!
#ifdef BOOST_MATH_INSTRUMENT
         std::cout << "Newton iteration, zero derivative found!" << std::endl;
#endif
         detail::handle_zero_derivative(f, last_f0, f0, delta, result, guess, min, max);
      }
      else
      {
         delta = f0 / f1;
      }
#ifdef BOOST_MATH_INSTRUMENT
      std::cout << "Newton iteration " << max_iter - count << ", delta = " << delta << std::endl;
#endif
      if (fabs(delta * 2) > fabs(delta2))
      {
         // Last two steps haven't converged.
         T shift = (delta > 0) ? (result - min) / 2 : (result - max) / 2;
         if ((result != 0) && (fabs(shift) > fabs(result)))
         {
            delta = sign(delta) * fabs(result) * 1.1f; // Protect against huge jumps!
            //delta = sign(delta) * result; // Protect against huge jumps! Failed for negative result. https://github.com/boostorg/math/issues/216
         }
         else
            delta = shift;
         // reset delta1/2 so we don't take this branch next time round:
         delta1 = 3 * delta;
         delta2 = 3 * delta;
      }
      guess = result;
      result -= delta;
      if (result <= min)
      {
         delta = 0.5F * (guess - min);
         result = guess - delta;
         if ((result == min) || (result == max))
            break;
      }
      else if (result >= max)
      {
         delta = 0.5F * (guess - max);
         result = guess - delta;
         if ((result == min) || (result == max))
            break;
      }
      // Update brackets:
      if (delta > 0)
      {
         max = guess;
         max_range_f = f0;
      }
      else
      {
         min = guess;
         min_range_f = f0;
      }
      //
      // Sanity check that we bracket the root:
      //
      if (max_range_f * min_range_f > 0)
      {
         return policies::raise_evaluation_error(function, "There appears to be no root to be found in boost::math::tools::newton_raphson_iterate, perhaps we have a local minima near current best guess of %1%", guess, boost::math::policies::policy<>());
      }
   }while(count && (fabs(result * factor) < fabs(delta)));

   max_iter -= count;

#ifdef BOOST_MATH_INSTRUMENT
   std::cout << "Newton Raphson final iteration count = " << max_iter << std::endl;

   static boost::uintmax_t max_count = 0;
   if (max_iter > max_count)
   {
      max_count = max_iter;
      // std::cout << "Maximum iterations: " << max_iter << std::endl;
      // Puzzled what this tells us, so commented out for now?
   }
#endif

   return result;
}

template <class F, class T>
inline T newton_raphson_iterate(F f, T guess, T min, T max, int digits) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value&& BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return newton_raphson_iterate(f, guess, min, max, digits, m);
}

namespace detail {

   struct halley_step
   {
      template <class T>
      static T step(const T& /*x*/, const T& f0, const T& f1, const T& f2) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T))
      {
         using std::fabs;
         T denom = 2 * f0;
         T num = 2 * f1 - f0 * (f2 / f1);
         T delta;

         BOOST_MATH_INSTRUMENT_VARIABLE(denom);
         BOOST_MATH_INSTRUMENT_VARIABLE(num);

         if ((fabs(num) < 1) && (fabs(denom) >= fabs(num) * tools::max_value<T>()))
         {
            // possible overflow, use Newton step:
            delta = f0 / f1;
         }
         else
            delta = denom / num;
         return delta;
      }
   };

   template <class F, class T>
   T bracket_root_towards_min(F f, T guess, const T& f0, T& min, T& max, boost::uintmax_t& count) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())));

   template <class F, class T>
   T bracket_root_towards_max(F f, T guess, const T& f0, T& min, T& max, boost::uintmax_t& count) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
   {
      using std::fabs;
      //
      // Move guess towards max until we bracket the root, updating min and max as we go:
      //
      T guess0 = guess;
      T multiplier = 2;
      T f_current = f0;
      if (fabs(min) < fabs(max))
      {
         while (--count && ((f_current < 0) == (f0 < 0)))
         {
            min = guess;
            guess *= multiplier;
            if (guess > max)
            {
               guess = max;
               f_current = -f_current;  // There must be a change of sign!
               break;
            }
            multiplier *= 2;
            unpack_0(f(guess), f_current);
         }
      }
      else
      {
         //
         // If min and max are negative we have to divide to head towards max:
         //
         while (--count && ((f_current < 0) == (f0 < 0)))
         {
            min = guess;
            guess /= multiplier;
            if (guess > max)
            {
               guess = max;
               f_current = -f_current;  // There must be a change of sign!
               break;
            }
            multiplier *= 2;
            unpack_0(f(guess), f_current);
         }
      }

      if (count)
      {
         max = guess;
         if (multiplier > 16)
            return (guess0 - guess) + bracket_root_towards_min(f, guess, f_current, min, max, count);
      }
      return guess0 - (max + min) / 2;
   }

   template <class F, class T>
   T bracket_root_towards_min(F f, T guess, const T& f0, T& min, T& max, boost::uintmax_t& count) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
   {
      using std::fabs;
      //
      // Move guess towards min until we bracket the root, updating min and max as we go:
      //
      T guess0 = guess;
      T multiplier = 2;
      T f_current = f0;

      if (fabs(min) < fabs(max))
      {
         while (--count && ((f_current < 0) == (f0 < 0)))
         {
            max = guess;
            guess /= multiplier;
            if (guess < min)
            {
               guess = min;
               f_current = -f_current;  // There must be a change of sign!
               break;
            }
            multiplier *= 2;
            unpack_0(f(guess), f_current);
         }
      }
      else
      {
         //
         // If min and max are negative we have to multiply to head towards min:
         //
         while (--count && ((f_current < 0) == (f0 < 0)))
         {
            max = guess;
            guess *= multiplier;
            if (guess < min)
            {
               guess = min;
               f_current = -f_current;  // There must be a change of sign!
               break;
            }
            multiplier *= 2;
            unpack_0(f(guess), f_current);
         }
      }

      if (count)
      {
         min = guess;
         if (multiplier > 16)
            return (guess0 - guess) + bracket_root_towards_max(f, guess, f_current, min, max, count);
      }
      return guess0 - (max + min) / 2;
   }

   template <class Stepper, class F, class T>
   T second_order_root_finder(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value&& BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
   {
      BOOST_MATH_STD_USING

#ifdef BOOST_MATH_INSTRUMENT
        std::cout << "Second order root iteration, guess = " << guess << ", min = " << min << ", max = " << max
        << ", digits = " << digits << ", max_iter = " << max_iter << std::endl;
#endif
      static const char* function = "boost::math::tools::halley_iterate<%1%>";
      if (min >= max)
      {
         return policies::raise_evaluation_error(function, "Range arguments in wrong order in boost::math::tools::halley_iterate(first arg=%1%)", min, boost::math::policies::policy<>());
      }

      T f0(0), f1, f2;
      T result = guess;

      T factor = ldexp(static_cast<T>(1.0), 1 - digits);
      T delta = (std::max)(T(10000000 * guess), T(10000000));  // arbitrarily large delta
      T last_f0 = 0;
      T delta1 = delta;
      T delta2 = delta;
      bool out_of_bounds_sentry = false;

   #ifdef BOOST_MATH_INSTRUMENT
      std::cout << "Second order root iteration, limit = " << factor << std::endl;
   #endif

      //
      // We use these to sanity check that we do actually bracket a root,
      // we update these to the function value when we update the endpoints
      // of the range.  Then, provided at some point we update both endpoints
      // checking that max_range_f * min_range_f <= 0 verifies there is a root
      // to be found somewhere.  Note that if there is no root, and we approach
      // a local minima, then the derivative will go to zero, and hence the next
      // step will jump out of bounds (or at least past the minima), so this
      // check *should* happen in pathological cases.
      //
      T max_range_f = 0;
      T min_range_f = 0;

      boost::uintmax_t count(max_iter);

      do {
         last_f0 = f0;
         delta2 = delta1;
         delta1 = delta;
         detail::unpack_tuple(f(result), f0, f1, f2);
         --count;

         BOOST_MATH_INSTRUMENT_VARIABLE(f0);
         BOOST_MATH_INSTRUMENT_VARIABLE(f1);
         BOOST_MATH_INSTRUMENT_VARIABLE(f2);

         if (0 == f0)
            break;
         if (f1 == 0)
         {
            // Oops zero derivative!!!
   #ifdef BOOST_MATH_INSTRUMENT
            std::cout << "Second order root iteration, zero derivative found!" << std::endl;
   #endif
            detail::handle_zero_derivative(f, last_f0, f0, delta, result, guess, min, max);
         }
         else
         {
            if (f2 != 0)
            {
               delta = Stepper::step(result, f0, f1, f2);
               if (delta * f1 / f0 < 0)
               {
                  // Oh dear, we have a problem as Newton and Halley steps
                  // disagree about which way we should move.  Probably
                  // there is cancelation error in the calculation of the
                  // Halley step, or else the derivatives are so small
                  // that their values are basically trash.  We will move
                  // in the direction indicated by a Newton step, but
                  // by no more than twice the current guess value, otherwise
                  // we can jump way out of bounds if we're not careful.
                  // See https://svn.boost.org/trac/boost/ticket/8314.
                  delta = f0 / f1;
                  if (fabs(delta) > 2 * fabs(guess))
                     delta = (delta < 0 ? -1 : 1) * 2 * fabs(guess);
               }
            }
            else
               delta = f0 / f1;
         }
   #ifdef BOOST_MATH_INSTRUMENT
         std::cout << "Second order root iteration, delta = " << delta << std::endl;
   #endif
         T convergence = fabs(delta / delta2);
         if ((convergence > 0.8) && (convergence < 2))
         {
            // last two steps haven't converged.
            delta = (delta > 0) ? (result - min) / 2 : (result - max) / 2;
            if ((result != 0) && (fabs(delta) > result))
               delta = sign(delta) * fabs(result) * 0.9f; // protect against huge jumps!
            // reset delta2 so that this branch will *not* be taken on the
            // next iteration:
            delta2 = delta * 3;
            delta1 = delta * 3;
            BOOST_MATH_INSTRUMENT_VARIABLE(delta);
         }
         guess = result;
         result -= delta;
         BOOST_MATH_INSTRUMENT_VARIABLE(result);

         // check for out of bounds step:
         if (result < min)
         {
            T diff = ((fabs(min) < 1) && (fabs(result) > 1) && (tools::max_value<T>() / fabs(result) < fabs(min)))
               ? T(1000)
               : (fabs(min) < 1) && (fabs(tools::max_value<T>() * min) < fabs(result))
               ? ((min < 0) != (result < 0)) ? -tools::max_value<T>() : tools::max_value<T>() : T(result / min);
            if (fabs(diff) < 1)
               diff = 1 / diff;
            if (!out_of_bounds_sentry && (diff > 0) && (diff < 3))
            {
               // Only a small out of bounds step, lets assume that the result
               // is probably approximately at min:
               delta = 0.99f * (guess - min);
               result = guess - delta;
               out_of_bounds_sentry = true; // only take this branch once!
            }
            else
            {
               if (fabs(float_distance(min, max)) < 2)
               {
                  result = guess = (min + max) / 2;
                  break;
               }
               delta = bracket_root_towards_min(f, guess, f0, min, max, count);
               result = guess - delta;
               guess = min;
               continue;
            }
         }
         else if (result > max)
         {
            T diff = ((fabs(max) < 1) && (fabs(result) > 1) && (tools::max_value<T>() / fabs(result) < fabs(max))) ? T(1000) : T(result / max);
            if (fabs(diff) < 1)
               diff = 1 / diff;
            if (!out_of_bounds_sentry && (diff > 0) && (diff < 3))
            {
               // Only a small out of bounds step, lets assume that the result
               // is probably approximately at min:
               delta = 0.99f * (guess - max);
               result = guess - delta;
               out_of_bounds_sentry = true; // only take this branch once!
            }
            else
            {
               if (fabs(float_distance(min, max)) < 2)
               {
                  result = guess = (min + max) / 2;
                  break;
               }
               delta = bracket_root_towards_max(f, guess, f0, min, max, count);
               result = guess - delta;
               guess = min;
               continue;
            }
         }
         // update brackets:
         if (delta > 0)
         {
            max = guess;
            max_range_f = f0;
         }
         else
         {
            min = guess;
            min_range_f = f0;
         }
         //
         // Sanity check that we bracket the root:
         //
         if (max_range_f * min_range_f > 0)
         {
            return policies::raise_evaluation_error(function, "There appears to be no root to be found in boost::math::tools::newton_raphson_iterate, perhaps we have a local minima near current best guess of %1%", guess, boost::math::policies::policy<>());
         }
      } while(count && (fabs(result * factor) < fabs(delta)));

      max_iter -= count;

   #ifdef BOOST_MATH_INSTRUMENT
      std::cout << "Second order root finder, final iteration count = " << max_iter << std::endl;
   #endif

      return result;
   }
} // T second_order_root_finder

template <class F, class T>
T halley_iterate(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value&& BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   return detail::second_order_root_finder<detail::halley_step>(f, guess, min, max, digits, max_iter);
}

template <class F, class T>
inline T halley_iterate(F f, T guess, T min, T max, int digits) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value&& BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return halley_iterate(f, guess, min, max, digits, m);
}

namespace detail {

   struct schroder_stepper
   {
      template <class T>
      static T step(const T& x, const T& f0, const T& f1, const T& f2) BOOST_NOEXCEPT_IF(BOOST_MATH_IS_FLOAT(T))
      {
         using std::fabs;
         T ratio = f0 / f1;
         T delta;
         if ((x != 0) && (fabs(ratio / x) < 0.1))
         {
            delta = ratio + (f2 / (2 * f1)) * ratio * ratio;
            // check second derivative doesn't over compensate:
            if (delta * ratio < 0)
               delta = ratio;
         }
         else
            delta = ratio;  // fall back to Newton iteration.
         return delta;
      }
   };

}

template <class F, class T>
T schroder_iterate(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value&& BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   return detail::second_order_root_finder<detail::schroder_stepper>(f, guess, min, max, digits, max_iter);
}

template <class F, class T>
inline T schroder_iterate(F f, T guess, T min, T max, int digits) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value&& BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return schroder_iterate(f, guess, min, max, digits, m);
}
//
// These two are the old spelling of this function, retained for backwards compatibility just in case:
//
template <class F, class T>
T schroeder_iterate(F f, T guess, T min, T max, int digits, boost::uintmax_t& max_iter) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value&& BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   return detail::second_order_root_finder<detail::schroder_stepper>(f, guess, min, max, digits, max_iter);
}

template <class F, class T>
inline T schroeder_iterate(F f, T guess, T min, T max, int digits) BOOST_NOEXCEPT_IF(policies::is_noexcept_error_policy<policies::policy<> >::value&& BOOST_MATH_IS_FLOAT(T) && noexcept(std::declval<F>()(std::declval<T>())))
{
   boost::uintmax_t m = (std::numeric_limits<boost::uintmax_t>::max)();
   return schroder_iterate(f, guess, min, max, digits, m);
}

#ifndef BOOST_NO_CXX11_AUTO_DECLARATIONS
/*
   * Why do we set the default maximum number of iterations to the number of digits in the type?
   * Because for double roots, the number of digits increases linearly with the number of iterations,
   * so this default should recover full precision even in this somewhat pathological case.
   * For isolated roots, the problem is so rapidly convergent that this doesn't matter at all.
   */
template<class Complex, class F>
Complex complex_newton(F g, Complex guess, int max_iterations = std::numeric_limits<typename Complex::value_type>::digits)
{
   typedef typename Complex::value_type Real;
   using std::norm;
   using std::abs;
   using std::max;
   // z0, z1, and z2 cannot be the same, in case we immediately need to resort to Muller's Method:
   Complex z0 = guess + Complex(1, 0);
   Complex z1 = guess + Complex(0, 1);
   Complex z2 = guess;

   do {
      auto pair = g(z2);
      if (norm(pair.second) == 0)
      {
         // Muller's method. Notation follows Numerical Recipes, 9.5.2:
         Complex q = (z2 - z1) / (z1 - z0);
         auto P0 = g(z0);
         auto P1 = g(z1);
         Complex qp1 = static_cast<Complex>(1) + q;
         Complex A = q * (pair.first - qp1 * P1.first + q * P0.first);

         Complex B = (static_cast<Complex>(2) * q + static_cast<Complex>(1)) * pair.first - qp1 * qp1 * P1.first + q * q * P0.first;
         Complex C = qp1 * pair.first;
         Complex rad = sqrt(B * B - static_cast<Complex>(4) * A * C);
         Complex denom1 = B + rad;
         Complex denom2 = B - rad;
         Complex correction = (z1 - z2) * static_cast<Complex>(2) * C;
         if (norm(denom1) > norm(denom2))
         {
            correction /= denom1;
         }
         else
         {
            correction /= denom2;
         }

         z0 = z1;
         z1 = z2;
         z2 = z2 + correction;
      }
      else
      {
         z0 = z1;
         z1 = z2;
         z2 = z2 - (pair.first / pair.second);
      }

      // See: https://math.stackexchange.com/questions/3017766/constructing-newton-iteration-converging-to-non-root
      // If f' is continuous, then convergence of x_n -> x* implies f(x*) = 0.
      // This condition approximates this convergence condition by requiring three consecutive iterates to be clustered.
      Real tol = (max)(abs(z2) * std::numeric_limits<Real>::epsilon(), std::numeric_limits<Real>::epsilon());
      bool real_close = abs(z0.real() - z1.real()) < tol && abs(z0.real() - z2.real()) < tol && abs(z1.real() - z2.real()) < tol;
      bool imag_close = abs(z0.imag() - z1.imag()) < tol && abs(z0.imag() - z2.imag()) < tol && abs(z1.imag() - z2.imag()) < tol;
      if (real_close && imag_close)
      {
         return z2;
      }

   } while (max_iterations--);

   // The idea is that if we can get abs(f) < eps, we should, but if we go through all these iterations
   // and abs(f) < sqrt(eps), then roundoff error simply does not allow that we can evaluate f to < eps
   // This is somewhat awkward as it isn't scale invariant, but using the Daubechies coefficient example code,
   // I found this condition generates correct roots, whereas the scale invariant condition discussed here:
   // https://scicomp.stackexchange.com/questions/30597/defining-a-condition-number-and-termination-criteria-for-newtons-method
   // allows nonroots to be passed off as roots.
   auto pair = g(z2);
   if (abs(pair.first) < sqrt(std::numeric_limits<Real>::epsilon()))
   {
      return z2;
   }

   return { std::numeric_limits<Real>::quiet_NaN(),
            std::numeric_limits<Real>::quiet_NaN() };
}
#endif

#if !defined(BOOST_NO_CXX17_IF_CONSTEXPR)
// https://stackoverflow.com/questions/48979861/numerically-stable-method-for-solving-quadratic-equations/50065711
namespace detail
{
#if defined(BOOST_GNU_STDLIB) && !defined(_GLIBCXX_USE_C99_MATH_TR1)
inline float fma_workaround(float x, float y, float z) { return ::fmaf(x, y, z); }
inline double fma_workaround(double x, double y, double z) { return ::fma(x, y, z); }
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline long double fma_workaround(long double x, long double y, long double z) { return ::fmal(x, y, z); }
#endif
#endif
template<class T>
inline T discriminant(T const& a, T const& b, T const& c)
{
   T w = 4 * a * c;
#if defined(BOOST_GNU_STDLIB) && !defined(_GLIBCXX_USE_C99_MATH_TR1)
   T e = fma_workaround(-c, 4 * a, w);
   T f = fma_workaround(b, b, -w);
#else
   T e = std::fma(-c, 4 * a, w);
   T f = std::fma(b, b, -w);
#endif
   return f + e;
}

template<class T>
std::pair<T, T> quadratic_roots_imp(T const& a, T const& b, T const& c)
{
#if defined(BOOST_GNU_STDLIB) && !defined(_GLIBCXX_USE_C99_MATH_TR1)
   using boost::math::copysign;
#else
   using std::copysign;
#endif
   using std::sqrt;
   if constexpr (std::is_floating_point<T>::value)
   {
      T nan = std::numeric_limits<T>::quiet_NaN();
      if (a == 0)
      {
         if (b == 0 && c != 0)
         {
            return std::pair<T, T>(nan, nan);
         }
         else if (b == 0 && c == 0)
         {
            return std::pair<T, T>(0, 0);
         }
         return std::pair<T, T>(-c / b, -c / b);
      }
      if (b == 0)
      {
         T x0_sq = -c / a;
         if (x0_sq < 0) {
            return std::pair<T, T>(nan, nan);
         }
         T x0 = sqrt(x0_sq);
         return std::pair<T, T>(-x0, x0);
      }
      T discriminant = detail::discriminant(a, b, c);
      // Is there a sane way to flush very small negative values to zero?
      // If there is I don't know of it.
      if (discriminant < 0)
      {
         return std::pair<T, T>(nan, nan);
      }
      T q = -(b + copysign(sqrt(discriminant), b)) / T(2);
      T x0 = q / a;
      T x1 = c / q;
      if (x0 < x1)
      {
         return std::pair<T, T>(x0, x1);
      }
      return std::pair<T, T>(x1, x0);
   }
   else if constexpr (boost::math::tools::is_complex_type<T>::value)
   {
      typename T::value_type nan = std::numeric_limits<typename T::value_type>::quiet_NaN();
      if (a.real() == 0 && a.imag() == 0)
      {
         using std::norm;
         if (b.real() == 0 && b.imag() && norm(c) != 0)
         {
            return std::pair<T, T>({ nan, nan }, { nan, nan });
         }
         else if (b.real() == 0 && b.imag() && c.real() == 0 && c.imag() == 0)
         {
            return std::pair<T, T>({ 0,0 }, { 0,0 });
         }
         return std::pair<T, T>(-c / b, -c / b);
      }
      if (b.real() == 0 && b.imag() == 0)
      {
         T x0_sq = -c / a;
         T x0 = sqrt(x0_sq);
         return std::pair<T, T>(-x0, x0);
      }
      // There's no fma for complex types:
      T discriminant = b * b - T(4) * a * c;
      T q = -(b + sqrt(discriminant)) / T(2);
      return std::pair<T, T>(q / a, c / q);
   }
   else // Most likely the type is a boost.multiprecision.
   {    //There is no fma for multiprecision, and in addition it doesn't seem to be useful, so revert to the naive computation.
      T nan = std::numeric_limits<T>::quiet_NaN();
      if (a == 0)
      {
         if (b == 0 && c != 0)
         {
            return std::pair<T, T>(nan, nan);
         }
         else if (b == 0 && c == 0)
         {
            return std::pair<T, T>(0, 0);
         }
         return std::pair<T, T>(-c / b, -c / b);
      }
      if (b == 0)
      {
         T x0_sq = -c / a;
         if (x0_sq < 0) {
            return std::pair<T, T>(nan, nan);
         }
         T x0 = sqrt(x0_sq);
         return std::pair<T, T>(-x0, x0);
      }
      T discriminant = b * b - 4 * a * c;
      if (discriminant < 0)
      {
         return std::pair<T, T>(nan, nan);
      }
      T q = -(b + copysign(sqrt(discriminant), b)) / T(2);
      T x0 = q / a;
      T x1 = c / q;
      if (x0 < x1)
      {
         return std::pair<T, T>(x0, x1);
      }
      return std::pair<T, T>(x1, x0);
   }
}
}  // namespace detail

template<class T1, class T2 = T1, class T3 = T1>
inline std::pair<typename tools::promote_args<T1, T2, T3>::type, typename tools::promote_args<T1, T2, T3>::type> quadratic_roots(T1 const& a, T2 const& b, T3 const& c)
{
   typedef typename tools::promote_args<T1, T2, T3>::type value_type;
   return detail::quadratic_roots_imp(static_cast<value_type>(a), static_cast<value_type>(b), static_cast<value_type>(c));
}

#endif

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_NEWTON_SOLVER_HPP


/// @file
///
/// A linear solver for parameters from the normal equations
///
/// @copyright (c) 2007 CSIRO
/// Australia Telescope National Facility (ATNF)
/// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
/// PO Box 76, Epping NSW 1710, Australia
/// atnf-enquiries@csiro.au
///
/// This file is part of the ASKAP software distribution.
///
/// The ASKAP software distribution is free software: you can redistribute it
/// and/or modify it under the terms of the GNU General Public License as
/// published by the Free Software Foundation; either version 2 of the License,
/// or (at your option) any later version.
///
/// This program is distributed in the hope that it will be useful,
/// but WITHOUT ANY WARRANTY; without even the implied warranty of
/// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
/// GNU General Public License for more details.
///
/// You should have received a copy of the GNU General Public License
/// along with this program; if not, write to the Free Software
/// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
///
/// @author Tim Cornwell <tim.cornwell@csiro.au>
///
#include <fitting/LinearSolver.h>

#include <askap/AskapError.h>
#include <profile/AskapProfiler.h>
#include <boost/config.hpp>

#include <casacore/casa/aips.h>
#include <casacore/casa/Arrays/Array.h>
#include <casacore/casa/Arrays/Matrix.h>
#include <casacore/casa/Arrays/Vector.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>

#include <askap/AskapLogging.h>
ASKAP_LOGGER(logger, ".linearsolver");

#include <iostream>

#include <string>
#include <map>

#include <cmath>
using std::abs;
using std::map;
using std::string;

namespace askap
{
  namespace scimath
  {
    BOOST_CONSTEXPR_OR_CONST double LinearSolver::KeepAllSingularValues;

    /// @brief Constructor
    /// @details Optionally, it is possible to limit the condition number of
    /// normal equation matrix to a given number.
    /// @param maxCondNumber maximum allowed condition number of the range
    /// of the normal equation matrix for the SVD algorithm. Effectively this
    /// puts the limit on the singular values, which are considered to be
    /// non-zero (all greater than the largest singular value divided by this
    /// condition number threshold). Default is 1e3. Put a negative number
    /// if you don't want to drop any singular values (may be a not very wise
    /// thing to do!). A very large threshold has the same effect. Zero
    /// threshold is not allowed and will cause an exception.
    LinearSolver::LinearSolver(double maxCondNumber) :
           itsMaxCondNumber(maxCondNumber)
    {
      ASKAPASSERT(itsMaxCondNumber!=0);
    };

    void LinearSolver::init()
    {
      resetNormalEquations();
    }

/// @brief test that all matrix elements are below tolerance by absolute value
/// @details This is a helper method to test all matrix elements
/// @param[in] matr matrix to test
/// @param[in] tolerance tolerance on the element absolute values
/// @return true if all elements are zero within the tolerance
bool LinearSolver::allMatrixElementsAreZeros(const casa::Matrix<double> &matr, const double tolerance)
{
  for (casa::uInt row = 0; row < matr.nrow(); ++row) {
       for (casa::uInt col = 0; col < matr.ncolumn(); ++col) {
            if (abs(matr(row,col)) > tolerance) {
                return false;
            }
       }
  }
  return true;
}

/// @brief extract an independent subset of parameters
/// @details This method analyses the normal equations and forms a subset of
/// parameters which can be solved for independently. Although the SVD is more than
/// capable of dealing with degeneracies, it is often too slow if the number of parameters is large.
/// This method essentially gives the solver a hint based on the structure of the equations
/// @param[in] names names for parameters to choose from
/// @param[in] tolerance tolerance on the matrix elements to decide whether they can be considered independent
/// @return names of parameters in this subset
std::vector<std::string> LinearSolver::getIndependentSubset(std::vector<std::string> &names, const double tolerance) const
{
   ASKAPTRACE("LinearSolver::getIndependentSubset");
   ASKAPDEBUGASSERT(names.size() > 0);
   std::vector<std::string> resultNames;
   resultNames.reserve(names.size());
   resultNames.push_back(names[0]);
   for (std::vector<std::string>::const_iterator ci = ++names.begin(); ci != names.end(); ++ci) {
        for (std::vector<std::string>::const_iterator ciRes = resultNames.begin(); ciRes != resultNames.end(); ++ciRes) {
             const casa::Matrix<double>& nm1 = normalEquations().normalMatrix(*ci, *ciRes);
             const casa::Matrix<double>& nm2 = normalEquations().normalMatrix(*ciRes, *ci);
             if (!allMatrixElementsAreZeros(nm1,tolerance) || !allMatrixElementsAreZeros(nm2,tolerance)) {
                 // this parameter (iterated in the outer loop) belongs to the same subset
                 resultNames.push_back(*ci);
                 break;
             }
        }
   }
   return resultNames;
}

/// @brief solve for a subset of parameters
/// @details This method is used in solveNormalEquations
/// @param[in] params parameters to be updated
/// @param[in] quality Quality of the solution
/// @param[in] names names of the parameters to solve for
std::pair<double,double>  LinearSolver::solveSubsetOfNormalEquations(Params &params, Quality& quality,
                   const std::vector<std::string> &names) const
{
    ASKAPTRACE("LinearSolver::solveSubsetOfNormalEquations");
    std::pair<double,double> result(0.,0.);

// Solving A^T Q^-1 V = (A^T Q^-1 A) P

    int nParameters = 0;

    std::vector<std::pair<string, int> > indices(names.size());
    {
      std::vector<std::pair<string, int> >::iterator it = indices.begin();
      for (vector<string>::const_iterator cit=names.begin(); cit!=names.end(); ++cit,++it)
      {
        ASKAPDEBUGASSERT(it != indices.end());
        it->second = nParameters;
        it->first = *cit;
        ASKAPLOG_DEBUG_STR(logger, "Processing "<<*cit<<" "<<nParameters);
        const casa::uInt newParameters = normalEquations().dataVector(*cit).nelements();
        nParameters += newParameters;
        ASKAPDEBUGASSERT((params.isFree(*cit) ? params.value(*cit).nelements() : newParameters) == newParameters);
      }
    }
    ASKAPLOG_DEBUG_STR(logger, "Done");
    ASKAPCHECK(nParameters>0, "No free parameters in a subset of normal equations");

    ASKAPDEBUGASSERT(indices.size() > 0);

    // Convert the normal equations to gsl format
    gsl_matrix * A = gsl_matrix_alloc (nParameters, nParameters);
    gsl_vector * B = gsl_vector_alloc (nParameters);
    gsl_vector * X = gsl_vector_alloc (nParameters);

    for (std::vector<std::pair<string, int> >::const_iterator indit2=indices.begin();indit2!=indices.end(); ++indit2)  {
        for (std::vector<std::pair<string, int> >::const_iterator indit1=indices.begin();indit1!=indices.end(); ++indit1)  {
             // Axes are dof, dof for each parameter
             // Take a deep breath for const-safe indexing into the double layered map
             const casa::Matrix<double>& nm = normalEquations().normalMatrix(indit1->first, indit2->first);

             for (size_t row=0; row<nm.nrow(); ++row)  {
                  for (size_t col=0; col<nm.ncolumn(); ++col) {
                       const double elem = nm(row,col);
                       ASKAPCHECK(!std::isnan(elem), "Normal matrix seems to have NaN for row = "<<row<<" and col = "<<col<<", this shouldn't happem!");
                       gsl_matrix_set(A, row+(indit1->second), col+(indit2->second), elem);
                       //   std::cout << "A " << row << " " << col << " " << nm(row,col) << std::endl;
                  }
             }
         }
    }

    for (std::vector<std::pair<string, int> >::const_iterator indit1=indices.begin();indit1!=indices.end(); ++indit1) {
        const casa::Vector<double> &dv = normalEquations().dataVector(indit1->first);
        for (size_t row=0; row<dv.nelements(); ++row) {
             const double elem = dv(row);
             ASKAPCHECK(!std::isnan(elem), "Data vector seems to have NaN for row = "<<row<<", this shouldn't happem!");
             gsl_vector_set(B, row+(indit1->second), elem);
//          std::cout << "B " << row << " " << dv(row) << std::endl;
        }
    }

      /*
      // temporary code to export matrices, which cause problems with the GSL
      // to write up a clear case
      {
        std::ofstream os("dbg.dat");
        os<<nParameters<<std::endl;
        for (int row=0;row<nParameters;++row) {
             for (int col=0;col<nParameters;++col) {
                  if (col) {
                      os<<" ";
                  }
                  os<<gsl_matrix_get(A,row,col);
             }
             os<<std::endl;
        }
      }
      // end of the temporary code
      */

    if(algorithm()=="SVD")  {
         gsl_matrix * V = gsl_matrix_alloc (nParameters, nParameters);
         ASKAPDEBUGASSERT(V!=NULL);
         gsl_vector * S = gsl_vector_alloc (nParameters);
         ASKAPDEBUGASSERT(S!=NULL);
         gsl_vector * work = gsl_vector_alloc (nParameters);
         ASKAPDEBUGASSERT(work!=NULL);

         const int status = gsl_linalg_SV_decomp (A, V, S, work);
         ASKAPCHECK(status == 0, "gsl_linalg_SV_decomp failed, status = "<<status);

         // a hack for now. For some reason, for some matrices gsl_linalg_SV_decomp may return NaN as singular value, perhaps some
         // numerical precision issue inside SVD. Although it needs to be investigated further  (see ASKAPSDP-2270), for now trying
         // to replace those singular values with zeros to exclude them from processing. Note, singular vectors may also contain NaNs
         for (int i=0; i<nParameters; ++i) {
              if (std::isnan(gsl_vector_get(S,i))) {
                  gsl_vector_set(S,i,0.);
              }
              for (int k=0; k < nParameters; ++k) {
                   ASKAPCHECK(!std::isnan(gsl_matrix_get(V,i,k)), "NaN in V: i="<<i<<" k="<<k);
              }
         }

         // end of the hack

         //SVDecomp (A, V, S);

         // code to put a limit on the condition number of the system
         const double singularValueLimit = nParameters>1 ?
                     gsl_vector_get(S,0)/itsMaxCondNumber : -1.;
         for (int i=1; i<nParameters; ++i) {
              if (gsl_vector_get(S,i)<singularValueLimit) {
                  gsl_vector_set(S,i,0.);
              }
         }

        /*
        // temporary code for debugging
        {
          std::ofstream os("dbg2.dat");
          for (int i=0; i<nParameters; ++i) {
               os<<i<<" "<<gsl_vector_get(S,i)<<std::endl;
          }

          //std::cout<<"new singular value spectrum is ready"<<std::endl;
          //char tst;
          //std::cin>>tst;

        }
        // end of temporary code
        */

         gsl_vector * X = gsl_vector_alloc(nParameters);
         ASKAPDEBUGASSERT(X!=NULL);

         const int solveStatus = gsl_linalg_SV_solve (A, V, S, B, X);
         ASKAPCHECK(solveStatus == 0, "gsl_linalg_SV_solve failed");

// Now find the statistics for the decomposition
         int rank=0;
         double smin = 1e50;
         double smax = 0.0;
         for (int i=0;i<nParameters; ++i) {
              const double sValue = std::abs(gsl_vector_get(S, i));
              ASKAPCHECK(!std::isnan(sValue), "Got NaN as a singular value for normal matrix, this shouldn't happen S[i]="<<gsl_vector_get(S,i)<<" parameter "<<i<<" singularValueLimit="<<singularValueLimit);
              if(sValue>0.0) {
                 ++rank;
                 if ((sValue>smax) || (i == 0)) {
                     smax=sValue;
                 }
                 if ((sValue<smin) || (i == 0)) {
                     smin=sValue;
                 }
               }
         }
         result.first = smin;
         result.second = smax;

         quality.setDOF(nParameters);
         quality.setRank(rank);
         quality.setCond(smax/smin);
         if(rank==nParameters) {
            quality.setInfo("SVD decomposition rank complete");
         } else {
            quality.setInfo("SVD decomposition rank deficient");
         }

// Update the parameters for the calculated changes. Exploit reference
// semantics of casa::Array.
         std::vector<std::pair<string, int> >::const_iterator indit;
         for (indit=indices.begin();indit!=indices.end();++indit) {
              casa::IPosition vecShape(1, params.value(indit->first).nelements());
              casa::Vector<double> value(params.value(indit->first).reform(vecShape));
              for (size_t i=0; i<value.nelements(); ++i)  {
//          	   std::cout << value(i) << " " << gsl_vector_get(X, indit->second+i) << std::endl;
                   const double adjustment = gsl_vector_get(X, indit->second+i);
                   ASKAPCHECK(!std::isnan(adjustment), "Solution resulted in NaN as an update for parameter "<<(indit->second + i));
                   value(i) += adjustment;
              }
          }
          gsl_vector_free(S);
          gsl_vector_free(work);
          gsl_matrix_free(V);
    } else {
        quality.setInfo("Cholesky decomposition");
        gsl_linalg_cholesky_decomp(A);
        gsl_linalg_cholesky_solve(A, B, X);
// Update the parameters for the calculated changes
        std::vector<std::pair<string, int> >::const_iterator indit;
        for (indit=indices.begin();indit!=indices.end();++indit)
        {
          casa::IPosition vecShape(1, params.value(indit->first).nelements());
          casa::Vector<double> value(params.value(indit->first).reform(vecShape));
          for (size_t i=0; i<value.nelements(); ++i)  {
               value(i)+=gsl_vector_get(X, indit->second+i);
          }
        }
    }

// Free up gsl storage
    gsl_vector_free(B);
    gsl_matrix_free(A);
    gsl_vector_free(X);
    return result;
}

    /// @brief solve for parameters
    /// The solution is constructed from the normal equations and given
    /// parameters are updated. If there are no free parameters in the
    /// given Params class, all unknowns in the normal
    /// equatons will be solved for.
    /// @param[in] params parameters to be updated
    /// @param[in] quality Quality of solution
    /// @note This is fully general solver for the normal equations for any shape
    /// parameters.
    bool LinearSolver::solveNormalEquations(Params &params, Quality& quality)
    {
      ASKAPTRACE("LinearSolver::solveNormalEquations");

// Solving A^T Q^-1 V = (A^T Q^-1 A) P

// Find all the free parameters
      vector<string> names(params.freeNames());
      if (names.size() == 0) {
          // list of parameters is empty, will solve for all
          // unknowns in the equation
          names = normalEquations().unknowns();
      }
      ASKAPCHECK(names.size()>0, "No free parameters in Linear Solver");

      if (names.size() < 100) {
          // no need to extract independent blocks if number of unknowns is small
          solveSubsetOfNormalEquations(params,quality,names);
      } else {
          while (names.size() > 0) {
              const std::vector<std::string> subsetNames = getIndependentSubset(names,1e-6);
              if (subsetNames.size() == names.size()) {
                  names.resize(0);
              } else {
                  // remove the elements corresponding to the current subset from the list of names prepared for the
                  // following integration
                  std::vector<size_t> indicesToRemove(subsetNames.size(),subsetNames.size());
                  for (size_t index = 0,indexToRemove = 0; index < names.size(); ++index) {
                       if (find(subsetNames.begin(),subsetNames.end(),names[index]) != subsetNames.end()) {
                           ASKAPDEBUGASSERT(indexToRemove < indicesToRemove.size());
                           indicesToRemove[indexToRemove++] = index;
                       }
                  }

                  // the following could be done more elegantly/faster, but leave the optimisation to later time
                  for (std::vector<size_t>::const_reverse_iterator ci = indicesToRemove.rbegin(); ci!=indicesToRemove.rend(); ++ci) {
                       ASKAPDEBUGASSERT(*ci < names.size());
                       names.erase(names.begin() + *ci);
                  }
              }
              solveSubsetOfNormalEquations(params,quality, subsetNames);
          }
      }

      return true;
    };

    Solver::ShPtr LinearSolver::clone() const
    {
      return Solver::ShPtr(new LinearSolver(*this));
    }

  }
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_GIS_LATLONG_DETAIL_GRATICULE_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GIS_LATLONG_DETAIL_GRATICULE_HPP

#include <cmath>
#include <sstream>
#include <string>

#include <boost/numeric/conversion/cast.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

/*!
    \brief Cardinal directions.
    \ingroup cs
    \details They are used in the dms-class. When specified by the library user,
    north/east/south/west is, in general, enough. When parsed or received by an algorithm,
    the user knows it it is lat/long but not more
*/
enum cd_selector
{
    /*cd_none, */
    north,
    east,
    south,
    west,
    cd_lat,
    cd_lon
};

/*!
    \brief Utility class to assign poinst with degree,minute,second
    \ingroup cs
    \note Normally combined with latitude and longitude classes
    \tparam CardinalDir selects if it is north/south/west/east
    \tparam coordinate value, double/float
    \par Example:
    Example showing how to use the dms class
    \dontinclude doxygen_1.cpp
    \skip example_dms
    \line {
    \until }
*/
template <cd_selector CardinalDir, typename T = double>
class dms
{
public:

    /// Constructs with a value
    inline explicit dms(T v)
        : m_value(v)
    {}

    /// Constructs with a degree, minute, optional second
    inline explicit dms(int d, int m, T s = 0.0)
    {
        double v = ((CardinalDir == west || CardinalDir == south) ? -1.0 : 1.0)
                    * (double(d) + (m / 60.0) + (s / 3600.0));

        m_value = boost::numeric_cast<T>(v);
    }

    // Prohibit automatic conversion to T
    // because this would enable lon(dms<south>)
    // inline operator T() const { return m_value; }

    /// Explicit conversion to T (double/float)
    inline const T& as_value() const
    {
        return m_value;
    }

    /// Get degrees as integer, minutes as integer, seconds as double.
    inline void get_dms(int& d, int& m, double& s,
                        bool& positive, char& cardinal) const
    {
        double value = m_value;

        // Set to normal earth latlong coordinates
        while (value < -180)
        {
            value += 360;
        }
        while (value > 180)
        {
            value -= 360;
        }
        // Make positive and indicate this
        positive = value > 0;

        // Todo: we might implement template/specializations here
        // Todo: if it is "west" and "positive", make east? or keep minus sign then?

        cardinal = ((CardinalDir == cd_lat && positive) ? 'N'
            :  (CardinalDir == cd_lat && !positive) ? 'S'
            :  (CardinalDir == cd_lon && positive) ? 'E'
            :  (CardinalDir == cd_lon && !positive) ? 'W'
            :  (CardinalDir == east) ? 'E'
            :  (CardinalDir == west) ? 'W'
            :  (CardinalDir == north) ? 'N'
            :  (CardinalDir == south) ? 'S'
            : ' ');

        value = geometry::math::abs(value);

        // Calculate the values
        double fraction = 0;
        double integer = 0;
        fraction = std::modf(value, &integer);
        d = int(integer);
        s = 60.0 * std::modf(fraction * 60.0, &integer);
        m = int(integer);
    }

    /// Get degrees, minutes, seconds as a string, separators can be specified optionally
    inline std::string get_dms(std::string const& ds = " ",
        const std::string& ms = "'",
        const std::string& ss = "\"") const
    {
        double s = 0;
        int d = 0;
        int m = 0;
        bool positive = false;
        char cardinal = 0;
        get_dms(d, m, s, positive, cardinal);
        std::ostringstream out;
        out << d << ds << m << ms << s << ss << " " << cardinal;

        return out.str();
    }

private:

    T m_value;
};

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{
/*!
    \brief internal base class for latitude and longitude classes
    \details The latitude longitude classes define different types for lat and lon. This is convenient
    to construct latlong class without ambiguity.
    \note It is called graticule, after <em>"This latitude/longitude "webbing" is known as the common
    graticule" (http://en.wikipedia.org/wiki/Geographic_coordinate_system)</em>
    \tparam S latitude/longitude
    \tparam T coordinate type, double float or int
*/
template <typename T>
class graticule
{
public:

    // TODO: Pass 'v' by const-ref
    inline explicit graticule(T v) : m_v(v) {}
    inline operator T() const { return m_v; }

private:

    T m_v;
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

/*!
    \brief Utility class to assign points with latitude value (north/south)
    \ingroup cs
    \tparam T coordinate type, double / float
    \note Often combined with dms class
*/
template <typename T = double>
class latitude : public detail::graticule<T>
{
public:

    /// Can be constructed with a value
    inline explicit latitude(T v)
        : detail::graticule<T>(v)
    {}

    /// Can be constructed with a NORTH dms-class
    inline explicit latitude(dms<north,T> const& v)
        : detail::graticule<T>(v.as_value())
    {}

    /// Can be constructed with a SOUTH dms-class
    inline explicit latitude(dms<south,T> const& v)
       : detail::graticule<T>(v.as_value())
   {}
};

/*!
\brief Utility class to assign points with longitude value (west/east)
\ingroup cs
\tparam T coordinate type, double / float
\note Often combined with dms class
*/
template <typename T = double>
class longitude : public detail::graticule<T>
{
public:

    /// Can be constructed with a value
    inline explicit longitude(T v)
        : detail::graticule<T>(v)
    {}

    /// Can be constructed with a WEST dms-class
    inline explicit longitude(dms<west, T> const& v)
        : detail::graticule<T>(v.as_value())
    {}

    /// Can be constructed with an EAST dms-class
    inline explicit longitude(dms<east, T> const& v)
        : detail::graticule<T>(v.as_value())
    {}
};

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GIS_LATLONG_DETAIL_GRATICULE_HPP


/*
 * Copyright (c) 2020 Johannes Pankert <pankertj@ethz.ch>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name of this work 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.
 */
#include <Eigen/Dense>
#include <perceptive_mpc/kinematics/KinematicsInterface.hpp>

using namespace perceptive_mpc;

template <typename SCALAR_T>
KinematicsInterface<SCALAR_T>::KinematicsInterface(const KinematicInterfaceConfig& config) : config_(config) {}

template <typename SCALAR_T>
void KinematicsInterface<SCALAR_T>::computeState2EndeffectorTransform(Eigen::Matrix<SCALAR_T, 4, 4>& transform,
                                                                      const Eigen::Matrix<SCALAR_T, -1, 1>& state) const {
  if (state.size() != 13) {
    std::stringstream ss;
    ss << "Error: state.size()=" << state.size() << "!=13";
    std::string errorMessage = ss.str();
    std::cerr << std::endl << errorMessage << std::endl << std::endl;
    throw std::runtime_error(ss.str());
  }

  Eigen::Quaternion<SCALAR_T> baseOrientation;
  baseOrientation.coeffs() = state.template head<7>().template head<4>();
  const Eigen::Matrix<SCALAR_T, 3, 1>& basePosition = state.template head<7>().template tail<3>();
  const Eigen::Matrix<SCALAR_T, 6, 1>& armState = state.template tail<6>();

  // homogeneous transform world -> base
  Eigen::Matrix<SCALAR_T, 4, 4> homWorld2Base = Eigen::Matrix<SCALAR_T, 4, 4>::Identity();
  homWorld2Base.template topLeftCorner<3, 3>() = baseOrientation.toRotationMatrix();
  homWorld2Base.template topRightCorner<3, 1>() = basePosition;

  // world -> endeffector
  transform = homWorld2Base * computeBase2EndeffectorTransform(armState);
}

template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 4, 4> KinematicsInterface<SCALAR_T>::computeBase2EndeffectorTransform(
    const Eigen::Matrix<SCALAR_T, 6, 1>& armState) const {
  const Eigen::Matrix<SCALAR_T, 4, 4> armBaseToWrist2Transform = computeArmMountToToolMountTransform(armState);
  return config_.transformBase_X_ArmMount.template cast<SCALAR_T>() * armBaseToWrist2Transform *
         config_.transformToolMount_X_Endeffector.template cast<SCALAR_T>();
}

template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 3, -1> KinematicsInterface<SCALAR_T>::computeState2MultiplePointsOnRobot(
    const Eigen::Matrix<SCALAR_T, -1, 1>& state, const std::vector<std::vector<double>>& points) const {
  if (state.size() != 13) {
    std::stringstream ss;
    ss << "Error: state.size()=" << state.size() << "!=13";
    std::string errorMessage = ss.str();
    std::cerr << std::endl << errorMessage << std::endl << std::endl;
    throw std::runtime_error(ss.str());
  }

  int dim = 0;
  for (int i = 0; i < points.size(); i++) {
    for (int j = 0; j < points[i].size(); j++) {
      dim++;
    }
  }
  if (dim == 0) {
    return Eigen::Matrix<SCALAR_T, 3, -1>(3, 0);
  }
  Eigen::Matrix<SCALAR_T, 3, -1> result(3, dim);
  int resultIndex = 0;
  int linkIndex;

  Eigen::Quaternion<SCALAR_T> baseOrientation;
  baseOrientation.coeffs() = state.template head<7>().template head<4>();
  const Eigen::Matrix<SCALAR_T, 3, 1>& basePosition = state.template head<7>().template tail<3>();
  const Eigen::Matrix<SCALAR_T, 6, 1>& armState = state.template tail<6>();

  Eigen::Matrix<SCALAR_T, 4, 4> worldXFrBase = Eigen::Matrix<SCALAR_T, 4, 4>::Identity();
  worldXFrBase.template topLeftCorner<3, 3>() = baseOrientation.toRotationMatrix();
  worldXFrBase.template topRightCorner<3, 1>() = basePosition;

  return computeArmState2MultiplePointsOnRobot(armState, points, config_.transformBase_X_ArmMount, config_.transformToolMount_X_Endeffector,
                                               worldXFrBase);
}

template <typename SCALAR_T>
Eigen::Quaternion<SCALAR_T> KinematicsInterface<SCALAR_T>::matrixToQuaternion(const Eigen::Matrix<SCALAR_T, 3, 3>& R) const {
  SCALAR_T t1, t2, t;
  SCALAR_T x1, x2, x;
  SCALAR_T y1, y2, y;
  SCALAR_T z1, z2, z;
  SCALAR_T w1, w2, w;

  t1 = CppAD::CondExpGt(R(0, 0), R(1, 1), 1 + R(0, 0) - R(1, 1) - R(2, 2), 1 - R(0, 0) + R(1, 1) - R(2, 2));
  t2 = CppAD::CondExpLt(R(0, 0), -R(1, 1), 1 - R(0, 0) - R(1, 1) + R(2, 2), 1 + R(0, 0) + R(1, 1) + R(2, 2));
  t = CppAD::CondExpLt(R(2, 2), SCALAR_T(0.0), t1, t2);

  x1 = CppAD::CondExpGt(R(0, 0), R(1, 1), t, R(1, 0) + R(0, 1));
  x2 = CppAD::CondExpLt(R(0, 0), -R(1, 1), R(0, 2) + R(2, 0), R(2, 1) - R(1, 2));
  x = CppAD::CondExpLt(R(2, 2), SCALAR_T(0.0), x1, x2);

  y1 = CppAD::CondExpGt(R(0, 0), R(1, 1), R(1, 0) + R(0, 1), t);
  y2 = CppAD::CondExpLt(R(0, 0), -R(1, 1), R(2, 1) + R(1, 2), R(0, 2) - R(2, 0));
  y = CppAD::CondExpLt(R(2, 2), SCALAR_T(0.0), y1, y2);

  z1 = CppAD::CondExpGt(R(0, 0), R(1, 1), R(0, 2) + R(2, 0), R(2, 1) + R(1, 2));
  z2 = CppAD::CondExpLt(R(0, 0), -R(1, 1), t, R(1, 0) - R(0, 1));
  z = CppAD::CondExpLt(R(2, 2), SCALAR_T(0.0), z1, z2);

  w1 = CppAD::CondExpGt(R(0, 0), R(1, 1), R(2, 1) - R(1, 2), R(0, 2) - R(2, 0));
  w2 = CppAD::CondExpLt(R(0, 0), -R(1, 1), R(1, 0) - R(0, 1), t);
  w = CppAD::CondExpLt(R(2, 2), SCALAR_T(0.0), w1, w2);

  Eigen::Matrix<SCALAR_T, 4, 1> q({x, y, z, w});
  q *= 0.5 / sqrt(t);

  Eigen::Quaternion<SCALAR_T> quaternion;
  quaternion.x() = q(0);
  quaternion.y() = q(1);
  quaternion.z() = q(2);
  quaternion.w() = q(3);

  return quaternion;
}
template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 3, 1> KinematicsInterface<SCALAR_T>::getCOMBaseFrame(const Eigen::Matrix<SCALAR_T, -1, 1>& state) const {
  const Eigen::Matrix<SCALAR_T, 6, 1>& armState = state.template tail<6>();
  Eigen::Matrix<SCALAR_T, 4, 1> armCOM = Eigen::Matrix<SCALAR_T, 4, 1>::Ones();
  armCOM.template head<3>() = getArmCOM(armState);
  Eigen::Matrix<SCALAR_T, 3, 1> armCOMBaseFrame = (config_.transformBase_X_ArmMount.template cast<SCALAR_T>() * armCOM).template head<3>();
  double armMass = getArmMass();
  return ((SCALAR_T)armMass * armCOMBaseFrame + (config_.baseMass * config_.baseCOM).template cast<SCALAR_T>()) /
         (SCALAR_T)(armMass + config_.baseMass);
}
template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 3, 1> KinematicsInterface<SCALAR_T>::getZMPBaseFrame(
    const Eigen::Matrix<SCALAR_T, -1, 1>& state, const Eigen::Matrix<SCALAR_T, 7, 1>& eePoseReference,
    const Eigen::Matrix<SCALAR_T, 6, 1>& wrenchReferenceEEFrame) const {
  using vector3_t = Eigen::Matrix<SCALAR_T, 3, 1>;
  using hom_transform_t = Eigen::Matrix<SCALAR_T, 4, 4>;
  vector3_t rcog = getCOMBaseFrame(state);
  double robotMass = config_.baseMass + getArmMass();

  // convert reference force from fixed ee frame to base frame
  hom_transform_t worldToEndeffectorReference = hom_transform_t::Zero();
  worldToEndeffectorReference.template block<3, 3>(0, 0) =
      Eigen::Quaternion<SCALAR_T>(eePoseReference.template head<4>()).toRotationMatrix();
  worldToEndeffectorReference.template block<3, 1>(0, 3) = eePoseReference.template tail<3>();
  worldToEndeffectorReference(3, 3) = 1;

  hom_transform_t worldToBase = getWorldToBaseTransform(state);
  hom_transform_t baseToWorld = worldToBase;
  baseToWorld.template block<3, 3>(0, 0) = worldToBase.template block<3, 3>(0, 0).transpose();
  baseToWorld.template block<3, 1>(0, 3) = -baseToWorld.template block<3, 3>(0, 0) * worldToBase.template block<3, 1>(0, 3);

  hom_transform_t baseToEndeffectorReference = baseToWorld * worldToEndeffectorReference;
  vector3_t forceBaseFrame = baseToEndeffectorReference.template block<3, 3>(0, 0) * wrenchReferenceEEFrame.template head<3>();
  vector3_t torqueBaseFrame = baseToEndeffectorReference.template block<3, 3>(0, 0) * wrenchReferenceEEFrame.template tail<3>();

  // compute a virtual mass for the acting end-effector force
  const double g = 9.81;
  vector3_t fg;
  fg << (SCALAR_T)0, (SCALAR_T)0, (SCALAR_T)-robotMass * g;

  // compute translation to the virtual mass in base frame
  hom_transform_t base_X_Endeffector = computeBase2EndeffectorTransform(state.template tail<6>());
  vector3_t ree = base_X_Endeffector.template block<3, 1>(0, 3);

  vector3_t n;
  n << (SCALAR_T)0, (SCALAR_T)0, (SCALAR_T)1;

  return n.template cross(rcog.cross(fg) + ree.cross(forceBaseFrame) + torqueBaseFrame) / n.dot(fg + forceBaseFrame);
}

template <typename SCALAR_T>
Eigen::Matrix<SCALAR_T, 4, 4> KinematicsInterface<SCALAR_T>::getWorldToBaseTransform(const Eigen::Matrix<SCALAR_T, -1, 1>& state) const {
  Eigen::Quaternion<SCALAR_T> baseOrientation;
  baseOrientation.coeffs() = state.template head<7>().template head<4>();
  const Eigen::Matrix<SCALAR_T, 3, 1>& basePosition = state.template head<7>().template tail<3>();

  // homogeneous transform world -> base
  Eigen::Matrix<SCALAR_T, 4, 4> homWorld2Base = Eigen::Matrix<SCALAR_T, 4, 4>::Identity();
  homWorld2Base.template topLeftCorner<3, 3>() = baseOrientation.toRotationMatrix();
  homWorld2Base.template topRightCorner<3, 1>() = basePosition;
  return homWorld2Base;
}

template class perceptive_mpc::KinematicsInterface<double>;
template class perceptive_mpc::KinematicsInterface<CppAD::AD<CppAD::cg::CG<double>>>;


#include <vector>
#include <iostream>
#include <boost/python.hpp>
#include "util.hpp"

using namespace std;

double treegkr(Tree T, vector<double> creation, vector<double> destruction, vector<double> x, vector<double> y);

double treegkr_python_wrapper(boost::python::list vs,
                          boost::python::list us,
                          boost::python::list cost,
                          boost::python::list creation,
                          boost::python::list destruction,
                          boost::python::list x,
                          boost::python::list y){
  int n = boost::python::len(x);
  assert(boost::python::len(vs) == n - 1);
  assert(boost::python::len(us) == n - 1);
  assert(boost::python::len(cost) == n - 1);
  assert(boost::python::len(y) == n);
  assert(boost::python::len(creation) == n);
  assert(boost::python::len(destruction) == n);
  Tree T(n);
  Unionfind uf(n);
  for(int i = 0; i < n - 1; i++){
    int v = boost::python::extract<int>(vs[i]);
    int u = boost::python::extract<int>(us[i]);
    double c = boost::python::extract<double>(cost[i]);
    T.add_edge(v, u, c);
    if(uf.same(u, v)){
      cerr << "Input is not a tree" << endl;
      exit(1);
    }
    uf.unite(u, v);
  }
  if(uf.size[uf.find(0)] != n){
    cerr << "Input is not a tree" << endl;
    exit(1);
  }
  vector<double> xv(n), yv(n), creationv(n), destructionv(n);
  for(int i = 0; i < n; i++){
    xv[i] = boost::python::extract<double>(x[i]);
    yv[i] = boost::python::extract<double>(y[i]);
    creationv[i] = boost::python::extract<double>(creation[i]);
    destructionv[i] = boost::python::extract<double>(destruction[i]);
  }
  return treegkr(T, creationv, destructionv, xv, yv);
}

BOOST_PYTHON_MODULE(treegkr){
  boost::python::def("treegkr",
                     treegkr_python_wrapper,
                     boost::python::args("vs",
                                         "us",
                                         "cost",
                                         "creation",
                                         "destruction",
                                         "x",
                                         "y"),
                     "Fast computation of the generalized Kantorovich Rubinstein (GKR) distance on tree metrics\n"
                     "\n"
                     "Parameters\n"
                     "----------\n"
                     "vs : list of ints\n"
                     "    A list of endpoints of edges\n"
                     "    The node indices should start from zero\n"
                     "    The length of the list should be n-1\n"
                     "us : list of ints\n"
                     "    A list of endpoints of edges\n"
                     "    The node indices should start from zero\n"
                     "    The length of the list should be n-1\n"
                     "cost : list of floats\n"
                     "    A list of costs of edges\n"
                     "    The i-th edge connects node vs[i] and us[i] with cost[i].\n"
                     "    The length of the list should be n-1\n"
                     "creation : list of floats\n"
                     "    A list of creation costs\n"
                     "    A mass on node i can be created by cost creation[i].\n"
                     "    The length of the list should be n\n"
                     "destruction : list of floats\n"
                     "    A list of destruction costs\n"
                     "    A mass on node i can be destructed by cost destruction[i].\n"
                     "    The length of the list should be n\n"
                     "x : list of floats\n"
                     "    A list of source mass\n"
                     "    There exist x[i] target mass on node i.\n"
                     "    The length of the list should be n\n"
                     "y : list of floats\n"
                     "    A list of target mass\n"
                     "    There exist y[i] target mass on node i.\n"
                     "    The length of the list should be n\n"
                     "\n"
                     "Returns\n"
                     "-------\n"
                     "gkr: float\n"
                     "    The GKR distance\n");
}