#include <cstdlib>
#include <list>
#include <string>
#include <iostream>
#include <map>
#include <regex>
#include <xercesc/dom/DOM.hpp>
#include <boost/algorithm/string/trim.hpp>
#include "Error.h"
#include "Array.h"
#include "cstringtools.h"
#include "BitArray.h"
#include "charptr_map.h"

#include "Pool.h"
#include "InputPool.h"

#include "UnicodeTools.h"
#include "XMLException.h"
#include "XMLElement.h"

#include "FluxMLContentObject.h"
#include "FluxMLUnicodeConstants.h"
#include "FluxMLInput.h"

#include "FluxMLDocument.h"

#include "ExprTree.h"
#include "LinearExpression.h"

#include "MathML.h"
#include "MathMLDocument.h"
#include "MathMLDeclare.h"
#include "MathMLExpression.h"

// Xerces C++ Namespace einbinden
XERCES_CPP_NAMESPACE_USE

using namespace flux::symb;

namespace flux {
namespace xml {

/**
 * Parst ein Input-Element aus einem DOM-Tree eines FluxML-Dokuments.
 *
 * @param node Knoten des Input-Elements
 */
void FluxMLInput::parseInput(DOMNode * input)
{
	DOMNode * label;
	DOMNamedNodeMap * nnm;
	DOMAttr * poolAttr, * typeAttr, * cfgAttr, * purityAttr, * costAttr, * idAttr;
        /*** Neue Prfoile-Version **/
        /** List der Bedingungen des Profiles */
	std::list<double> profile_conditions_;
        DOMAttr *profileAttr;
        profile_defined=false;
        /*** Ende: Neue Prfoile-Version **/
	int natoms = 0;
	double cost;
        Array< double > purity;
	charptr_map< Array< double > > values;
        charptr_map< Array< double > > purities;
        charptr_map< double > costs;
        charptr_map< data::InputProfile> profiles;
	BitArray mask;
	data::InputPool::Type ip_type =
        data::InputPool::ip_isotopomer; // "isotopomer" ist default

	if (input_pool_)
		fTHROW(XMLException,input,"FluxMLInput object already initialized!");

	if (not XMLElement::match(input,fml_input,fml_xmlns_uri))
		fTHROW(XMLException,input,"element node (input) expected.");

	nnm = input->getAttributes();
	if (nnm == 0)
		fTHROW(XMLException,input,"element node (input) lacks attributes.");

	// Attribut pool (Pool-Bezeichnung)
	poolAttr = static_cast< DOMAttr* >(nnm->getNamedItem(fml_pool));
	if (poolAttr == 0)
		fTHROW(XMLException,input,"element node (input) lacks pool attribute.");
	U2A utf_pool_name(poolAttr->getValue());

	// Gibt es den Pool?
	data::Pool * ipool = lookupPool(utf_pool_name);
	if (ipool == 0)
	{
		fTHROW(XMLException,input,
				"unknown pool %s in input substrate spec",
				(char const*)utf_pool_name);
	}
        data::Configuration * cfg = doc_->getRootConfiguration();
	std::list< data::Constraint > const & eqConstraints = cfg->getEqualities();
	std::list< data::Constraint >::const_iterator eqConstraints_it;

	for (eqConstraints_it=eqConstraints.begin(); eqConstraints_it!=eqConstraints.end(); eqConstraints_it++)
	{
		charptr_array constraintsVars = eqConstraints_it->getConstraint()->getVarNames();
		if(constraintsVars.find((char const*)utf_pool_name)!=constraintsVars.end())
			fTHROW(XMLException,input,
				"input pool \"%s\" specified in equality constraint",
				(char const*)utf_pool_name);
	}

	std::list< data::Constraint > const & ineqConstraints = cfg->getInEqualities();
	std::list< data::Constraint >::const_iterator ineqConstraints_it;

	for (ineqConstraints_it=ineqConstraints.begin(); ineqConstraints_it!=ineqConstraints.end(); ineqConstraints_it++)
	{
		charptr_array constraintsVars = ineqConstraints_it->getConstraint()->getVarNames();
		if(constraintsVars.find((char const*)utf_pool_name)!=constraintsVars.end())
			fTHROW(XMLException,input,
				"input pool \"%s\" specified in inequality constraint",
				(char const*)utf_pool_name);
	}
	// Anzahl der Atome / Init. der Maske
	natoms = ipool->getNumAtoms();
	mask.resize(natoms,false);

	// Attribut id (Experimental Design)
	idAttr = static_cast< DOMAttr* >(nnm->getNamedItem(fml_id));
	U2A utf_id(idAttr ? idAttr->getValue() : 0);

	// Attribut type (isotopomer/cumomer/emu)
	typeAttr = static_cast< DOMAttr* >(nnm->getNamedItem(fml_type));
	if (typeAttr != 0)
	{
		if (XMLString::equals(typeAttr->getValue(), fml_isotopomer))
			ip_type = data::InputPool::ip_isotopomer;
		else if (XMLString::equals(typeAttr->getValue(),fml_cumomer))
			ip_type = data::InputPool::ip_cumomer;
		else if (XMLString::equals(typeAttr->getValue(),fml_emu))
			ip_type = data::InputPool::ip_emu;
		else
		{
			fTHROW(XMLException,input,
				"input pool %s: type is restricted to \"isotopomer\", "
				"\"cumomer\" or \"emu\"",
				ipool->getName());
		}
	}

        // Zweite Version der Input-Proflie definiert über profile-Attribut
        // gilt für alle Labels
	profileAttr = static_cast< DOMAttr* >(nnm->getNamedItem(fml_profile));
	if (profileAttr != 0)
	{
            // profile definiert
            profile_defined=true;

        // füge den Startpunkt Null zu Bedinngungsliste hinzu
        profile_conditions_.push_back(0.);

        // Profile-Bedingungen parsen
        U2A utf_profile(profileAttr->getValue());
        charptr_array expr_list = charptr_array::split(utf_profile,",");
        charptr_array::const_iterator i;
        double prv_val= 0., act_val=0.;

        for (i=expr_list.begin(); i!=expr_list.end(); i++)
        {
            char const * w;
            for (w=*i; *w!='\0'; w++)
                if (*w > 32) break;
            if (*w == '\0')
                continue;

            act_val= std::atof(*i);

            // Überprüfung der INF und Null-Zeitangabe
            if(act_val==0. or std::isinf(act_val))
                fTHROW(XMLException,input,
                       "parse error in input profile attribute (%s): "
                       "The timestamp \"t=0\" and \"t=inf\" must not be explicitly stated!", *i);

            // Überprüfung von streng monoton aufsteigender kommagetrennter Zeiten
            if(act_val<= prv_val)
                fTHROW(XMLException,input,
                       "parse error in input profile attribute (%s): "
                       "The timestamps must be specified in a strictly monotonously manner", *i);

            profile_conditions_.push_back(act_val);
            prv_val= act_val;
        }
	} else // Prüfe die Input-Substrate ob es sich um eine Substrate-Profile handelt
    {
        label = XMLElement::skipJunkNodes(input->getFirstChild());
        do
        {
            if(label!= 0)
            {
                DOMNode * child = XMLElement::skipJunkNodes(label->getFirstChild());
                U2A data(static_cast< DOMText* >(child)->getData());
                std::string str= (char * )data;
                if (str.find("t") != std::string::npos)
                {
                    profile_defined=true;

                    // füge den Startpunkt Null zu Bedinngungsliste hinzu
                    profile_conditions_.push_back(0.);

                    break;
                }
            }

            // Next label-Element
            label = XMLElement::nextNode(label);
        }
        while (label != 0);
    }

	label = XMLElement::skipJunkNodes(input->getFirstChild());
	do
	{
		if (not XMLElement::match(label,fml_label,fml_xmlns_uri))
		{
			if (label == 0)
			{
				// keine label-Kinder
				// => pool als natürlich markierten
				// Isotopomer-Pool initialisieren
				input_pool_ = new data::InputPool(
					utf_id,
					ipool->getName(),
					mask,
					data::InputPool::ip_isotopomer
					);
				if (mask.size())
                                {
					// Registrierung von eingesetzten Isotope zur Ermöglichung von
                                        // multi-Isotopic Tracer MFA Simulation,
                                        charptr_map<int> iso_map = ipool->getIsotopesCfg();
                                        if(iso_map.size())
                                        {
                                            input_pool_->setIsotopeCfg(iso_map);
                                            charptr_map<int>::const_iterator ii =iso_map.begin();
                                            std::stringstream iso_elem;
                                            iso_elem<<" for [" <<ii->key;
                                            ii++;
                                            while(ii!=iso_map.end())
                                            {
                                               iso_elem<<", "<< ii->key;
                                               ii++;
                                            }
                                            iso_elem<<"]";
                                            fNOTICE("assuming natural abundance %s in pool \"%s\"", iso_elem.str().c_str(),
                                                    ipool->getName());
                                        }
                                        else
                                            fNOTICE("assuming natural abundance 13C in pool \"%s\"",
                                                    ipool->getName());
                                }
				return;
			}
			else
			{
				fTHROW(XMLException,label,
					"input pool %s: element node (label) expected",
					ipool->getName()
					);
			}
		}

		nnm = label->getAttributes();
		if (nnm == 0)
			fTHROW(XMLException,label,"element node (label) lacks attributes.");

                cfgAttr = static_cast< DOMAttr* >(nnm->getNamedItem(fml_cfg));
		if (cfgAttr == 0)
			fTHROW(XMLException,label,"element node (label) lacks cfg attribute.");
		U2A utf_cfg(cfgAttr->getValue());

		// das cfg-Attribut muß einem speziellen Format entsprechen
		if (strlen(utf_cfg) != (size_t)natoms)
		{
			fTHROW(XMLException,label,
				"label spec. of pool %s [%s] with wrong size; expected size %i.",
				(char const *)utf_pool_name, (char const*)utf_cfg, natoms);
		}
		for (int i=0; i<natoms; i++)
		{
			switch (ip_type)
			{
			case data::InputPool::ip_cumomer:
			case data::InputPool::ip_emu:
				if (not (((char const*)utf_cfg)[i]=='x' or ((char const *)utf_cfg)[i]=='1'))
				{
					fTHROW(XMLException,label,
						"illegal %s label spec. [%s] for pool %s",
						ip_type == data::InputPool::ip_emu
							? "EMU" : "Cumomer",
						(char const*)utf_cfg,
						(char const*)utf_pool_name
						);
				}
				break;
			case data::InputPool::ip_isotopomer:
				if (not (((char const*)utf_cfg)[i]=='0' or ((char const*)utf_cfg)[i]=='1'))
				{
					fTHROW(XMLException,label,
						"illegal Isotopomer label spec. [%s] for pool %s",
						(char const*)utf_cfg, (char const*)utf_pool_name
						);
				}
			}
		}

		// den Wert aus [0,1] bzw. die Werteliste parsen
		size_t fvalues_len_exp;
		char const * w;

                Array< double > fvalues = parseLabel(static_cast< DOMElement * >(label));

                // Substrate profile soll zunächst nur für Isotopomer unterstützen
                if((ip_type!= data::InputPool::ip_isotopomer) && (profile_defined==true))
                    fTHROW(XMLException,label,
                            "the used substrate profiling in pool \"%s\" is only supported for isotopomer type yet!",
                            (char const*)utf_pool_name);
		switch (ip_type)
		{
                    case data::InputPool::ip_cumomer:
                    case data::InputPool::ip_isotopomer:
                            if ((fvalues.size() != 1) and not profile_defined)
                            {
                                    fTHROW(XMLException,label,
                                            "illegal label spec. (expecting exactly "
                                            "one fraction value)."
                                            );
                            }
                            break;
                    case data::InputPool::ip_emu:
                            for (w = (char const *)utf_cfg, fvalues_len_exp = 1;
                                    *w != '\0'; ++w)
                            {
                                    if (*w == '1') ++fvalues_len_exp;
                            }
                            if (fvalues.size() != fvalues_len_exp)
                            {
                                    fTHROW(XMLException,label,
                                            "invalid number of mass isotopomer fractions; "
                                            "found %i, expected %i",
                                            int(fvalues.size()), int(fvalues_len_exp));
                            }
		}

		// das purity-Attribut
		if ((purityAttr = static_cast< DOMAttr* >(nnm->getNamedItem(fml_purity))) != 0)
		{
			if (ip_type != data::InputPool::ip_isotopomer)
			{
				fTHROW(XMLException,label,
					"the \"purity\" attribute is allowed only for "
					"isotopomer substrate specifications.");
			}
                        U2A utf_purity(purityAttr->getValue());
                        charptr_array purityList= charptr_array::split(utf_purity," ");
                        size_t i=0,size = purityList.size();
                        if(size!= ipool->getNumOfActiveIsotope())
                            fTHROW(XMLException,label,
                                            "label spec. of pool %s [%s]: illegal number of values "
                                            "in attribute purity: %s",
                                            (char const *)utf_pool_name,
                                            (char const *)utf_cfg,
                                            (char const *)utf_purity
                                            );
                        purity= Array<double>(size);
                        if(size==1)
                        {
                            if (not XMLElement::parseDouble(purityAttr->getValue(),purity[0])
				or (purity[0] < 0. or purity[0] > 1.))
                            {
                                    U2A utf_purity(purityAttr->getValue());
                                    fTHROW(XMLException,label,
                                            "label spec. of pool %s [%s]: illegal value "
                                            "for attribute purity: %s",
                                            (char const *)utf_pool_name,
                                            (char const *)utf_cfg,
                                            (char const *)utf_purity
                                            );
                            }
                        }
                        else if(size>1)
                        {
                            charptr_array::const_iterator pli;
                            for (pli=purityList.begin(), i=0; pli!=purityList.end(); ++pli,++i)
                            {
                                purity[i]= atof(*pli);
                                if ((purity[i] < 0. or purity[i] > 1.))
                                {
                                        U2A utf_purity(purityAttr->getValue());
                                        fTHROW(XMLException,label,
                                                "label spec. of pool %s [%s]: illegal value "
                                                "for attribute purity: %s",
                                                (char const *)utf_pool_name,
                                                (char const *)utf_cfg,
                                                (char const *)utf_purity
                                                );
                                }

                            }
                        }
                        else
                        {
                            fTHROW(XMLException,label,
					"error parsing the \"purity\" attribute.");
                        }
		}
		else
                {
                    purity=Array<double>(1);
                    purity[0]= 2.; // Berechnung abschalten (default)
                }

		// das cost-Attribut
		if ((costAttr = static_cast< DOMAttr* >(nnm->getNamedItem(fml_cost))) != 0)
		{
			if (ip_type != data::InputPool::ip_isotopomer)
			{
				fTHROW(XMLException,label,
					"the \"cost\" attribute is allowed only for "
					"isotopomer substrate specifications.");
			}

			if (not XMLElement::parseDouble(costAttr->getValue(),cost))
			{
				U2A utf_cost(costAttr->getValue());
				fTHROW(XMLException,label,
					"label spec. of pool %s [%s]: illegal value "
					"for attribute cost: %s",
					(char const *)utf_pool_name,
					(char const *)utf_cfg,
					(char const *)utf_cost
				        );
			}
		}
		else cost = 0.; // kostenlos

		// Konfiguration mit Fraction-Wert registrieren
		if (values.exists(utf_cfg))
			fTHROW(XMLException,label,"duplicate specification of fraction %s",
				(char const *)utf_cfg);
		values.insert(utf_cfg,fvalues);
                purities.insert(utf_cfg,purity);
		costs.insert(utf_cfg,cost);
        if(profile_defined)
        {
            /** Check duplicate **/
            if (profiles.exists(utf_cfg))
                fTHROW(XMLException,label,"duplicate profile specification of fraction %s in pool %s",
                       (char const *)utf_cfg, (char const *)utf_pool_name);

            /* add profile conditions to parsed label values*/
            std::list<double>::iterator pc;
            for(pc=profile_conditions_.begin(); pc!=profile_conditions_.end();++pc)
                input_profile_->addCondition(*pc);

            /** Check profile consistency **/
            fWARNING("#cond= %d  vs. #val= %d", int(input_profile_->getConditions().size()), int(input_profile_->getValues().size()));
            if(input_profile_->getConditions().size()!=input_profile_->getValues().size())
                fTHROW(XMLException,label,"The specified profile values of fraction %s and profile conditions are inconsistent with each other!",
                       (char const *)utf_cfg);

            profiles.insert(utf_cfg,*input_profile_);
        }
                // Next Sep-Element
		label = XMLElement::nextNode(label);
	}
	while (label != 0);

	// Konfigurationen der Fractions prüfen und Maske konfigurieren
	charptr_map< Array< double > >::const_iterator vi;
	for (vi=values.begin(); vi!=values.end(); vi++)
	{
            BitArray * vi_cfg = BitArray::parseBin(vi->key);
            fASSERT(vi_cfg);
            vi_cfg->resize(natoms,false);
            mask = mask | (*vi_cfg);
            delete vi_cfg;
	}

	// InputPool-Objekt anlegen
	input_pool_ = new data::InputPool(
			utf_id,
			(char const *)utf_pool_name,
			mask,
			ip_type
			);

	// Fractions eintragen
	for (vi=values.begin(); vi!=values.end(); vi++)
	{
		BitArray * vi_cfg = BitArray::parseBin(vi->key);
		fASSERT(vi_cfg);
		vi_cfg->resize(natoms,false);
		switch (ip_type)
		{
		case data::InputPool::ip_isotopomer:
			input_pool_->setIsotopomerValue(
					*vi_cfg,
					vi->value[0],
					purities[vi->key],
					costs[vi->key]
					);
			break;
		case data::InputPool::ip_cumomer:
			input_pool_->setCumomerValue(
					*vi_cfg,
					vi->value[0]
					);
			break;
		case data::InputPool::ip_emu:
			input_pool_->setEMUValue(
					*vi_cfg,
					vi->value
					);
			break;
		}
		delete vi_cfg;
	}

	// der geparste Pool wird bei der später folgenden Validierung ggfs.
	// von der Isotopomer-Form in die Cumomer-Form konvertiert

        // Profile eintragen
        charptr_map< data::InputProfile >::const_iterator pi;
	for (pi=profiles.begin(); pi!=profiles.end();pi++)
	{
            BitArray * vi_cfg = BitArray::parseBin(pi->key);
            fASSERT(vi_cfg);
            vi_cfg->resize(natoms,false);
                    input_pool_->setInputProfileValue(
                                    *vi_cfg,
                                    pi->value,
                                    purities[pi->key],
                                    costs[pi->key]
                                    );
            delete vi_cfg;
	}

        // Registrierung von Isotope-Konfigurationens zur Ermöglichung von
        // multi-Isotopic Tracer MFA Simulation,
        input_pool_->setIsotopeCfg(ipool->getIsotopesCfg());
}

data::Pool * FluxMLInput::lookupPool(char const * pool_name)
{
	charptr_map< data::Pool* > * pmap = doc_->getPoolMap();
	data::Pool ** pptr = pmap->findPtr(pool_name);
	if (pptr == 0)
		return 0;
	return *pptr;
}

Array< double > FluxMLInput::parseLabel(DOMElement * label)
{
	struct val_list_t
	{
		double value;
		val_list_t * next;

		val_list_t() : value(0),next(0) { }
		~val_list_t() { delete next; }
	} head, * walk;
	bool next_is_sep = false;
        double sum;
	size_t size = 0;

        DOMNode * child = XMLElement::skipJunkNodes(label->getFirstChild());
        walk = &head;
        while (child != 0)
        {
        	if(profile_defined) // parse the profile and skip this while loop
        	{
        		parseProfile(child);
        	}
            // standard labeling specification
        	else if (next_is_sep)
        	{
        		if (not XMLElement::match(child,fml_sep,fml_xmlns_uri))
        			fTHROW(XMLException,child,"element node (sep) expected.");
        	}
        	else
        	{
        		if (child->getNodeType() != DOMNode::TEXT_NODE)
        			fTHROW(XMLException,child,"#PCDATA element expected.");

        		if (not XMLElement::parseDouble(
        				static_cast< DOMText* >(child)->getData(),
						walk->value))
        		{
        			U2A utf_value(static_cast< DOMText* >(child)->getData());
        			fTHROW(XMLException,child,
        					"error parsing fraction value: %s",
							(char const*)utf_value
        			);
        		}

        		if (walk->value < 0. or walk->value > 1.)
        		{
        			U2A utf_value(static_cast< DOMText* >(child)->getData());
        			fTHROW(XMLException,child,
        					"labeling fraction value %s is out of range [0,1]",
							(char const*)utf_value
        			);
        		}

        		++size;
        		walk->next = new val_list_t;
        		walk = walk->next;
        	}
        	next_is_sep = not next_is_sep;
        	child = XMLElement::nextNode(child);
        }

        Array< double >values(size);
        walk = &head;
        sum = 0.;
        for (size_t k=0; k<size; ++k)
        {
        	values[k] = walk->value;
        	sum += walk->value;
        	walk = walk->next;
        }
        if (size > 1 and fabs(1.-sum) > 1e-5)
        {
        	fTHROW(XMLException,label,
        			"labeling fraction values are out of range [0,1]");
        }
        return values;
}

void  FluxMLInput::parseProfile(DOMNode * profile)
{
	input_profile_ =  new data::InputProfile();

	if (XMLElement::match(profile,fml_math,fml_xmlns_mathml_uri))
		parseProfileMathML(profile);
	else if (XMLElement::match(profile,fml_textual,fml_xmlns_uri))
		parseProfileTextual(profile);
	else
		fTHROW(XMLException,profile,
				"values in textual or MathML notation expected");
}

void FluxMLInput::parseProfileMathML(
	DOMNode * node
	)
{
	MathMLDocument * mathml;
	MathMLDeclare const * mathml_def;
	MathMLContentObject const * mathml_cont;
	MathMLExpression const * mathml_expr;
	std::string mathml_cname;
	ExprTree * expr;

	// den MathML-Parser anwerfen:
	try
	{
		mathml = new MathMLDocument( node );
	}
	catch (XMLException & e)
	{
		// MathML-Fehler in FluxML-Fehler konvertieren:
		fTHROW(XMLException,node,"MathML-error in profile: %s", e.toString());
	}

	// handelt es sich bei den Profile aus dem MathML-Dokument wirklich
	// um Profile?
	std::list< MathMLDeclare const * > mml_constr = mathml->getDefinitionsByRegExp(".*");

	// Sinnüberprüfung der Profile
	std::list< MathMLDeclare const * >::iterator l_iter = mml_constr.begin();
	while (l_iter != mml_constr.end())
	{
		mathml_def = *l_iter;
		mathml_cont = mathml_def->getValue();
		mathml_cname = mathml_def->getName();

		// Profile sind einfache MathMLExpressions:
		if (mathml_cont->getType() != MathMLContentObject::co_expression)
			fTHROW(XMLException,node,"Profile %s: simple MathML-expression expected.",
					mathml_cname.c_str());

		mathml_expr = static_cast<MathMLExpression const*>(mathml_cont);
		expr = mathml_expr->get();
                // Überprüfung der Variablennamen
                charptr_array vnames = expr->getVarNames();
                charptr_array::const_iterator vni;

                for (vni = vnames.begin(); vni != vnames.end(); vni++)
                {
                    if((std::strcmp(*vni,"t") != 0) and (std::strcmp(*vni,"otherwise") != 0) )
                            fTHROW(XMLException,node,
                                    "invalid variable name \"%s\" in "
                                    "profile specification \"%s\" (%s)",
                                    *vni, input_profile_->getName(), expr->toString().c_str());
                };

                input_profile_->addValue(expr);
                l_iter++;
	}

	// MathML-Dokument freigeben:
	delete mathml;
}

void FluxMLInput::parseProfileTextual(
	DOMNode * node
	)
{
	DOMNode * child;
	char const anon[] = "anonymous";
	if (not XMLElement::match(node,fml_textual,fml_xmlns_uri))
		fTHROW(XMLException,node,"element node (textual) expected.");
	child = node->getFirstChild();

	while (child != 0)
	{
		// unter Element textual ist alles erlaubt -- ausgewertet
		// werden nur TEXT_NODEs
		while (child != 0 && child->getNodeType() != DOMNode::TEXT_NODE)
			child = child->getNextSibling();

		if (child == 0)
			break;

		U2A cblock(static_cast< DOMText* >(child)->getData());
		charptr_array expr_list = charptr_array::split((char const*)cblock,";");
		charptr_array::const_iterator i;

		for (i=expr_list.begin(); i!=expr_list.end(); i++)
		{
			ExprTree * expr;
			int ci;
			char * cname, * u;
			char const * w;
			for (w=*i; *w!='\0'; w++)
				if (*w > 32) break;
			if (*w == '\0')
				continue;

			charptr_array nv_pair = charptr_array::split(*i,":");
			switch (nv_pair.size())
			{
			case 1:
				ci = 0;
				cname = strdup_alloc(anon);
				break;
			case 2:
				ci = 1;
				cname = strdup_alloc(nv_pair[0]);
				for (u=cname; *u!='\0'; u++)
					if (*u < 32) *u = ' ';
				strtrim_inplace(cname);
				break;
			default:
				fTHROW(XMLException,child,
					"parse error in profile (%s): "
					"confused by too many ':'", *i);
			}

			try
			{
				expr = ExprTree::parse(nv_pair[ci]);
			}
			catch (ExprParserException &)
			{
				fTHROW(XMLException,child,"parse error in profile %s(%s)",
					cname, nv_pair[ci]);
			}

                        // Überprüfung der Variablennamen
                        charptr_array vnames = expr->getVarNames();
                        charptr_array::const_iterator vni;

                        for (vni = vnames.begin(); vni != vnames.end(); vni++)
                        {
                            if((std::strcmp(*vni,"t") != 0) and (std::strcmp(*vni,"otherwise") != 0) )
                                    fTHROW(XMLException,child,
                                            "invalid variable name \"%s\" in "
                                            "profile specification \"%s\" (%s)",
                                            *vni, input_profile_->getName(), nv_pair[ci]);
                        };

                        input_profile_->addValue(expr);

			delete[] cname;
			delete expr;
		}
		child = child->getNextSibling();
	}
}
} // namespace flux::xml
} // namespace flux


#include "liver/fill_liver_volume.hpp"
#include "liver/physical_vessel.hpp"
#include "liver/physical_vessel_tree_updater.hpp"
#include "messages/vessel_tree.pb.h"
#include "shape/voxelized_shape.hpp"
#include "utility/binary_tree.hpp"
#include "utility/git_hash.hpp"
#include "utility/options.hpp"
#include "utility/protobuf_zip_ostream.hpp"
#include <boost/filesystem.hpp>
#include <boost/optional.hpp>
#include <boost/program_options.hpp>
#include <fmt/ostream.h>
#include <google/protobuf/io/coded_stream.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <chrono>
#include <iostream>
#include <random>

using namespace jhmi;
namespace fs = boost::filesystem;
namespace po = boost::program_options;

struct node {
  m3 pt;
  binary_node_t<physical_vessel> n;
};
int main(int argc, char* argv[]) {
  try {
    GOOGLE_PROTOBUF_VERIFY_VERSION;
    auto opts = options<data_directory_option, output_path_option>{};
    if (!opts.parse(argc, argv))
      return 1;

    auto liver = voxelized_shape{opts.data_directory() / "liver_extents.datz", adjust::do_open};
    auto full_start = std::chrono::high_resolution_clock::now();
    auto tracts = tracts_in(liver) | ranges::view::transform([](m3 pt) { return node{pt, {}}; }) | ranges::to_vector;
    auto num_levels = std::ceil(std::log2(tracts.size())) + 1;
    auto levels = std::vector<std::vector<node>>(num_levels);
    levels.back() = std::move(tracts);

    auto vend = dbl3{45.447, 85.777, 54.023} * mm;
    for (int lvl = 1; lvl < num_levels; ++lvl) {
      auto& old_lvl = levels[num_levels - lvl];
      auto& new_lvl = levels[num_levels - lvl - 1];
      auto p = double(lvl) / (num_levels - 1);
      new_lvl = old_lvl | ranges::view::chunk(2)
        | ranges::view::transform([=](auto&& pts) {
            auto ctr = ranges::accumulate(pts | ranges::view::transform(&node::pt), m3{}) / double(pts.size());
            return node{ctr * (1. - p) + vend * p, {}};
        }) | ranges::to_vector;
    }
    int idx = 0;
    auto cell_pressure = Pa{25_mmHg};
    auto proper_ha_flow = 400. * mL / minutes;
    auto cell_flow = cubic_meters_per_second{proper_ha_flow / double(tracts.size())};
    auto vessels = binary_tree<physical_vessel>{
      physical_vessel{dbl3{50.143, 78.571, 47.562} * mm, vend, 10_um,
          cell_id::invalid(), cell_flow, cell_pressure, vessel_id{++idx}}};
    levels[0][0].n = vessels.root();
    for (int r = 1; r < num_levels; ++r) {
      for (int c = 0; c < levels[r].size(); ++c) {
        auto& parent = levels[r - 1][c / 2];
        auto& child = levels[r][c];
        auto dv = physical_vessel{parent.n.value().end(), vend, 10_um,
              cell_id::invalid(), cell_flow, cell_pressure, vessel_id{++idx}};
        if (c % 2 == 0)
          child.n = parent.n.set_left_child(dv);
        else
          child.n = parent.n.set_right_child(dv);
      }
    }
    //Now, update flow, pressure, and radii.
    auto vu = physical_vessel_tree_updater{vessels, 2.7, cell_pressure};
    vu.normalize_all();

    //tree is populated now, write to disk.
    auto p = opts.output_path() / "run_50_50";
    fs::create_directories(p);

    protobuf_zip_ostream out_stream{p / "fifty_tree.pbz"};
    jhmi_message::VesselTree vt;
    RANGES_FOR(auto&& n, vessels | view::node_level_order) {
      auto p = n.parent();
      auto r = n.right_child();
      auto l = n.left_child();
      auto vtv = vt.add_vessels();
      auto& v = n.value();
      vtv->set_id(v.id().value());
      vtv->set_parent((p ? p.value().id() : vessel_id::invalid()).value());
      vtv->set_left(  (l ? l.value().id() : vessel_id::invalid()).value());
      vtv->set_right( (r ? r.value().id() : vessel_id::invalid()).value());
      vtv->set_radius(v.radius().value());
      vtv->set_cell(v.cell().value());
      vtv->set_flow(v.flow().value());
      vtv->set_entry_pressure(v.entry_pressure().value());
      vtv->set_exit_pressure(v.exit_pressure().value());
      vtv->set_sx(v.start().x.value());
      vtv->set_sy(v.start().y.value());
      vtv->set_sz(v.start().z.value());
      vtv->set_ex(v.end().x.value());
      vtv->set_ey(v.end().y.value());
      vtv->set_ez(v.end().z.value());
      vtv->set_is_const(false);
    }
    if (!vt.SerializeToZeroCopyStream(out_stream.get()))
      throw std::runtime_error("Failed to write 50/50 tree.");

    auto full_stop = std::chrono::high_resolution_clock::now();
    fmt::print("Done in {} s!\n",
      std::chrono::duration<float>(full_stop - full_start).count());
    google::protobuf::ShutdownProtobufLibrary();
  }
  catch(std::exception const& e) {
    fmt::print("Caught exception: {}\n", e.what());
  }
  return 0;
}


// I hate this, might rewrite in future

#include <iostream>
#include <bass.h>
#include <thread>
#include <future>
#include <string>
#include <Windows.h>
#include <random>
#include <iomanip>
#include <cmath>
#include <boost/filesystem.hpp>
#include "clearscreen.h"
#include "saveandload.h"
#include "buydrugs.h"
#include "selldrugs.h"

// global code yes i know plz dont kill me >.<
typedef int(__cdecl* MYPROC)(int);
HINSTANCE hinstLib;
MYPROC dlcMagic;
BOOL fFreeResult, fRunTimeLinkSuccess = FALSE;

// dll exporting lmao
__declspec(dllexport) std::string name;
bool globalexit = FALSE;
bool musicon = TRUE;
bool gameprestart = FALSE;
bool gamestarted = FALSE;
__declspec(dllexport) float money = 1000;
__declspec(dllexport) int debt = 1000;
__declspec(dllexport) int days = 0;
int weedprice;
int cocaineprice;
int acidprice;
int lsdprice;
int condomprice;
int raid;
__declspec(dllexport) int weedpocket = 0;
__declspec(dllexport) int cocainepocket = 0;
__declspec(dllexport) int acidpocket = 0;
__declspec(dllexport) int lsdpocket = 0;
__declspec(dllexport) int condompocket = 0;
int cheatused = 0;
bool dlc = FALSE;
__declspec(dllexport) int dlcPocket;
__declspec(dllexport) int dlcPrice;
__declspec(dllexport) std::string dlcDrugName;
__declspec(dllexport) std::string dlcDrugLetter;
__declspec(dllexport) std::random_device rd;
__declspec(dllexport) std::mt19937 eng(rd());

void dubstep()
{
	BASS_Free();
	BASS_Init(-1, 48000, 0, 0, 0);
	HSTREAM stream = BASS_StreamCreateFile(FALSE, "resource.bin", 0, 0, BASS_SAMPLE_LOOP);
	BASS_ChannelPlay(stream, true);
	while (true)

		if (globalexit == TRUE)
		{
			return;
		}
		else if (musicon == TRUE)
		{
			DWORD playing = BASS_ChannelIsActive(stream);
			if (playing == BASS_ACTIVE_PAUSED)
			{
				BASS_ChannelPlay(stream, true);
			}
			else
				std::this_thread::sleep_for(std::chrono::seconds(1));
		}
		else if (musicon == FALSE)
		{
			BASS_ChannelPause(stream);
		}

}

void makedrugprice()
{
	std::uniform_int_distribution<> weedpricerange(230, 700);
	weedprice = weedpricerange(eng);
	// std::cout << weedprice << '\n';

	std::uniform_int_distribution<> cocainepricerange(8000, 18000);
	cocaineprice = cocainepricerange(eng);
	// std::cout << cocaineprice << '\n';

	std::uniform_int_distribution<> acidpricerange(700, 3000);
	acidprice = acidpricerange(eng);
	// std::cout << acidprice << '\n';

	std::uniform_int_distribution<> lsdpricerange(2500, 3500);
	lsdprice = lsdpricerange(eng);
	// std::cout << lsdprice << '\n';

	std::uniform_int_distribution<> condompricerange(7, 50);
	condomprice = condompricerange(eng);
	// std::cout << condomprice << '\n';

	std::uniform_int_distribution<> raidchance(1, 100);
	raid = raidchance(eng);

	if (dlc == TRUE)
	{
		dlcMagic(8);
	}
	if (raid == 1 && days != 1)
	{
		weedprice = weedprice + 300;
		cocaineprice = cocaineprice + 3200;
		acidprice = acidprice + 600;
		lsdprice = lsdprice + 2000;
		if (dlc == TRUE)
		{
			dlcMagic(9);
		}
		std::cout << "damn, a police raid. prices have gone through the roof" << '\n';
	}

	return;
}

void nextday()
{
	clearscreen();
	if (debt < 0)
	{
		debt = 0;
	}
	if (debt != 0)
	{
		debt = debt*pow((1+0.01),days);
	}
	days++;
	makedrugprice();
	return;
}

void payloan()
{
	clearscreen();
	if (debt == 0)
	{
		std::cout << "you dont have a loan to pay!" << '\n';
		std::this_thread::sleep_for(std::chrono::seconds(2));
		return;
	}
	else
	{
		std::cout << "how much of your loan do you want to pay? \nyou owe $" << debt << " and you have $" << money << '\n';
		int loanpay;
		std::cin >> loanpay;
		if (loanpay > money)
		{
			std::cout << "you dont have enough money" << '\n';
			return;
		}
		if (loanpay <= money && loanpay <= debt)
		{
			std::cout << "you will have a debt of $" << debt - loanpay << " and you will have $" << money - loanpay << " in cash" << '\n' << '\n';
			std::cout << "do you want to pay off your debt? y for yes, n for no" << '\n';
			char loanpayconfirm;
			std::cin >> loanpayconfirm;
			switch (loanpayconfirm)
			{
			case 'y':
				clearscreen();
				debt = debt - loanpay;
				money = money - loanpay;
				break;
			case 'n':
				clearscreen();
				break;
			default:
				clearscreen();
				std::cout << "invalid option" <<'\n';
				std::this_thread::sleep_for(std::chrono::seconds(2));
				break;
			}
		}
		return;
	}
}

void sellmenu2()
{
	std::cout << "okay, which drug would you like to sell? type in the drug initial then press enter." << '\n';
	if (dlc == TRUE)
	{
		std::cout << "type d for the dlc menu (yeah it sucks idk how to do this better unless i rewrite the whole thing \n";
	}
	char sellmenu2;
	std::cin >> sellmenu2;
	switch (sellmenu2)
	{
	case 'w':
		sellweed();
		break;
	case 'c':
		sellcocaine();
		break;
	case 'a':
		sellacid();
		break;
	case 'l':
		selllsd();
		break;
	case 'C':
		sellcondoms();
		break;
	case 'd':
		if (dlc == TRUE)
		{
			dlcMagic(6);
			break;
		}
		else {
			std::cout << "no dlc \n";
			break;
		}
	default:
		std::cout << "invalid drug name. make sure you type in the initial letter of the drug then press enter. cocaine is c and condoms are C" << '\n';
		std::this_thread::sleep_for(std::chrono::seconds(2));
		break;
	}
	return;
}

void sellmenu1()
{
	clearscreen();
	bool selling = TRUE;
	while (selling == TRUE)
	{
		std::cout << "welcome to the sell menu. you have $" << money << '\n' << '\n';
		std::cout << "current drugs in your pockets" << '\n';
		std::cout << "weed: " << weedpocket << '\n';
		std::cout << "cocaine: " << cocainepocket << '\n';
		std::cout << "acid: " << acidpocket << '\n';
		std::cout << "lsd: " << lsdpocket << '\n';
		std::cout << "Condoms: " << condompocket << '\n';
		if (dlc == TRUE)
		{
			std::cout << dlcDrugName << ": " << dlcPocket << '\n' << '\n';
		}
		else std::cout << '\n';
		std::cout << "current drug prices" << '\n';
		if (raid == 1)
			std::cout << "there has been a police raid, prices are increased" << '\n';
		std::cout << "weed: $" << weedprice << '\n';
		std::cout << "cocaine: $" << cocaineprice << '\n';
		std::cout << "acid: $" << acidprice << '\n';
		std::cout << "lsd: $" << lsdprice << '\n';
		std::cout << "Condoms: $" << condomprice << '\n';
		if (dlc == TRUE)
		{
			std::cout << dlcDrugName << ": $" << dlcPrice << '\n' << '\n';
		}
		else std::cout << '\n';
		std::cout <<
			"s: sell some drugs \n"
			"e: exit the buy menu \n" << '\n';
		char sellmenu1;
		std::cin >> sellmenu1;
		switch (sellmenu1)
		{
		case 's':
			sellmenu2();
			break;
		case 'e':
			selling = FALSE;
			break;
		default:
			std::cout << "invalid option" << '\n';
			break;
		}
	}
	clearscreen();
	return;
}

void buymenu2()
{
	std::cout << "okay, which drug would you like to buy? type in the drug initial then press enter." << '\n';
	if (dlc == TRUE)
	{
		std::cout << "type d for the dlc menu (yeah it sucks idk how to do this better unless i rewrite the whole thing \n";
	}

		char buymenu2menu;
		std::cin >> buymenu2menu;
		switch (buymenu2menu)
		{
		case 'w':
			buyweed();
			break;
		case 'c':
			buycocaine();
			break;
		case 'a':
			buyacid();
			break;
		case 'l':
			buylsd();
			break;
		case 'C':
			buycondoms();
			break;
		case 'd':
			if (dlc == TRUE)
			{
				dlcMagic(5);
				break;
			}
			else {
				std::cout << "no dlc \n";
				break;
			}
		default:
			std::cout << "invalid drug name. make sure you type in the initial letter of the drug then press enter. cocaine is c and condoms are C" << '\n';
			std::this_thread::sleep_for(std::chrono::seconds(2));
			break;
		}

	return;
}

void buymenu1()
{
	clearscreen();
	bool buying = TRUE;
	while (buying == TRUE)
	{
		std::cout << "welcome to the buy menu. you have $" << money << '\n' << '\n';
		std::cout << "current drugs in your pockets" << '\n';
		std::cout << "weed: " << weedpocket << '\n';
		std::cout << "cocaine: " << cocainepocket << '\n';
		std::cout << "acid: " << acidpocket << '\n';
		std::cout << "lsd: " << lsdpocket << '\n';
		std::cout << "Condoms: " << condompocket << '\n';
		if (dlc == TRUE)
		{
			std::cout << dlcDrugName << ": " << dlcPocket << '\n' <<'\n';
		}
		else std::cout << '\n';
		std::cout << "current drug prices" << '\n';
		if (raid == 1)
			std::cout << "there has been a police raid, prices are increased" << '\n';
		std::cout << "weed: $" << weedprice << '\n';
		std::cout << "cocaine: $" << cocaineprice << '\n';
		std::cout << "acid: $" << acidprice << '\n';
		std::cout << "lsd: $" << lsdprice << '\n';
		std::cout << "Condoms: $" << condomprice << '\n';
		if (dlc == TRUE)
		{
			std::cout << dlcDrugName << ": $" << dlcPrice << '\n' << '\n';
		}
		else std::cout << '\n';
		std::cout <<
			"b: buy some drugs \n"
			"e: exit the buy menu \n" << '\n';
		char buymenu1;
		std::cin >> buymenu1;
		switch (buymenu1)
		{
		case 'b':
			buymenu2();
			break;
		case 'e':
			buying = FALSE;
			break;
		default:
			std::cout << "invalid option" << '\n';
			break;
		}
	}
	clearscreen();
	return;
}

int main(int argc, char** argv)
{
	// DLC checking but im lazy to seperate it
	// i also don't know what this code does or how it works :^)
	// ms dev docs ftw
	if (argc == 2) {
		std::string dlcFileName = argv[1];
		std::wstring dlcFileNameWide(dlcFileName.begin(), dlcFileName.end());
		if (boost::filesystem::exists(dlcFileName))
		{
			LPCWSTR lpcwstrLMAO = dlcFileNameWide.c_str();
			hinstLib = LoadLibrary(lpcwstrLMAO);
			if (hinstLib != NULL)
			{
				dlcMagic = (MYPROC)GetProcAddress(hinstLib, "dlcMagic"); // ?dlcMagic@@YAXH@Z
				if (dlcMagic == NULL)
				{
					std::cout << "loading DLC failed, GetProcAddress error code: \n";
					std::cout << GetLastError() << '\n';
				}
				if (NULL != dlcMagic)
				{
					std::cout << "we have successfully loaded function from DLL \n";
					dlc = TRUE;
					int pirate = dlcMagic(1);
					if (pirate == 202)
					{
						return 0;
					}
					else if (pirate == 0)
						std::cout << '\n';
				}
			}
		}
	}

	std::cout << "what is your name?" << '\n';
	std::getline(std::cin, name);
	clearscreen();

	auto music = std::async(std::launch::async, dubstep);

	// main menu
	if (days == 0)
	{
		bool validoption = FALSE;
		while (validoption == FALSE)
		{
			std::cout << "hi " << name << " and welcome to cpp dope wars" << '\n';
			if (dlc == TRUE)
			{
				std::cout << "you have the following DLC loaded \n";
				dlcMagic(2);
				std::cout << '\n' << '\n';
			}
			else std::cout << '\n';
			std::cout <<
				"p: start new game \n"
				"l: load game \n"
				"e: exit program :( \n"
				"m: toggle music \n"
				"a: about \n"
				"make your selection then press enter \n";

			char mainmenu;
			std::cin >> mainmenu;

			switch (mainmenu)
			{
			case 'p':
				std::cout << "starting game";
				days++;
				gameprestart = TRUE;
				validoption = TRUE;
				break;
			case 'e':
				globalexit = TRUE;
				validoption = TRUE;
				break;
			case 'm':
				if (musicon == TRUE)
				{
					musicon = FALSE;
					clearscreen();
					break;
				}
				if (musicon == FALSE)
				{
					musicon = TRUE;
					clearscreen();
					break;
				}
			case 'l':
				std::cout << "attempting to load save file \n";
				if (dlc == TRUE)
				{
					if (dlcMagic(3) == 201)
					{
						std::cout << "exiting.... \n";
						std::this_thread::sleep_for(std::chrono::seconds(2));
						globalexit = TRUE;
						return 0;
					}
					else if (dlcMagic(3) == 200)
						std::cout << ' ';
				}
				else loadfromfile();
				gameprestart = TRUE;
				validoption = TRUE;
				break;
			case 'h':
				clearscreen();
				std::cout << "help is on the way soon tm (mfw its been like 1 year and still no help)" << '\n';
				break;
			case 'a':
				std::cout << "version 7, dlc aware with dlc compatible logic" << '\n';
				std::this_thread::sleep_for(std::chrono::seconds(2));
				clearscreen();
				break;
			default:
				clearscreen();
				std::cout << "you did not select any of the listed options, please try again" << '\n' << '\n';
				std::this_thread::sleep_for(std::chrono::seconds(2));
				break;
			}
		}
	}

	// initialise game
	if (gameprestart == TRUE)
	{
		clearscreen();
		makedrugprice();
		gamestarted = TRUE;
	}

	// game menu
	while (gamestarted == TRUE)
	{
		{
			std::cout << "Welcome to day " << days << " of CPP Dope Wars. You owe $" << debt << " to the loan shark. You currently have $" << money << '\n';
			std::cout <<
				"b: buy menu \n"
				"v: sell menu \n"
				"p: pay off your loan (if you have one) \n"
				"n: next day \n"
				"m: toggle music \n"
				"e: exit :( \n"
				"l: load game \n"
				"s: save game \n"
				"make your selection then press enter \n";
			char gamemenu;
			std::cin >> gamemenu;

			switch (gamemenu)
			{
			case 'b':
				buymenu1();
				break;
			case 'v':
				sellmenu1();
				break;
			case 'n':
				nextday();
				break;
			case 'p':
				payloan();
				break;
			case 'e':
				globalexit = TRUE;
				gamestarted = FALSE;
				break;
			case 'm':
				if (musicon == TRUE)
				{
					musicon = FALSE;
					clearscreen();
					break;
				}
				if (musicon == FALSE)
				{
					musicon = TRUE;
					clearscreen();
					break;
				}
			case 'l':
				std::cout << "attempting to load save file \n";
				if (dlc == TRUE)
				{
					if (dlcMagic(3) == 201)
					{
						std::cout << "exiting.... \n";
						std::this_thread::sleep_for(std::chrono::seconds(2));
						globalexit = TRUE;
						return 0;
					}
					else if (dlcMagic(3) == 200)
						std::cout << ' ';
				}
				else loadfromfile();
				clearscreen();
				break;
				break;
			case 's':
				std::cout << "saving game progress to file..." << '\n';
				if (dlc == TRUE)
				{
					dlcMagic(4);
				}
				else savetofile();
				clearscreen();
				break;
			case '=':
				if (cheatused < 4)
				{
					cheatused = cheatused + 1;
					money = money + 1000;
					clearscreen();
					break;
				}
				if (cheatused > 4)
				{
					clearscreen();
					break;
				}
			default:
				clearscreen();
				std::cout << "you did not select any of the listed options, please try again" << '\n';
				std::this_thread::sleep_for(std::chrono::seconds(2));
				break;
			}
		}
	}

	fFreeResult = FreeLibrary(hinstLib);
	return 0;
}


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga 2005-2013. Distributed under the Boost
// Software License, Version 1.0. (See accompanying file
// LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/container for documentation.
//
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_CONTAINER_DETAIL_NODE_POOL_IMPL_HPP
#define BOOST_CONTAINER_DETAIL_NODE_POOL_IMPL_HPP

#ifndef BOOST_CONFIG_HPP
#  include <boost/config.hpp>
#endif

#if defined(BOOST_HAS_PRAGMA_ONCE)
#  pragma once
#endif

#include <boost/container/detail/config_begin.hpp>
#include <boost/container/detail/workaround.hpp>
#include <boost/container/container_fwd.hpp>

#include <boost/container/detail/math_functions.hpp>
#include <boost/container/detail/mpl.hpp>
#include <boost/container/detail/pool_common.hpp>
#include <boost/container/detail/to_raw_pointer.hpp>
#include <boost/container/detail/type_traits.hpp>

#include <boost/intrusive/pointer_traits.hpp>
#include <boost/intrusive/set.hpp>
#include <boost/intrusive/slist.hpp>

#include <boost/core/no_exceptions_support.hpp>
#include <boost/assert.hpp>
#include <cstddef>

namespace boost {
namespace container {
namespace container_detail {

template<class SegmentManagerBase>
class private_node_pool_impl
{
   //Non-copyable
   private_node_pool_impl();
   private_node_pool_impl(const private_node_pool_impl &);
   private_node_pool_impl &operator=(const private_node_pool_impl &);

   //A node object will hold node_t when it's not allocated
   public:
   typedef typename SegmentManagerBase::void_pointer              void_pointer;
   typedef typename node_slist<void_pointer>::slist_hook_t        slist_hook_t;
   typedef typename node_slist<void_pointer>::node_t              node_t;
   typedef typename node_slist<void_pointer>::node_slist_t        free_nodes_t;
   typedef typename SegmentManagerBase::multiallocation_chain     multiallocation_chain;
   typedef typename SegmentManagerBase::size_type                 size_type;

   private:
   typedef typename bi::make_slist
      < node_t, bi::base_hook<slist_hook_t>
      , bi::linear<true>
      , bi::constant_time_size<false> >::type      blockslist_t;

   static size_type get_rounded_size(size_type orig_size, size_type round_to)
   {  return ((orig_size-1)/round_to+1)*round_to;  }

   public:

   //!Segment manager typedef
   typedef SegmentManagerBase segment_manager_base_type;

   //!Constructor from a segment manager. Never throws
   private_node_pool_impl(segment_manager_base_type *segment_mngr_base, size_type node_size, size_type nodes_per_block)
   :  m_nodes_per_block(nodes_per_block)
   ,  m_real_node_size(lcm(node_size, size_type(alignment_of<node_t>::value)))
      //General purpose allocator
   ,  mp_segment_mngr_base(segment_mngr_base)
   ,  m_blocklist()
   ,  m_freelist()
      //Debug node count
   ,  m_allocated(0)
   {}

   //!Destructor. Deallocates all allocated blocks. Never throws
   ~private_node_pool_impl()
   {  this->purge_blocks();  }

   size_type get_real_num_node() const
   {  return m_nodes_per_block; }

   //!Returns the segment manager. Never throws
   segment_manager_base_type* get_segment_manager_base()const
   {  return container_detail::to_raw_pointer(mp_segment_mngr_base);  }

   void *allocate_node()
   {  return this->priv_alloc_node();  }

   //!Deallocates an array pointed by ptr. Never throws
   void deallocate_node(void *ptr)
   {  this->priv_dealloc_node(ptr); }

   //!Allocates a singly linked list of n nodes ending in null pointer.
   void allocate_nodes(const size_type n, multiallocation_chain &chain)
   {
      //Preallocate all needed blocks to fulfill the request
      size_type cur_nodes = m_freelist.size();
      if(cur_nodes < n){
         this->priv_alloc_block(((n - cur_nodes) - 1)/m_nodes_per_block + 1);
      }

      //We just iterate the needed nodes to get the last we'll erase
      typedef typename free_nodes_t::iterator free_iterator;
      free_iterator before_last_new_it = m_freelist.before_begin();
      for(size_type j = 0; j != n; ++j){
         ++before_last_new_it;
      }

      //Cache the first node of the allocated range before erasing
      free_iterator first_node(m_freelist.begin());
      free_iterator last_node (before_last_new_it);

      //Erase the range. Since we already have the distance, this is O(1)
      m_freelist.erase_after( m_freelist.before_begin()
                            , ++free_iterator(before_last_new_it)
                            , n);

      //Now take the last erased node and just splice it in the end
      //of the intrusive list that will be traversed by the multialloc iterator.
      chain.incorporate_after(chain.before_begin(), &*first_node, &*last_node, n);
      m_allocated += n;
   }

   void deallocate_nodes(multiallocation_chain &chain)
   {
      typedef typename multiallocation_chain::iterator iterator;
      iterator it(chain.begin()), itend(chain.end());
      while(it != itend){
         void *pElem = &*it;
         ++it;
         this->priv_dealloc_node(pElem);
      }
   }

   //!Deallocates all the free blocks of memory. Never throws
   void deallocate_free_blocks()
   {
      typedef typename free_nodes_t::iterator nodelist_iterator;
      typename blockslist_t::iterator bit(m_blocklist.before_begin()),
                                      it(m_blocklist.begin()),
                                      itend(m_blocklist.end());
      free_nodes_t backup_list;
      nodelist_iterator backup_list_last = backup_list.before_begin();

      //Execute the algorithm and get an iterator to the last value
      size_type blocksize = (get_rounded_size)
         (m_real_node_size*m_nodes_per_block, (size_type) alignment_of<node_t>::value);

      while(it != itend){
         //Collect all the nodes from the block pointed by it
         //and push them in the list
         free_nodes_t free_nodes;
         nodelist_iterator last_it = free_nodes.before_begin();
         const void *addr = get_block_from_hook(&*it, blocksize);

         m_freelist.remove_and_dispose_if
            (is_between(addr, blocksize), push_in_list(free_nodes, last_it));

         //If the number of nodes is equal to m_nodes_per_block
         //this means that the block can be deallocated
         if(free_nodes.size() == m_nodes_per_block){
            //Unlink the nodes
            free_nodes.clear();
            it = m_blocklist.erase_after(bit);
            mp_segment_mngr_base->deallocate((void*)addr);
         }
         //Otherwise, insert them in the backup list, since the
         //next "remove_if" does not need to check them again.
         else{
            //Assign the iterator to the last value if necessary
            if(backup_list.empty() && !m_freelist.empty()){
               backup_list_last = last_it;
            }
            //Transfer nodes. This is constant time.
            backup_list.splice_after
               ( backup_list.before_begin()
               , free_nodes
               , free_nodes.before_begin()
               , last_it
               , free_nodes.size());
            bit = it;
            ++it;
         }
      }
      //We should have removed all the nodes from the free list
      BOOST_ASSERT(m_freelist.empty());

      //Now pass all the node to the free list again
      m_freelist.splice_after
         ( m_freelist.before_begin()
         , backup_list
         , backup_list.before_begin()
         , backup_list_last
         , backup_list.size());
   }

   size_type num_free_nodes()
   {  return m_freelist.size();  }

   //!Deallocates all used memory. Precondition: all nodes allocated from this pool should
   //!already be deallocated. Otherwise, undefined behaviour. Never throws
   void purge_blocks()
   {
      //check for memory leaks
      BOOST_ASSERT(m_allocated==0);
      size_type blocksize = (get_rounded_size)
         (m_real_node_size*m_nodes_per_block, (size_type)alignment_of<node_t>::value);

      //We iterate though the NodeBlock list to free the memory
      while(!m_blocklist.empty()){
         void *addr = get_block_from_hook(&m_blocklist.front(), blocksize);
         m_blocklist.pop_front();
         mp_segment_mngr_base->deallocate((void*)addr);
      }
      //Just clear free node list
      m_freelist.clear();
   }

   void swap(private_node_pool_impl &other)
   {
      BOOST_ASSERT(m_nodes_per_block == other.m_nodes_per_block);
      BOOST_ASSERT(m_real_node_size == other.m_real_node_size);
      std::swap(mp_segment_mngr_base, other.mp_segment_mngr_base);
      m_blocklist.swap(other.m_blocklist);
      m_freelist.swap(other.m_freelist);
      std::swap(m_allocated, other.m_allocated);
   }

   private:

   struct push_in_list
   {
      push_in_list(free_nodes_t &l, typename free_nodes_t::iterator &it)
         :  slist_(l), last_it_(it)
      {}

      void operator()(typename free_nodes_t::pointer p) const
      {
         slist_.push_front(*p);
         if(slist_.size() == 1){ //Cache last element
            ++last_it_ = slist_.begin();
         }
      }

      private:
      free_nodes_t &slist_;
      typename free_nodes_t::iterator &last_it_;
   };

   struct is_between
   {
      typedef typename free_nodes_t::value_type argument_type;
      typedef bool                              result_type;

      is_between(const void *addr, std::size_t size)
         :  beg_(static_cast<const char *>(addr)), end_(beg_+size)
      {}

      bool operator()(typename free_nodes_t::const_reference v) const
      {
         return (beg_ <= reinterpret_cast<const char *>(&v) &&
                 end_ >  reinterpret_cast<const char *>(&v));
      }
      private:
      const char *      beg_;
      const char *      end_;
   };

   //!Allocates one node, using single segregated storage algorithm.
   //!Never throws
   node_t *priv_alloc_node()
   {
      //If there are no free nodes we allocate a new block
      if (m_freelist.empty())
         this->priv_alloc_block(1);
      //We take the first free node
      node_t *n = (node_t*)&m_freelist.front();
      m_freelist.pop_front();
      ++m_allocated;
      return n;
   }

   //!Deallocates one node, using single segregated storage algorithm.
   //!Never throws
   void priv_dealloc_node(void *pElem)
   {
      //We put the node at the beginning of the free node list
      node_t * to_deallocate = static_cast<node_t*>(pElem);
      m_freelist.push_front(*to_deallocate);
      BOOST_ASSERT(m_allocated>0);
      --m_allocated;
   }

   //!Allocates several blocks of nodes. Can throw
   void priv_alloc_block(size_type num_blocks)
   {
      BOOST_ASSERT(num_blocks > 0);
      size_type blocksize =
         (get_rounded_size)(m_real_node_size*m_nodes_per_block, (size_type)alignment_of<node_t>::value);

      BOOST_TRY{
         for(size_type i = 0; i != num_blocks; ++i){
            //We allocate a new NodeBlock and put it as first
            //element in the free Node list
            char *pNode = reinterpret_cast<char*>
               (mp_segment_mngr_base->allocate(blocksize + sizeof(node_t)));
            char *pBlock = pNode;
            m_blocklist.push_front(get_block_hook(pBlock, blocksize));

            //We initialize all Nodes in Node Block to insert
            //them in the free Node list
            for(size_type j = 0; j < m_nodes_per_block; ++j, pNode += m_real_node_size){
               m_freelist.push_front(*new (pNode) node_t);
            }
         }
      }
      BOOST_CATCH(...){
         //to-do: if possible, an efficient way to deallocate allocated blocks
         BOOST_RETHROW
      }
      BOOST_CATCH_END
   }

   //!Deprecated, use deallocate_free_blocks
   void deallocate_free_chunks()
   {  this->deallocate_free_blocks(); }

   //!Deprecated, use purge_blocks
   void purge_chunks()
   {  this->purge_blocks(); }

   private:
   //!Returns a reference to the block hook placed in the end of the block
   static node_t & get_block_hook (void *block, size_type blocksize)
   {
      return *reinterpret_cast<node_t*>(reinterpret_cast<char*>(block) + blocksize);
   }

   //!Returns the starting address of the block reference to the block hook placed in the end of the block
   void *get_block_from_hook (node_t *hook, size_type blocksize)
   {
      return (reinterpret_cast<char*>(hook) - blocksize);
   }

   private:
   typedef typename boost::intrusive::pointer_traits
      <void_pointer>::template rebind_pointer<segment_manager_base_type>::type   segment_mngr_base_ptr_t;

   const size_type m_nodes_per_block;
   const size_type m_real_node_size;
   segment_mngr_base_ptr_t mp_segment_mngr_base;   //Segment manager
   blockslist_t      m_blocklist;      //Intrusive container of blocks
   free_nodes_t      m_freelist;       //Intrusive container of free nods
   size_type       m_allocated;      //Used nodes for debugging
};

}  //namespace container_detail {
}  //namespace container {
}  //namespace boost {

#include <boost/container/detail/config_end.hpp>

#endif   //#ifndef BOOST_CONTAINER_DETAIL_ADAPTIVE_NODE_POOL_IMPL_HPP


#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <boost/algorithm/string.hpp>
#include <tf/transform_datatypes.h>
#include <math.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <time.h>
#include "kalman-Ndof.hpp"
#include <boost/date_time/posix_time/posix_time.hpp>
#include <tms_msg_db/TmsdbGetData.h>
#include <tms_msg_db/TmsdbStamped.h>
#include <tms_msg_db/Tmsdb.h>
#include <visualization_msgs/Marker.h>

#define DEVNAME "/dev/arduino"
#define BAUDRATE B115200
#define BUFSIZE 256

#define PI 3.14159265
#define DEG2RAD(x) ((x)*(PI/180))

using namespace std;
using namespace boost;

Kalman *kalman;

#define BED_X1 9.33
#define BED_X2 10.7
#define BED_Y1 2.3
#define BED_Y2 3.1

int main(int argc, char **argv)
{
  ros::init(argc,argv,"tms_ss_pozyx");
  ros::NodeHandle n;
  ros::Publisher db_pub = n.advertise<tms_msg_db::TmsdbStamped> ("tms_db_data",1000);
  ros::Publisher pos_pub = n.advertise<visualization_msgs::Marker> ("pozyx",1000);
  std::string frame_id("/world");

	int fd = open(DEVNAME, O_RDWR | O_NOCTTY);
	if(fd<0){
		ROS_ERROR("cannot open device");
		return 0;
	}
	ROS_INFO("device opened");

	struct termios tio;
	memset(&tio,0,sizeof(tio));
	tio.c_cflag = CS8 | CLOCAL | CREAD;
	tio.c_cc[VTIME]=0;
	tio.c_lflag=ICANON;
	tio.c_iflag=IGNPAR | ICRNL;
	cfsetspeed(&tio,BAUDRATE);
	tcsetattr(fd,TCSANOW,&tio);

  kalman = new Kalman(6,6,3);
  ros::Time last_time,now;
  now = ros::Time::now();

	int len;
	char buf[BUFSIZE];
  double X,Y,Z,qx,qy,qz,qw;
  double preX,preY,preZ,preqx,preqy,preqz,preqw;
  double kfX=0;
  double kfY=0;
  double kfZ=0;
	while(ros::ok()){
		memset(&buf,0,sizeof(buf));
		len = read(fd,buf,BUFSIZE);
		if(len==0)
			continue;
	  std::string data(buf);
	  std::vector<std::string> v_data;
	  v_data.clear();
    boost::split(v_data,data,boost::is_any_of(","));
    if(v_data.size()==8){
      preX=X;
      preY=Y;
      preZ=Z;
      preqx=qx;
      preqy=qy;
      preqz=qz;
      preqw=qw;
      int id = atoi(v_data.at(0).c_str());
      X = 0.001*atoi(v_data.at(1).c_str());
      Y = 0.001*atoi(v_data.at(2).c_str());
      Z = 0.001*atoi(v_data.at(3).c_str());
      qx = atof(v_data.at(4).c_str());
      qy = atof(v_data.at(5).c_str());
      qz = atof(v_data.at(6).c_str());
      qw = atof(v_data.at(7).c_str());
      if(qx==0&&qy==0&&qz==0&&qw==0){
        qx=preqx;
        qy=preqy;
        qz=preqz;
        qw=preqw;
      }

      ROS_INFO("%d,%f,%f,%f,%f,%f,%f,%f",id,X,Y,Z,qx,qy,qz,qw);

      tf::Quaternion q0(qx,qy,qz,qw);
      tf::Matrix3x3 m(q0);
      double roll,pitch,yaw;
      m.getRPY(roll,pitch,yaw);
      yaw-=1.144898;
      double roll2,pitch2,yaw2;
      roll2=-pitch;
      pitch2=roll-PI/2;
      yaw2=yaw-PI/2;

      //kalman_filter
      last_time = now;
      now = ros::Time::now();
      double delta_t = (now.sec+now.nsec*0.000000001)-(last_time.sec+last_time.nsec*0.000000001);
      double velX = (X-preX)/delta_t;
      double velY = (Y-preY)/delta_t;
      double velZ = (Z-preZ)/delta_t;

      static int count = 0;

      if(kalman==NULL)return 0;
      if(count==0){
        count++;
        continue;
      }else if(count==1){
        // 初期化 (システム雑音，観測雑音，積分時間)
        kalman->init(0.15,0.7,0.08);
        double C[6][6];
        memset(C,0,sizeof(C));
        C[0][0]=C[1][1]=C[2][2]=1;
        kalman->setC((double *)&C);
        kalman->setX(0,X);
        kalman->setX(1,Y);
        kalman->setX(2,Z);
        kalman->setX(3,velX);
        kalman->setX(4,velY);
        kalman->setX(5,velZ);
        count++;
      }

      double A[3][6];
      memset(A,0,sizeof(A));
      A[0][0]=A[1][1]=A[2][2]=1;
      A[0][3]=A[1][4]=A[2][5]=delta_t;
      double obs[6]={X,Y,Z,velX,velY,velZ};
      double input[3]={0,0,0};

      kalman->update(obs,input);

      kfX=kalman->getX(0);
      kfY=kalman->getX(1);
      kfZ=kalman->getX(2);

      ROS_INFO("kfX=%f kfY=%f kfZ=%f",kfX,kfY,kfZ);

      //----------------

      ros::Time db_time = ros::Time::now() + ros::Duration(9*60*60);
      tms_msg_db::TmsdbStamped db_msg;

      db_msg.header.frame_id = frame_id;
      db_msg.header.stamp = db_time;
      db_msg.tmsdb.clear();
      tms_msg_db::Tmsdb tmpData;

      tmpData.time = boost::posix_time::to_iso_extended_string(db_time.toBoost());
      tmpData.name = "person_pozyx1";
      tmpData.id = 1100;
      tmpData.sensor = 0;
      tmpData.state = 1;
      tmpData.x = kfX;
      tmpData.y = kfY;
      tmpData.rr = roll2;
      tmpData.rp = pitch2;
      tmpData.ry = yaw2;
      double height = 1.1*cos(pitch2)*cos(roll2);
      if(height<0.1) height = 0.1;

      if(kfX > BED_X1 && kfX < BED_X2 && kfY > BED_Y1 && kfY < BED_Y2){
        tmpData.place = 6017;
        tmpData.z = height+0.3;
      }else{
        tmpData.place = 5001;
        tmpData.z = height;
      }

      db_msg.tmsdb.push_back(tmpData);
      db_pub.publish(db_msg);

      visualization_msgs::Marker msg;
      msg.header.frame_id = "world_link";
      msg.header.stamp = ros::Time();
      msg.type = visualization_msgs::Marker::SPHERE;
      msg.pose.position.x = kfX;
      msg.pose.position.y = kfY;
      msg.pose.position.z = kfZ;
      msg.pose.orientation.x = qx;
      msg.pose.orientation.y = qy;
      msg.pose.orientation.z = qz;
      msg.pose.orientation.w = qw;
      msg.scale.x = 0.5;
      msg.scale.y = 0.5;
      msg.scale.z = 0.5;
      msg.color.a = 0.5;
      msg.color.r = 1.0;
      pos_pub.publish(msg);
    }
  }
  return 0;
}


// 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_ALGORITHMS_DISTANCE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DISTANCE_HPP

#include <boost/concept_check.hpp>
#include <boost/mpl/if.hpp>
#include <boost/range.hpp>
#include <boost/typeof/typeof.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/reverse_dispatch.hpp>
#include <boost/geometry/core/tag_cast.hpp>

#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/algorithms/detail/throw_on_empty_input.hpp>

#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/default_distance_result.hpp>
#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/within.hpp>

#include <boost/geometry/views/closeable_view.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace distance
{

// To avoid spurious namespaces here:
using strategy::distance::services::return_type;

template <typename P1, typename P2, typename Strategy>
struct point_to_point
{
    static inline typename return_type<Strategy>::type apply(P1 const& p1,
                P2 const& p2, Strategy const& strategy)
    {
        boost::ignore_unused_variable_warning(strategy);
        return strategy.apply(p1, p2);
    }
};

template<typename Point, typename Segment, typename Strategy>
struct point_to_segment
{
    static inline typename return_type<Strategy>::type apply(Point const& point,
                Segment const& segment, Strategy const& )
    {
        typename strategy::distance::services::default_strategy
            <
                segment_tag,
                Point,
                typename point_type<Segment>::type,
                typename cs_tag<Point>::type,
                typename cs_tag<typename point_type<Segment>::type>::type,
                Strategy
            >::type segment_strategy;

        typename point_type<Segment>::type p[2];
        geometry::detail::assign_point_from_index<0>(segment, p[0]);
        geometry::detail::assign_point_from_index<1>(segment, p[1]);
        return segment_strategy.apply(point, p[0], p[1]);
    }
};

template
<
    typename Point,
    typename Range,
    closure_selector Closure,
    typename PPStrategy,
    typename PSStrategy
>
struct point_to_range
{
    typedef typename return_type<PSStrategy>::type return_type;

    static inline return_type apply(Point const& point, Range const& range,
            PPStrategy const& pp_strategy, PSStrategy const& ps_strategy)
    {
        return_type const zero = return_type(0);

        if (boost::size(range) == 0)
        {
            return zero;
        }

        typedef typename closeable_view<Range const, Closure>::type view_type;

        view_type view(range);

        // line of one point: return point distance
        typedef typename boost::range_iterator<view_type const>::type iterator_type;
        iterator_type it = boost::begin(view);
        iterator_type prev = it++;
        if (it == boost::end(view))
        {
            return pp_strategy.apply(point, *boost::begin(view));
        }

        // Create comparable (more efficient) strategy
        typedef typename strategy::distance::services::comparable_type<PSStrategy>::type eps_strategy_type;
        eps_strategy_type eps_strategy = strategy::distance::services::get_comparable<PSStrategy>::apply(ps_strategy);

        // start with first segment distance
        return_type d = eps_strategy.apply(point, *prev, *it);
        return_type rd = ps_strategy.apply(point, *prev, *it);

        // check if other segments are closer
        for (++prev, ++it; it != boost::end(view); ++prev, ++it)
        {
            return_type const ds = eps_strategy.apply(point, *prev, *it);
            if (geometry::math::equals(ds, zero))
            {
                return ds;
            }
            else if (ds < d)
            {
                d = ds;
                rd = ps_strategy.apply(point, *prev, *it);
            }
        }

        return rd;
    }
};

template
<
    typename Point,
    typename Ring,
    closure_selector Closure,
    typename PPStrategy,
    typename PSStrategy
>
struct point_to_ring
{
    typedef std::pair
        <
            typename return_type<PPStrategy>::type, bool
        > distance_containment;

    static inline distance_containment apply(Point const& point,
                Ring const& ring,
                PPStrategy const& pp_strategy, PSStrategy const& ps_strategy)
    {
        return distance_containment
            (
                point_to_range
                    <
                        Point,
                        Ring,
                        Closure,
                        PPStrategy,
                        PSStrategy
                    >::apply(point, ring, pp_strategy, ps_strategy),
                geometry::within(point, ring)
            );
    }
};

template
<
    typename Point,
    typename Polygon,
    closure_selector Closure,
    typename PPStrategy,
    typename PSStrategy
>
struct point_to_polygon
{
    typedef typename return_type<PPStrategy>::type return_type;
    typedef std::pair<return_type, bool> distance_containment;

    static inline distance_containment apply(Point const& point,
                Polygon const& polygon,
                PPStrategy const& pp_strategy, PSStrategy const& ps_strategy)
    {
        // Check distance to all rings
        typedef point_to_ring
            <
                Point,
                typename ring_type<Polygon>::type,
                Closure,
                PPStrategy,
                PSStrategy
            > per_ring;

        distance_containment dc = per_ring::apply(point,
                        exterior_ring(polygon), pp_strategy, ps_strategy);

        typename interior_return_type<Polygon const>::type rings
                    = interior_rings(polygon);
        for (BOOST_AUTO_TPL(it, boost::begin(rings)); it != boost::end(rings); ++it)
        {
            distance_containment dcr = per_ring::apply(point,
                            *it, pp_strategy, ps_strategy);
            if (dcr.first < dc.first)
            {
                dc.first = dcr.first;
            }
            // If it was inside, and also inside inner ring,
            // turn off the inside-flag, it is outside the polygon
            if (dc.second && dcr.second)
            {
                dc.second = false;
            }
        }
        return dc;
    }
};

// Helper metafunction for default strategy retrieval
template <typename Geometry1, typename Geometry2>
struct default_strategy
    : strategy::distance::services::default_strategy
          <
              point_tag,
              typename point_type<Geometry1>::type,
              typename point_type<Geometry2>::type
          >
{};

}} // namespace detail::distance
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

using strategy::distance::services::return_type;

template
<
    typename Geometry1, typename Geometry2,
    typename Strategy = typename detail::distance::default_strategy<Geometry1, Geometry2>::type,
    typename Tag1 = typename tag_cast<typename tag<Geometry1>::type, multi_tag>::type,
    typename Tag2 = typename tag_cast<typename tag<Geometry2>::type, multi_tag>::type,
    typename StrategyTag = typename strategy::distance::services::tag<Strategy>::type,
    bool Reverse = reverse_dispatch<Geometry1, Geometry2>::type::value
>
struct distance: not_implemented<Tag1, Tag2>
{};

// If reversal is needed, perform it
template
<
    typename Geometry1, typename Geometry2, typename Strategy,
    typename Tag1, typename Tag2, typename StrategyTag
>
struct distance
<
    Geometry1, Geometry2, Strategy,
    Tag1, Tag2, StrategyTag,
    true
>
    : distance<Geometry2, Geometry1, Strategy, Tag2, Tag1, StrategyTag, false>
{
    static inline typename return_type<Strategy>::type apply(
        Geometry1 const& g1,
        Geometry2 const& g2,
        Strategy const& strategy)
    {
        return distance
            <
                Geometry2, Geometry1, Strategy,
                Tag2, Tag1, StrategyTag,
                false
            >::apply(g2, g1, strategy);
    }
};

// If reversal is needed and we got the strategy by default, invert it before
// proceeding to the reversal.
template
<
    typename Geometry1, typename Geometry2,
    typename Tag1, typename Tag2, typename StrategyTag
>
struct distance
<
    Geometry1, Geometry2,
    typename detail::distance::default_strategy<Geometry1, Geometry2>::type,
    Tag1, Tag2, StrategyTag,
    true
>
    : distance
          <
              Geometry2, Geometry1,
              typename detail::distance::default_strategy<Geometry2, Geometry1>::type,
              Tag2, Tag1, StrategyTag,
              false
          >
{
    typedef typename detail::distance::default_strategy<Geometry2, Geometry1>::type reversed_strategy;

    static inline typename strategy::distance::services::return_type<reversed_strategy>::type apply(
        Geometry1 const& g1,
        Geometry2 const& g2,
        typename detail::distance::default_strategy<Geometry1, Geometry2>::type const&)
    {
        return distance
            <
                Geometry2, Geometry1, reversed_strategy,
                Tag2, Tag1, StrategyTag,
                false
            >::apply(g2, g1, reversed_strategy());
    }
};

// Point-point
template <typename P1, typename P2, typename Strategy>
struct distance
    <
        P1, P2, Strategy,
        point_tag, point_tag, strategy_tag_distance_point_point,
        false
    >
    : detail::distance::point_to_point<P1, P2, Strategy>
{};

// Point-line version 1, where point-point strategy is specified
template <typename Point, typename Linestring, typename Strategy>
struct distance
<
    Point, Linestring, Strategy,
    point_tag, linestring_tag, strategy_tag_distance_point_point,
    false
>
{

    static inline typename return_type<Strategy>::type apply(Point const& point,
            Linestring const& linestring,
            Strategy const& strategy)
    {
        typedef typename strategy::distance::services::default_strategy
                    <
                        segment_tag,
                        Point,
                        typename point_type<Linestring>::type,
                        typename cs_tag<Point>::type,
                        typename cs_tag<typename point_type<Linestring>::type>::type,
                        Strategy
                    >::type ps_strategy_type;

        return detail::distance::point_to_range
            <
                Point, Linestring, closed, Strategy, ps_strategy_type
            >::apply(point, linestring, strategy, ps_strategy_type());
    }
};

// Point-line version 2, where point-segment strategy is specified
template <typename Point, typename Linestring, typename Strategy>
struct distance
<
    Point, Linestring, Strategy,
    point_tag, linestring_tag, strategy_tag_distance_point_segment,
    false
>
{
    static inline typename return_type<Strategy>::type apply(Point const& point,
            Linestring const& linestring,
            Strategy const& strategy)
    {
        typedef typename Strategy::point_strategy_type pp_strategy_type;
        return detail::distance::point_to_range
            <
                Point, Linestring, closed, pp_strategy_type, Strategy
            >::apply(point, linestring, pp_strategy_type(), strategy);
    }
};

// Point-ring , where point-segment strategy is specified
template <typename Point, typename Ring, typename Strategy>
struct distance
<
    Point, Ring, Strategy,
    point_tag, ring_tag, strategy_tag_distance_point_point,
    false
>
{
    typedef typename return_type<Strategy>::type return_type;

    static inline return_type apply(Point const& point,
            Ring const& ring,
            Strategy const& strategy)
    {
        typedef typename strategy::distance::services::default_strategy
            <
                segment_tag,
                Point,
                typename point_type<Ring>::type
            >::type ps_strategy_type;

        std::pair<return_type, bool>
            dc = detail::distance::point_to_ring
            <
                Point, Ring,
                geometry::closure<Ring>::value,
                Strategy, ps_strategy_type
            >::apply(point, ring, strategy, ps_strategy_type());

        return dc.second ? return_type(0) : dc.first;
    }
};

// Point-polygon , where point-segment strategy is specified
template <typename Point, typename Polygon, typename Strategy>
struct distance
<
    Point, Polygon, Strategy,
    point_tag, polygon_tag, strategy_tag_distance_point_point,
    false
>
{
    typedef typename return_type<Strategy>::type return_type;

    static inline return_type apply(Point const& point,
            Polygon const& polygon,
            Strategy const& strategy)
    {
        typedef typename strategy::distance::services::default_strategy
            <
                segment_tag,
                Point,
                typename point_type<Polygon>::type
            >::type ps_strategy_type;

        std::pair<return_type, bool>
            dc = detail::distance::point_to_polygon
            <
                Point, Polygon,
                geometry::closure<Polygon>::value,
                Strategy, ps_strategy_type
            >::apply(point, polygon, strategy, ps_strategy_type());

        return dc.second ? return_type(0) : dc.first;
    }
};

// Point-segment version 1, with point-point strategy
template <typename Point, typename Segment, typename Strategy>
struct distance
<
    Point, Segment, Strategy,
    point_tag, segment_tag, strategy_tag_distance_point_point,
    false
> : detail::distance::point_to_segment<Point, Segment, Strategy>
{};

// Point-segment version 2, with point-segment strategy
template <typename Point, typename Segment, typename Strategy>
struct distance
<
    Point, Segment, Strategy,
    point_tag, segment_tag, strategy_tag_distance_point_segment,
    false
>
{
    static inline typename return_type<Strategy>::type apply(Point const& point,
                Segment const& segment, Strategy const& strategy)
    {

        typename point_type<Segment>::type p[2];
        geometry::detail::assign_point_from_index<0>(segment, p[0]);
        geometry::detail::assign_point_from_index<1>(segment, p[1]);
        return strategy.apply(point, p[0], p[1]);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief \brief_calc2{distance} \brief_strategy
\ingroup distance
\details
\details \details_calc{area}. \brief_strategy. \details_strategy_reasons

\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam Strategy \tparam_strategy{Distance}
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param strategy \param_strategy{distance}
\return \return_calc{distance}
\note The strategy can be a point-point strategy. In case of distance point-line/point-polygon
    it may also be a point-segment strategy.

\qbk{distinguish,with strategy}

\qbk{
[heading Available Strategies]
\* [link geometry.reference.strategies.strategy_distance_pythagoras Pythagoras (cartesian)]
\* [link geometry.reference.strategies.strategy_distance_haversine Haversine (spherical)]
\* [link geometry.reference.strategies.strategy_distance_cross_track Cross track (spherical\, point-to-segment)]
\* [link geometry.reference.strategies.strategy_distance_projected_point Projected point (cartesian\, point-to-segment)]
\* more (currently extensions): Vincenty\, Andoyer (geographic)
}
 */

/*
Note, in case of a Compilation Error:
if you get:
 - "Failed to specialize function template ..."
 - "error: no matching function for call to ..."
for distance, it is probably so that there is no specialization
for return_type<...> for your strategy.
*/
template <typename Geometry1, typename Geometry2, typename Strategy>
inline typename strategy::distance::services::return_type<Strategy>::type distance(
                Geometry1 const& geometry1, Geometry2 const& geometry2,
                Strategy const& strategy)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();

    detail::throw_on_empty_input(geometry1);
    detail::throw_on_empty_input(geometry2);

    return dispatch::distance
               <
                   Geometry1,
                   Geometry2,
                   Strategy
               >::apply(geometry1, geometry2, strategy);
}

/*!
\brief \brief_calc2{distance}
\ingroup distance
\details The default strategy is used, corresponding to the coordinate system of the geometries
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\return \return_calc{distance}

\qbk{[include reference/algorithms/distance.qbk]}
 */
template <typename Geometry1, typename Geometry2>
inline typename default_distance_result<Geometry1, Geometry2>::type distance(
                Geometry1 const& geometry1, Geometry2 const& geometry2)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();

    return distance(geometry1, geometry2,
                    typename detail::distance::default_strategy<Geometry1, Geometry2>::type());
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DISTANCE_HPP