/**
 * GeoDa TM, Copyright (C) 2011-2015 by Luc Anselin - all rights reserved
 *
 * This file is part of GeoDa.
 *
 * GeoDa 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 3 of the License, or
 * (at your option) any later version.
 *
 * GeoDa 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, see <http://www.gnu.org/licenses/>.
 */

#include <wx/wx.h>

#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <wx/filename.h>
#include <wx/string.h>
#include <wx/stopwatch.h>
#include "ShpFile.h"
#include "PointSetAlgs.h"
#include "GenGeomAlgs.h"
#include "SpatialIndAlgs.h"
#include "VarCalc/NumericTests.h"
#include "GdaException.h"
#include "logger.h"

using namespace std;

void SpatialIndAlgs::get_centroids(std::vector<pt_2d>& centroids,
                                   const Shapefile::Main& main_data)
{
	size_t num_obs = main_data.records.size();
	if (centroids.size() != num_obs) centroids.resize(num_obs);
	if (main_data.header.shape_type == Shapefile::POINT_TYP) {
		Shapefile::PointContents* pc;
		for (size_t i=0; i<num_obs; ++i) {
			pc = (Shapefile::PointContents*) main_data.records[i].contents_p;
			if (pc->shape_type == 0) {
				centroids[i] = pt_2d(0, 0);
			} else {
				centroids[i] = pt_2d(pc->x, pc->y);
			}
		}
	} else if (main_data.header.shape_type == Shapefile::POLYGON) {
		Shapefile::PolygonContents* pc;
		for (size_t i=0; i<num_obs; ++i) {
			pc = (Shapefile::PolygonContents*) main_data.records[i].contents_p;
			GdaPolygon poly(pc);
			if (poly.isNull()) {
				centroids[i] = pt_2d(0, 0);
			} else {
				wxRealPoint rp(GdaShapeAlgs::calculateCentroid(&poly));
				centroids[i] = pt_2d(rp.x, rp.y);
			}
		}
	}
}

void SpatialIndAlgs::get_centroids(std::vector<pt_lonlat>& centroids,
                                   const Shapefile::Main& main_data)
{
	// Note: Boost Geometry Spherical Equatorial system uses
	// points in order longitude, latitude as does Shapefiles. This
	// is so that arc points can be plotted as-is on an x/y plane,
	// although doing this results in some distortion.
	size_t num_obs = main_data.records.size();
	if (centroids.size() != num_obs)
        centroids.resize(num_obs);

	if (main_data.header.shape_type == Shapefile::POINT_TYP) {
		Shapefile::PointContents* pc;
		for (size_t i=0; i<num_obs; ++i) {
			pc = (Shapefile::PointContents*) main_data.records[i].contents_p;
			if (pc->shape_type == 0) {
				centroids[i] = pt_lonlat(0, 0);
			} else {
				centroids[i] = pt_lonlat(pc->x, pc->y);
			}
		}
	} else if (main_data.header.shape_type == Shapefile::POLYGON) {
		Shapefile::PolygonContents* pc;
		for (size_t i=0; i<num_obs; ++i) {
			pc = (Shapefile::PolygonContents*) main_data.records[i].contents_p;
			GdaPolygon poly(pc);
			if (poly.isNull()) {
				centroids[i] = pt_lonlat(0, 0);
			} else {
				wxRealPoint rp(GdaShapeAlgs::calculateCentroid(&poly));
				centroids[i] = pt_lonlat(rp.x, rp.y);
			}
		}
	}
}

void SpatialIndAlgs::to_3d_centroids(const vector<pt_2d>& pt2d,
                                     vector<pt_3d>& pt3d)
{
	size_t obs = pt2d.size();
	pt3d.resize(obs);
	for (size_t i=0; i<obs; ++i) {
		pt3d[i] = pt_3d(bg::get<0>(pt2d[i]), bg::get<1>(pt2d[i]), 0);
	}
}

void SpatialIndAlgs::to_3d_centroids(const vector<pt_lonlat>& ptll,
                                     vector<pt_3d>& pt3d)
{
	size_t obs = ptll.size();
	pt3d.resize(obs);
	for (size_t i=0; i<obs; ++i) {
		double x, y, z;
		GenGeomAlgs::LongLatDegToUnit(bg::get<0>(ptll[i]),
                                      bg::get<1>(ptll[i]),
									  x, y, z);
		pt3d[i] = pt_3d(x, y, z);
	}
}

void SpatialIndAlgs::get_shp_bb(Shapefile::PolygonContents* p,
                                double& xmin, double& ymin,
                                double& xmax, double& ymax)
{
	if (!p || p->num_points <= 0) {
		xmin = 0; ymin = 0; xmax = 0; ymax = 0;
		return;
	}

	xmin = p->points[0].x;
	ymin = p->points[0].y;
	xmax = p->points[0].x;
	ymax = p->points[0].y;

	for (int i=0; i<p->num_points; ++i) {
		if (p->points[i].x < xmin) {
			xmin = p->points[i].x;
		} else if (p->points[i].x > xmax) {
			xmax = p->points[i].x;
		}
		if (p->points[i].y < ymin) {
			ymin = p->points[i].y;
		} else if (p->points[i].y > ymax) {
			ymax = p->points[i].y;
		}
	}
}

bool comp_polys(Shapefile::PolygonContents* p1,
                Shapefile::PolygonContents* p2,
				bool rook, double prec)
{
	if (!p1 || !p2)
        return false;

	for (int i=0; i<p1->num_points; ++i) {
		for (int j=0; j<p1->num_points; ++j) {
			if (p1->points[i] == p2->points[j])
                return true;
		}
	}
	return false;
}

void SpatialIndAlgs::default_test()
{
    // create the rtree using default constructor
    rtree_box_2d_t rtree;

    // create some values
    for ( unsigned i = 0 ; i < 10 ; ++i ) {
        // create a box
        box_2d b(pt_2d(i + 0.0f, i + 0.0f), pt_2d(i + 0.5f, i + 0.5f));
        // insert new value
        rtree.insert(std::make_pair(b, i));
    }

    // find values intersecting some area defined by a box
    box_2d query_box(pt_2d(0, 0), pt_2d(5, 5));
    std::vector<box_2d_val> result_s;
    rtree.query(bgi::intersects(query_box), std::back_inserter(result_s));

	const int k=3;
    // find k nearest values to a point
    std::vector<box_2d_val> result_n;
    rtree.query(bgi::nearest(pt_2d(0, 0), k), std::back_inserter(result_n));

    // note: in Boost.Geometry WKT representation of a box is polygon

    // display results
	stringstream ss;
    ss << "spatial query box:" << std::endl;
    ss << bg::wkt<box_2d>(query_box) << std::endl;
    ss << "spatial query result:" << std::endl;
    BOOST_FOREACH(box_2d_val const& v, result_s) {
        ss << bg::wkt<box_2d>(v.first) << " - " << v.second << std::endl;
	}

    ss << k << "-nn query point:" << std::endl;
    ss << bg::wkt<pt_2d>(pt_2d(0, 0)) << std::endl;
    ss << k << "-nn query result:" << std::endl;
    BOOST_FOREACH(box_2d_val const& v, result_n) {
        ss << bg::wkt<box_2d>(v.first) << " - " << v.second << std::endl;
	}

	pt_lonlat sp(0, 45);
	ss << "Spherical pt get<0>: " << bg::get<0>(sp) << std::endl;
	ss << "Spherical pt get<1>: " << bg::get<1>(sp) << std::endl;
	ss << "Spherical pt: " << bg::wkt<pt_lonlat>(sp) << std::endl;

	ss << "default_test() END";
}

void SpatialIndAlgs::print_rtree_stats(rtree_box_2d_t& rtree)
{
	stringstream ss;
	ss << "Rtree stats:" << endl;
	ss << "  size: " << rtree.size() << endl;
	ss << "  empty?: " << rtree.empty() << endl;
	box_2d bnds = rtree.bounds();
	ss << "  bounds: " << bg::wkt<box_2d>(bnds);
}

void SpatialIndAlgs::query_all_boxes(rtree_box_2d_t& rtree)
{
	int dzero = 0;
	int dpos = 0;
	int cnt=0;
	box_2d bnds = rtree.bounds();

	for (rtree_box_2d_t::const_query_iterator it =
			 rtree.qbegin(bgi::intersects(rtree.bounds()));
		 it != rtree.qend() ; ++it) { ++cnt; }

	cnt = 0;

    rtree_box_2d_t::const_query_iterator it;
	for (it=rtree.qbegin(bgi::intersects(rtree.bounds())); it != rtree.qend(); ++it)
	{
		const box_2d_val& v = *it;
		pt_2d c;
		boost::geometry::centroid(v.first, c);
		vector<box_2d_val> q;
		rtree.query(bgi::intersects(v.first), std::back_inserter(q));
		int qcnt=0;
		BOOST_FOREACH(box_2d_val const& w, q) {
			if (w.second == v.second)
                continue;
			++cnt;
			++qcnt;
		}
	}
}

void SpatialIndAlgs::knn_query(const rtree_pt_2d_t& rtree, int nn)
{
	int cnt=0;
	box_2d bnds = rtree.bounds();

    rtree_pt_2d_t::const_query_iterator it;
	for (it= rtree.qbegin(bgi::intersects(rtree.bounds())); it != rtree.qend(); ++it)
    {
        ++cnt;
    }

	cnt = 0;

	const int k=nn+1;
	for (it= rtree.qbegin(bgi::intersects(rtree.bounds())); it != rtree.qend(); ++it)
	{
		const pt_2d_val& v = *it;
		vector<pt_2d_val> q;
		rtree.query(bgi::nearest(v.first, k), std::back_inserter(q));
		BOOST_FOREACH(pt_2d_val const& w, q) {
            if (w.second == v.second) {
                continue;
            }
			++cnt;
		}
	}
}

GwtWeight* SpatialIndAlgs::knn_build(const vector<double>& x,
                                     const vector<double>& y,
                                     int nn, bool is_arc, bool is_mi)
{
	size_t nobs = x.size();
	GwtWeight* gwt = 0;
	if (is_arc) {
		rtree_pt_3d_t rtree;
		{
			vector<pt_3d> pts;
			{
				vector<pt_lonlat> ptll(nobs);
				for (int i=0; i<nobs; ++i) ptll[i] = pt_lonlat(x[i], y[i]);
				to_3d_centroids(ptll, pts);
			}
			fill_pt_rtree(rtree, pts);
		}
		gwt = knn_build(rtree, nn, true, is_mi);

	} else {
		rtree_pt_2d_t rtree;
		{
			vector<pt_2d> pts(nobs);
			for (int i=0; i<nobs; ++i) pts[i] = pt_2d(x[i], y[i]);
			fill_pt_rtree(rtree, pts);
		}
		gwt = knn_build(rtree, nn);

	}
	return gwt;
}

GwtWeight* SpatialIndAlgs::knn_build(const rtree_pt_2d_t& rtree, int nn)
{
	GwtWeight* Wp = new GwtWeight;
	Wp->num_obs = rtree.size();
	Wp->is_symmetric = false;
	Wp->symmetry_checked = true;
	Wp->gwt = new GwtElement[Wp->num_obs];

	int cnt=0;
	const int k=nn+1;
	for (rtree_pt_2d_t::const_query_iterator it =
			 rtree.qbegin(bgi::intersects(rtree.bounds()));
		 it != rtree.qend() ; ++it)
	{
		const pt_2d_val& v = *it;
		size_t obs = v.second;
		vector<pt_2d_val> q;
		rtree.query(bgi::nearest(v.first, k), std::back_inserter(q));
		GwtElement& e = Wp->gwt[obs];
		e.alloc(q.size());
		BOOST_FOREACH(pt_2d_val const& w, q) {
			if (w.second == v.second) continue;
			GwtNeighbor neigh;
			neigh.nbx = w.second;
			neigh.weight = bg::distance(v.first, w.first);
			e.Push(neigh);
			++cnt;
		}
	}

	return Wp;
}

GwtWeight* SpatialIndAlgs::knn_build(const rtree_pt_3d_t& rtree, int nn,
					 bool is_arc, bool is_mi)
{
	wxStopWatch sw;
	using namespace GenGeomAlgs;

	GwtWeight* Wp = new GwtWeight;
	Wp->num_obs = rtree.size();
	Wp->is_symmetric = false;
	Wp->symmetry_checked = true;
	Wp->gwt = new GwtElement[Wp->num_obs];

	int cnt=0;
	const int k=nn+1;
	for (rtree_pt_3d_t::const_query_iterator it =
			 rtree.qbegin(bgi::intersects(rtree.bounds()));
		 it != rtree.qend() ; ++it)
	{
		const pt_3d_val& v = *it;
		size_t obs = v.second;
		vector<pt_3d_val> q;
		rtree.query(bgi::nearest(v.first, k), std::back_inserter(q));
		GwtElement& e = Wp->gwt[obs];
		e.alloc(q.size());
		double lon_v, lat_v;
		double x_v, y_v;
		if (is_arc) {
			UnitToLongLatDeg(bg::get<0>(v.first), bg::get<1>(v.first),
							 bg::get<2>(v.first), lon_v, lat_v);
		} else {
			x_v = bg::get<0>(v.first);
			y_v = bg::get<1>(v.first);
		}
		BOOST_FOREACH(pt_3d_val const& w, q) {
			if (w.second == v.second) continue;
			GwtNeighbor neigh;
			neigh.nbx = w.second;
			if (is_arc) {
				double lon_w, lat_w;
				UnitToLongLatDeg(bg::get<0>(w.first), bg::get<1>(w.first),
								 bg::get<2>(w.first), lon_w, lat_w);
				if (is_mi) {
					neigh.weight = ComputeArcDistMi(lon_v, lat_v, lon_w, lat_w);
				} else {
					neigh.weight = ComputeArcDistKm(lon_v, lat_v, lon_w, lat_w);
				}
			} else {
				//neigh.weight = bg::distance(v.first, w.first);
				neigh.weight = ComputeEucDist(x_v, y_v,
											  bg::get<0>(w.first),
                                              bg::get<1>(w.first));
			}
			e.Push(neigh);
			++cnt;
		}
	}

	stringstream ss;
	ss << "Time to create 3D " << (is_arc ? " arc " : "")
	   << nn << "-NN GwtWeight "
	   << "with " << cnt << " total neighbors in ms : " << sw.Time();
	return Wp;
}

double SpatialIndAlgs::est_thresh_for_num_pairs(const rtree_pt_2d_t& rtree,
												double num_pairs)
{
	double nobs_d = (double) rtree.size();
	if (num_pairs >= (nobs_d*(nobs_d-1.0))/2.0) {
		return bg::distance(rtree.bounds().min_corner(), rtree.bounds().max_corner());
	}
	// Need roughly double since pairs are visited twice.
	double avg_n = (num_pairs / nobs_d)*2.0;
	// To avoid the use of a hash table, we just allow pairs
	// to be counted twice each bin is just an average. Although
	// distances will be calculated twice, the cost should be offset
	// by the faster performance of no hash table inserts / lookups.
	double thresh = est_thresh_for_avg_num_neigh(rtree, avg_n);
	return thresh;
}

double SpatialIndAlgs::est_thresh_for_avg_num_neigh(const rtree_pt_2d_t& rtree,
													double avg_n)
{
	// Use a binary search to estimate threshold to acheive average num neighbors
	wxStopWatch sw;
	using namespace GenGeomAlgs;
	int max_iters = 20;
	int iters = 0;
	double lower = 0;
	double lower_avg = 0;
	box_2d bnds(rtree.bounds());
	double upper = bg::distance(bnds.min_corner(), bnds.max_corner());
	double upper_avg = (double) rtree.size();
	double guess = upper;
	double guess_avg = upper_avg;
	double th = guess;

	bool was_improvement = true;
	for (iters=0; iters<max_iters && was_improvement; ++iters) {
		guess = lower + (upper-lower)/2.0;
		guess_avg = est_avg_num_neigh_thresh(rtree, guess);
		{
			stringstream ss;
			ss << "\niter: " << iters << "   target avg: " << avg_n << endl;
			ss << "  lower: " << lower << ", lower_avg: " << lower_avg << endl;
			ss << "  guess: " << guess << ", guess_avg: " << guess_avg << endl;
			ss << "  upper: " << upper << ", upper_avg: " << upper_avg;
		}
		if (guess_avg == avg_n) {
			//LOG_MSG("new guess was exact!");
			// this will never happen, but put case here for completeness
			th = guess;
			was_improvement = false;
		} else if (guess_avg <= lower_avg) {
			//LOG_MSG("new guess below lower bound");
			was_improvement = false;
		} else if (guess_avg >= upper_avg) {
			//LOG_MSG("new guess above lower bound");
			was_improvement = false;
		} else if (guess_avg < avg_n) {
			//LOG_MSG("increase lower bound");
			lower = guess;
			lower_avg = guess_avg;
		} else { // guess_avg > avg_n
			//LOG_MSG("decrease upper bound");
			upper = guess;
			upper_avg = guess_avg;
		}
		if (was_improvement) {
			th = guess;
		}
	}

	stringstream ss;
	ss << "Estimated " << th << " threshold for average "
	   << "number neighbors " << avg_n << "." << endl;
	ss << "Calculation time to peform " << iters << " iterations: "
	   << sw.Time() << " ms.";
	LOG_MSG("Exiting est_thresh_for_avg_num_neigh");
	return th;
}

double SpatialIndAlgs::est_avg_num_neigh_thresh(const rtree_pt_2d_t& rtree,
																								double th, size_t trials)
{
	wxStopWatch sw;
	using namespace GenGeomAlgs;

	vector<pt_2d_val> query_pts;
	rtree.query(bgi::intersects(rtree.bounds()), back_inserter(query_pts));
	// Mersenne Twister random number generator, randomly seeded
	// with current time in seconds since Jan 1 1970.
	static boost::mt19937 rng(std::time(0));
	static boost::random::uniform_int_distribution<> X(0, query_pts.size()-1);
	size_t tot_neigh = 0;
	for (size_t i=0; i<trials; ++i) {
		const pt_2d_val& v = query_pts[X(rng)];
		double x = v.first.get<0>();
		double y = v.first.get<1>();
		box_2d b(pt_2d(x-th, y-th), pt_2d(x+th, y+th));
		vector<pt_2d_val> q;
		rtree.query(bgi::intersects(b), std::back_inserter(q));
		BOOST_FOREACH(const pt_2d_val& w, q) {
			if (w.second != v.second && bg::distance(v.first, w.first) <= th)
			{
				++tot_neigh;
			}
		}
	}

	double avg = ((double) tot_neigh) / ((double) trials);

	stringstream ss;
	ss << "Estimated " << avg << " neighbors on average for "
	   << "threshold " << th << "." << endl;
	ss << "Time to perform " << trials << " random trials: "
	   << sw.Time() << " ms.";
	//LOG_MSG(ss.str());
	return avg;
}

double SpatialIndAlgs::est_mean_distance(const std::vector<double>& x,
										 const std::vector<double>& y,
										 bool is_arc, size_t max_iters)
{
	wxStopWatch sw;
	using namespace GenGeomAlgs;
	const size_t pts_sz = x.size();
	const size_t all_pairs_sz = (pts_sz*(pts_sz-1))/2;
	if (x.size() != y.size() || x.size() == 0 || y.size() == 0) { return -1; }
	double sum = 0;
	double smp_cnt = 0;

	if (all_pairs_sz <= max_iters) {
		for (size_t i=0; i<pts_sz; ++i) {
			for (size_t j=i+1; j<pts_sz; ++j) {
				sum += (is_arc ? ComputeArcDistRad(x[i], y[i], x[j], y[j]) :
						ComputeEucDist(x[i], y[i], x[j], y[j]));
			}
		}
		smp_cnt = (double) all_pairs_sz;
	} else {
		// Mersenne Twister random number generator, randomly seeded
		// with current time in seconds since Jan 1 1970.
		static boost::mt19937 rng(std::time(0));
		static boost::random::uniform_int_distribution<> X(0, pts_sz-1);
		for (size_t t=0; t<max_iters; ++t) {
			size_t i=X(rng);
			size_t j=X(rng);
			sum += (is_arc ? ComputeArcDistRad(x[i], y[i], x[j], y[j]) :
					ComputeEucDist(x[i], y[i], x[j], y[j]));
		}
		smp_cnt = max_iters;
	}
	stringstream ss;
	ss << "est_mean_distance finished in " << sw.Time() << " ms.";
	return sum/smp_cnt;
}

double SpatialIndAlgs::est_median_distance(const std::vector<double>& x,
										   const std::vector<double>& y,
										   bool is_arc, size_t max_iters)
{
	wxStopWatch sw;
	using namespace GenGeomAlgs;
	if (x.size() != y.size() || x.size() == 0 || y.size() == 0) { return -1; }
	const size_t pts_sz = x.size();
	const size_t all_pairs_sz = (pts_sz*(pts_sz-1))/2;
	vector<double> v;

	if (all_pairs_sz <= max_iters) {
		v.resize((pts_sz*(pts_sz-1))/2);
		size_t t=0;
		for (size_t i=0; i<pts_sz; ++i) {
			for (size_t j=i+1; j<pts_sz; ++j) {
				v[t] = (is_arc ? ComputeArcDistRad(x[i], y[i], x[j], y[j]) :
						ComputeEucDist(x[i], y[i], x[j], y[j]));
				++t;
			}
		}
	} else {
		v.resize(max_iters);
		// Mersenne Twister random number generator, randomly seeded
		// with current time in seconds since Jan 1 1970.
		static boost::mt19937 rng(std::time(0));
		static boost::random::uniform_int_distribution<> X(0, pts_sz-1);
		size_t cnt=0;
		for (size_t t=0; t<max_iters; ++t) {
			size_t i=X(rng);
			size_t j=X(rng);
			//if (i==j) continue;
			v[t] = (is_arc ? ComputeArcDistRad(x[i], y[i], x[j], y[j]) :
					ComputeEucDist(x[i], y[i], x[j], y[j]));
			if (!Gda::is_finite(v[t]) || Gda::is_nan(v[t])) {
				stringstream ss;
				ss << "d(i="<<i<<",j="<<j<<"): "<<v[t];
			}

		}
	}
	sort(v.begin(), v.end());
	stringstream ss;
	ss << "est_median_distance finished in " << sw.Time() << " ms.";
	return v[v.size()/2];
}

GwtWeight* SpatialIndAlgs::thresh_build(const std::vector<double>& x,
                                        const std::vector<double>& y,
                                        double th, bool is_arc, bool is_mi)
{
	using namespace GenGeomAlgs;
	size_t nobs = x.size();
	GwtWeight* gwt = 0;
	if (is_arc) {
		double r_th = is_mi ? EarthMiToRad(th) : EarthKmToRad(th);
		double u_th = RadToUnitDist(r_th);
		rtree_pt_3d_t rtree;
		{
			vector<pt_3d> pts;
			{
				vector<pt_lonlat> ptll(nobs);
				for (int i=0; i<nobs; ++i) ptll[i] = pt_lonlat(x[i], y[i]);
				to_3d_centroids(ptll, pts);
			}
			fill_pt_rtree(rtree, pts);
		}
		gwt = thresh_build(rtree, u_th, is_mi);
	} else {
		rtree_pt_2d_t rtree;
		{
			vector<pt_2d> pts(nobs);
            for (int i=0; i<nobs; ++i) {
                pts[i] = pt_2d(x[i], y[i]);
            }
			fill_pt_rtree(rtree, pts);
		}
		gwt = thresh_build(rtree, th);
	}
	return gwt;
}

GwtWeight* SpatialIndAlgs::thresh_build(const rtree_pt_2d_t& rtree, double th)
{
	wxStopWatch sw;

	GwtWeight* Wp = new GwtWeight;
	Wp->num_obs = rtree.size();
	Wp->is_symmetric = false;
	Wp->symmetry_checked = true;

    int num_obs = Wp->num_obs;
	Wp->gwt = new GwtElement[num_obs];

	int cnt=0;
	bool ignore_too_large_compute = false;
    rtree_pt_2d_t::const_query_iterator it;
	for (it = rtree.qbegin(bgi::intersects(rtree.bounds()));
		 it != rtree.qend() ; ++it)
	{
		const pt_2d_val& v = *it;
		double x = v.first.get<0>();
		double y = v.first.get<1>();
		box_2d b(pt_2d(x-th, y-th), pt_2d(x+th, y+th));
		size_t obs = v.second;
		vector<pt_2d_val> q;
		rtree.query(bgi::intersects(b), std::back_inserter(q));
		size_t lcnt = 0;
		list<pt_2d_val> l;
		BOOST_FOREACH(pt_2d_val const& w, q) {
			if (w.second != v.second &&
				bg::distance(v.first, w.first) <= th)
			{
				l.push_front(w);
				++lcnt;
			}
		}
        if (lcnt > 200 && ignore_too_large_compute == false) {

            wxString msg = _("The current threshold distance value is too large to compute. Please input a smaller distance band (which might leave some observations neighborless) or use other weights (e.g. KNN).");
			wxMessageDialog dlg(NULL, msg, "Do you want to continue?", wxYES_NO | wxYES_DEFAULT);
			if (dlg.ShowModal() != wxID_YES) {
				// clean up memory
				delete Wp;
				throw GdaException(msg.mb_str());
			}
			else {
				ignore_too_large_compute = true;
			}

        }
		GwtElement& e = Wp->gwt[obs];
		e.alloc(lcnt);
		BOOST_FOREACH(pt_2d_val const& w, l) {
			GwtNeighbor neigh;
			neigh.nbx = w.second;
			neigh.weight = bg::distance(v.first, w.first);
			e.Push(neigh);
			++cnt;
		}
	}

	stringstream ss;
	ss << "Time to create " << th << " threshold GwtWeight,"
	   << endl << "  with " << cnt << " total neighbors in ms : "
	   << sw.Time();
	return Wp;
}

double SpatialIndAlgs::est_avg_num_neigh_thresh(const rtree_pt_3d_t& rtree,
					 double th,	size_t trials)
{
	wxStopWatch sw;
	using namespace GenGeomAlgs;

	vector<pt_3d_val> query_pts;
	rtree.query(bgi::intersects(rtree.bounds()), back_inserter(query_pts));
	// Mersenne Twister random number generator, randomly seeded
	// with current time in seconds since Jan 1 1970.
	static boost::mt19937 rng(std::time(0));
	static boost::random::uniform_int_distribution<> X(0, query_pts.size()-1);
	size_t tot_neigh = 0;
	for (size_t i=0; i<trials; ++i) {
		const pt_3d_val& v = query_pts[X(rng)];
		double x = v.first.get<0>();
		double y = v.first.get<1>();
		double z = v.first.get<2>();
		box_3d b(pt_3d(x-th, y-th, z-th), pt_3d(x+th, y+th, z+th));
		vector<pt_3d_val> q;
		rtree.query(bgi::intersects(b), std::back_inserter(q));
		BOOST_FOREACH(const pt_3d_val& w, q) {
			if (w.second != v.second && bg::distance(v.first, w.first) <= th)
			{
				++tot_neigh;
			}
		}
	}
	double avg = ((double) tot_neigh) / ((double) trials);
	stringstream ss;
	ss << "Estimated " << avg << " neighbors on average for "
	   << "threshold " << th << "." << endl;
	ss << "Time to perform " << trials << " random trials: "
	   << sw.Time() << " ms.";
	return avg;
}

/** threshold th is the radius of intersection sphere with
  respect to the unit shpere of the 3d point rtree */
GwtWeight* SpatialIndAlgs::thresh_build(const rtree_pt_3d_t& rtree, double th, bool is_mi)
{
	wxStopWatch sw;
	using namespace GenGeomAlgs;

	GwtWeight* Wp = new GwtWeight;
	Wp->num_obs = rtree.size();
	Wp->is_symmetric = false;
	Wp->symmetry_checked = true;
	Wp->gwt = new GwtElement[Wp->num_obs];

	{
		stringstream ss;
		ss << "In thresh_build for unit sphere" << endl;
		ss << "th : " << th << endl;
		ss << "Input th (unit sphere secant distance): " << th << endl;
		double r = UnitDistToRad(th);
		ss << "Input th (unit sphere rad): " << r << endl;
		ss << "Input th (earth km): " << EarthRadToKm(r) << endl;
		ss << "Input th (earth mi): " << EarthRadToMi(r);
	}
	int cnt=0;
	for (rtree_pt_3d_t::const_query_iterator it =
			 rtree.qbegin(bgi::intersects(rtree.bounds()));
		 it != rtree.qend() ; ++it)
	{
		const pt_3d_val& v = *it;
		double vx = v.first.get<0>();
		double vy = v.first.get<1>();
		double vz = v.first.get<2>();
		double lon_v, lat_v;
		UnitToLongLatDeg(vx, vy, vz, lon_v, lat_v);
		box_3d b(pt_3d(vx-th, vy-th, vz-th), pt_3d(vx+th, vy+th, vz+th));
		size_t obs = v.second;
		vector<pt_3d_val> q;
		rtree.query(bgi::intersects(b), std::back_inserter(q));
		size_t lcnt = 0;
		list<pt_3d_val> l;
		BOOST_FOREACH(pt_3d_val const& w, q) {
			if (w.second != v.second &&
				bg::distance(v.first, w.first) <= th)
			{
				l.push_front(w);
				++lcnt;
			}
		}
		GwtElement& e = Wp->gwt[obs];
		e.alloc(lcnt);
		BOOST_FOREACH(pt_3d_val const& w, l) {
			GwtNeighbor neigh;
			neigh.nbx = w.second;
			double wx = w.first.get<0>();
			double wy = w.first.get<1>();
			double wz = w.first.get<2>();
			double lon_w, lat_w;
			double d;
			UnitToLongLatDeg(wx, wy, wz, lon_w, lat_w);
			if (is_mi) {
				d = ComputeArcDistMi(lon_v, lat_v, lon_w, lat_w);
			} else {
				d = ComputeArcDistKm(lon_v, lat_v, lon_w, lat_w);
			}
			neigh.weight = d;
			e.Push(neigh);
			++cnt;
		}
	}

	stringstream ss;
	ss << "Time to create arc " << th << " threshold GwtWeight,"
	   << endl << "  with " << cnt << " total neighbors in ms : "
	   << sw.Time();
	return Wp;
}

double SpatialIndAlgs::find_max_1nn_dist(const std::vector<double>& x,
                                         const std::vector<double>& y,
                                         bool is_arc, bool is_mi)
{
	using namespace GenGeomAlgs;
	size_t nobs = x.size();
	double min_d_1nn, max_d_1nn, mean_d_1nn, median_d_1nn, d;
	if (is_arc) {
		rtree_pt_3d_t rtree;
		{
			vector<pt_3d> pts;
			{
				vector<pt_lonlat> ptll(nobs);
				for (int i=0; i<nobs; ++i) ptll[i] = pt_lonlat(x[i], y[i]);
				to_3d_centroids(ptll, pts);
			}
			fill_pt_rtree(rtree, pts);
		}
		get_pt_rtree_stats(rtree, min_d_1nn, max_d_1nn, mean_d_1nn, median_d_1nn);
		d = is_mi ? EarthRadToMi(max_d_1nn) : EarthRadToKm(max_d_1nn);
	} else {
		rtree_pt_2d_t rtree;
		{
			vector<pt_2d> pts(nobs);
			for (int i=0; i<nobs; ++i) pts[i] = pt_2d(x[i], y[i]);
			fill_pt_rtree(rtree, pts);
		}
		get_pt_rtree_stats(rtree, min_d_1nn, max_d_1nn, mean_d_1nn, median_d_1nn);
		d = max_d_1nn;
	}
	return d;
}

void SpatialIndAlgs::get_pt_rtree_stats(const rtree_pt_2d_t& rtree,
                                        double& min_d_1nn, double& max_d_1nn,
                                        double& mean_d_1nn, double& median_d_1nn)
{
	wxStopWatch sw;
	const int k=2;
	size_t obs = rtree.size();
	vector<double> d(obs);
	for (rtree_pt_2d_t::const_query_iterator it =
			 rtree.qbegin(bgi::intersects(rtree.bounds()));
		 it != rtree.qend() ; ++it)
	{
		const pt_2d_val& v = *it;
		vector<pt_2d_val> q;
		rtree.query(bgi::nearest(v.first, k), std::back_inserter(q));
		BOOST_FOREACH(pt_2d_val const& w, q) {
			if (w.second == v.second) continue;
			d[v.second] = bg::distance(v.first, w.first);
		}
	}
	sort(d.begin(), d.end());
	min_d_1nn = d[0];
	max_d_1nn = d[d.size()-1];
	median_d_1nn = d[(d.size()-1)/2];
	double s=0;
	for (size_t i=0; i<obs; ++i) s += d[i];
	mean_d_1nn = s / (double) obs;

	stringstream ss;
	ss << "Euclidean points stats:" << endl;
	ss << "  min_d_1nn: " << min_d_1nn << endl;
	ss << "  max_d_1nn: " << max_d_1nn << endl;
	ss << "  median_d_1nn: " << median_d_1nn << endl;
	ss << "  mean_d_1nn: " << mean_d_1nn << endl;
	ss << "  running time in ms: " << sw.Time();
}

/** results returned in radians */
void SpatialIndAlgs::get_pt_rtree_stats(const rtree_pt_3d_t& rtree,
					 double& min_d_1nn, double& max_d_1nn,
					 double& mean_d_1nn, double& median_d_1nn)
{
	wxStopWatch sw;
	using namespace GenGeomAlgs;
	size_t obs = rtree.size();
	vector<double> d(obs);
	for (rtree_pt_3d_t::const_query_iterator it =
			 rtree.qbegin(bgi::intersects(rtree.bounds()));
		 it != rtree.qend() ; ++it)
	{
		const pt_3d_val& v = *it;
		vector<pt_3d_val> q;
		rtree.query(bgi::nearest(v.first, 2), std::back_inserter(q));
		BOOST_FOREACH(pt_3d_val const& w, q) {
			if (w.second == v.second) continue;
			double lonv, latv, lonw, latw;
			UnitToLongLatRad(v.first.get<0>(), v.first.get<1>(),
							 v.first.get<2>(), lonv, latv);
			UnitToLongLatRad(w.first.get<0>(), w.first.get<1>(),
							 w.first.get<2>(), lonw, latw);
			d[v.second] = LonLatRadDistRad(lonv, latv, lonw, latw);
		}
	}
	sort(d.begin(), d.end());
	min_d_1nn = d[0];
	max_d_1nn = d[d.size()-1];
	median_d_1nn = d[(d.size()-1)/2];
	double s=0;
	for (size_t i=0; i<obs; ++i) s += d[i];
	mean_d_1nn = s / (double) obs;

	stringstream ss;
	ss << "Long / Lat points stats:" << endl;
	ss << "  min_d_1nn: " << min_d_1nn << " rad, "
	   << RadToDeg(min_d_1nn) << " deg, "
	   << EarthRadToKm(min_d_1nn) << " km, "
	   << EarthRadToMi(min_d_1nn) << " mi" << endl;
	ss << "  max_d_1nn: " << max_d_1nn << " rad, "
	   << RadToDeg(max_d_1nn) << " deg, "
	   << EarthRadToKm(max_d_1nn) << " km, "
	   << EarthRadToMi(max_d_1nn) << " mi" << endl;
	ss << "  median_d_1nn: " << median_d_1nn << " rad, "
	   << RadToDeg(median_d_1nn) << " deg, "
	   << EarthRadToKm(median_d_1nn) << " km, "
	   << EarthRadToMi(median_d_1nn) << " mi" << endl;
	ss << "  mean_d_1nn: " << mean_d_1nn << " rad, "
	   << RadToDeg(mean_d_1nn) << " deg, "
	   << EarthRadToKm(mean_d_1nn) << " km, "
	   << EarthRadToMi(mean_d_1nn) << " mi" << endl;
	ss << "  running time in ms: " << sw.Time();
}

GwtWeight* SpatialIndAlgs::knn_build(const rtree_pt_lonlat_t& rtree, int nn)
{
	GwtWeight* Wp = new GwtWeight;
	Wp->num_obs = rtree.size();
	Wp->is_symmetric = false;
	Wp->symmetry_checked = true;
	Wp->gwt = new GwtElement[Wp->num_obs];

	int cnt=0;
	const int k=nn+1;
	for (rtree_pt_lonlat_t::const_query_iterator it =
			 rtree.qbegin(bgi::intersects(rtree.bounds()));
		 it != rtree.qend() ; ++it)
	{
		const pt_lonlat_val& v = *it;
		size_t obs = v.second;
		vector<pt_lonlat_val> q;
		rtree.query(bgi::nearest(v.first, k), std::back_inserter(q));
		GwtElement& e = Wp->gwt[obs];
		e.alloc(q.size());
		BOOST_FOREACH(const pt_lonlat_val& w, q) {
			if (w.second == v.second) continue;
			GwtNeighbor neigh;
			neigh.nbx = w.second;
			neigh.weight = bg::distance(v.first, w.first);
			e.Push(neigh);
			++cnt;
		}
	}

	return Wp;
}

bool SpatialIndAlgs::write_gwt(const GwtWeight* W,
							   const wxString& _layer_name,
							   const wxString& ofname,
							   const wxString& vname,
							   const std::vector<wxInt64>& id_vec)
{
    if (!W) {
        return false;
    }
	const GwtElement* g = W->gwt;
	size_t num_obs = W->num_obs;

    if (!g ||
        _layer_name.IsEmpty() ||
        ofname.IsEmpty() ||
        id_vec.size() == 0 ||
        num_obs != id_vec.size())
    {
        return false;
    }

    wxFileName gwtfn(ofname);
    gwtfn.SetExt("gwt");
    wxString gwt_ofn(gwtfn.GetFullPath());
    std::ofstream out;
	out.open(GET_ENCODED_FILENAME(gwt_ofn));

    if (!(out.is_open() && out.good())) {
        return false;
    }

    wxString layer_name(_layer_name);
    // if layer_name contains an empty space, the layer name should be
    // braced with quotes "layer name"
    if (layer_name.Contains(" ")) {
        layer_name = "\"" + layer_name + "\"";
    }

    out << "0" << " " << num_obs << " " << layer_name;
    out << " " << vname.mb_str() << endl;

    for (size_t i=0; i<num_obs; ++i) {
        for (long nbr=0, sz=g[i].Size(); nbr<sz; ++nbr) {
            GwtNeighbor current=g[i].elt(nbr);
            double w = current.weight;
            out << id_vec[i] << ' ' << id_vec[current.nbx];
			out << ' ' << setprecision(9) << w << endl;
        }
    }
    return true;
}

void SpatialIndAlgs::fill_box_rtree(rtree_box_2d_t& rtree,
									const Shapefile::Main& main_data)
{
	wxStopWatch sw;
	namespace sf = Shapefile;
	size_t obs = main_data.records.size();
	sf::PolygonContents* p;
	for (size_t i=0; i<obs; ++i) {
		p = (sf::PolygonContents*) main_data.records[i].contents_p;
		double xmin, ymin, xmax, ymax;
		get_shp_bb(p, xmin, ymin, xmax, ymax);
		box_2d b(pt_2d(xmin, ymin), pt_2d(xmax, ymax));
		rtree.insert(std::make_pair(b, i));
	}
}

void SpatialIndAlgs::fill_pt_rtree(rtree_pt_2d_t& rtree,
								   const std::vector<pt_2d>& pts)
{
	size_t obs = pts.size();
	for (size_t i=0; i<obs; ++i) {
		rtree.insert(make_pair(pts[i], i));
	}
}

void SpatialIndAlgs::fill_pt_rtree(rtree_pt_lonlat_t& rtree,
								   const std::vector<pt_lonlat>& pts)
{
	size_t obs = pts.size();
	for (size_t i=0; i<obs; ++i) {
		rtree.insert(make_pair(pts[i], i));
	}
}

void SpatialIndAlgs::fill_pt_rtree(rtree_pt_3d_t& rtree,
								   const std::vector<pt_3d>& pts)
{
	size_t obs = pts.size();
	for (size_t i=0; i<obs; ++i) {
		rtree.insert(make_pair(pts[i], i));
	}
}

std::ostream& SpatialIndAlgs::operator<< (std::ostream &out,
						  const LonLatPt& pt) {
	out << "(" << pt.lon << "," << pt.lat << ")";
    return out;
}

std::ostream& SpatialIndAlgs::operator<< (std::ostream &out,
										  const wxRealPoint& pt) {
	out << "(" << pt.x << "," << pt.y << ")";
    return out;
}

std::ostream& SpatialIndAlgs::operator<< (std::ostream &out, const XyzPt& pt) {
	out << "(" << pt.x << "," << pt.y << "," << pt.z << ")";
    return out;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file DiscreteFactorGraph.cpp
 *  @date Feb 14, 2011
 *  @author Duy-Nguyen Ta
 *  @author Frank Dellaert
 */

//#define ENABLE_TIMING
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/DiscreteBayesTree.h>
#include <gtsam/discrete/DiscreteEliminationTree.h>
#include <gtsam/discrete/DiscreteJunctionTree.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
#include <boost/make_shared.hpp>

namespace gtsam {

  // Instantiate base classes
  template class FactorGraph<DiscreteFactor>;
  template class EliminateableFactorGraph<DiscreteFactorGraph>;

  /* ************************************************************************* */
  bool DiscreteFactorGraph::equals(const This& fg, double tol) const
  {
    return Base::equals(fg, tol);
  }

  /* ************************************************************************* */
  KeySet DiscreteFactorGraph::keys() const {
    KeySet keys;
    for(const sharedFactor& factor: *this)
    if (factor) keys.insert(factor->begin(), factor->end());
    return keys;
  }

  /* ************************************************************************* */
  DecisionTreeFactor DiscreteFactorGraph::product() const {
    DecisionTreeFactor result;
    for(const sharedFactor& factor: *this)
      if (factor) result = (*factor) * result;
    return result;
  }

  /* ************************************************************************* */
  double DiscreteFactorGraph::operator()(
      const DiscreteFactor::Values &values) const {
    double product = 1.0;
    for( const sharedFactor& factor: factors_ )
      product *= (*factor)(values);
    return product;
  }

  /* ************************************************************************* */
  void DiscreteFactorGraph::print(const std::string& s,
      const KeyFormatter& formatter) const {
    std::cout << s << std::endl;
    std::cout << "size: " << size() << std::endl;
    for (size_t i = 0; i < factors_.size(); i++) {
      std::stringstream ss;
      ss << "factor " << i << ": ";
      if (factors_[i] != nullptr) factors_[i]->print(ss.str(), formatter);
    }
  }

//  /* ************************************************************************* */
//  void DiscreteFactorGraph::permuteWithInverse(
//    const Permutation& inversePermutation) {
//      for(const sharedFactor& factor: factors_) {
//        if(factor)
//          factor->permuteWithInverse(inversePermutation);
//      }
//  }
//
//  /* ************************************************************************* */
//  void DiscreteFactorGraph::reduceWithInverse(
//    const internal::Reduction& inverseReduction) {
//      for(const sharedFactor& factor: factors_) {
//        if(factor)
//          factor->reduceWithInverse(inverseReduction);
//      }
//  }

  /* ************************************************************************* */
  DiscreteFactorGraph::Values DiscreteFactorGraph::optimize() const
  {
    gttic(DiscreteFactorGraph_optimize);
    return BaseEliminateable::eliminateSequential()->optimize();
  }

  /* ************************************************************************* */
  std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>  //
  EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) {

    // PRODUCT: multiply all factors
    gttic(product);
    DecisionTreeFactor product;
    for(const DiscreteFactor::shared_ptr& factor: factors)
      product = (*factor) * product;
    gttoc(product);

    // sum out frontals, this is the factor on the separator
    gttic(sum);
    DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys);
    gttoc(sum);

    // Ordering keys for the conditional so that frontalKeys are really in front
    Ordering orderedKeys;
    orderedKeys.insert(orderedKeys.end(), frontalKeys.begin(), frontalKeys.end());
    orderedKeys.insert(orderedKeys.end(), sum->keys().begin(), sum->keys().end());

    // now divide product/sum to get conditional
    gttic(divide);
    DiscreteConditional::shared_ptr cond(new DiscreteConditional(product, *sum, orderedKeys));
    gttoc(divide);

    return std::make_pair(cond, sum);
  }

/* ************************************************************************* */
} // namespace


#include <iostream>
#include <fstream>
#include <iomanip>
#include <cmath>
#include <list>
#include <vector>
#include <unordered_map>
#include <algorithm>
#include <stdexcept>
#include <boost/tokenizer.hpp>
#include <boost/filesystem.hpp>
#include "string_tools.hpp"
#include <cassert>

std::unordered_map<std::string, size_t> dic_words;
std::vector<std::string> words;
int64_t * frequencies;

size_t get_filesize(const std::string & name_file)
{
	std::ifstream in(name_file, std::ifstream::ate | std::ifstream::binary);
	if (!in.is_open())
	{
		std::string message = "get_filesize() can not open file ";
		throw std::runtime_error (message + name_file);
	}
	return in.tellg();
}

template <typename T>
void safe_delete(T * &ptr)
{
	if (ptr != NULL)
	{
		delete[] ptr;
		ptr = NULL;
	}
}

template <typename T>
class Comparator_pos
{
public:
	const T* scores;
	Comparator_pos(const T*  _scores ): scores(_scores) {}
	bool operator()(size_t a, size_t b) const
	{
		return scores[a] < scores[b];
	}
};

template <typename T>
class Score
{
public:
	size_t id;
	T score;
	Score(size_t _id, T _score): id(_id), score(_score) {}
};

template<typename T>
class Sparse
{
public:
	T* data;
	T * cache_norm;
	size_t * row_ptr;
	size_t * col_ind;
	size_t cnt_nnz;
	size_t dim_x;
	size_t dim_y;
	Sparse(): data(NULL), cache_norm(NULL), row_ptr(NULL), col_ind(NULL)
	{}
	~Sparse()
	{
		safe_delete(data);
		safe_delete(cache_norm);
		safe_delete(row_ptr);
		safe_delete(col_ind);
	}
	void print()
	{
		std::cout << dim_x << " x " << dim_y << " sparse matrix\n";
		std::cout << " words_of_interesth " << cnt_nnz << " nonzero elements\n";
		std::cout << " [";
		for (size_t i = 0; i < 10; i++)
			std::cout << std::fixed << std::setprecision(4) << data[i] << " ";
//			std::cout<<std::fixed << std::setprecision(4) << col_ind[i]<<" ";
		std::cout << " ...]\n";
	}
	void prefetch_norm()
	{
		std::cerr << "prefetching norm \n";
		size_t i;
		cache_norm = new T[dim_y];
		for (i = 0; i < dim_y; i++)
		{
			T norm = 0;
			for (size_t x = row_ptr[i]; x < row_ptr[i + 1]; x++)
				norm += data[x] * data[x];
			cache_norm[i] = sqrt(norm);
		}
		std::cerr << "done \n";
	}
	void load(std::string path)
	{
		std::cerr << "loading bigrams \n";
		cnt_nnz = load_from_raw(path + "bigrams.data.bin", data);
		std::cerr << "loading col_ind \n";
		cnt_nnz = load_from_raw(path + "bigrams.col_ind.bin", col_ind);
		std::cerr << "loading row_ptr \n";
		dim_x = load_from_raw(path + "bigrams.row_ptr.bin", row_ptr);
		dim_y = dim_x;
		std::cerr << "dim y = " << dim_y << "\n";
		prefetch_norm();
	}
	inline T dotproduct_rows(size_t u, size_t v) const
	{
		size_t pos_u = row_ptr[u];
		size_t pos_v = row_ptr[v];
		T result = 0;
		while (true)
		{
			if (pos_u >= row_ptr[u + 1]) break;
			if (pos_v >= row_ptr[v + 1]) break;
			if (col_ind[pos_u] == col_ind[pos_v])
			{
				result += data[pos_u] * data[pos_v];
				pos_v++;
				pos_u++;
				continue;
			}
			if (col_ind[pos_u] < col_ind[pos_v])
			{
				pos_u++;
				continue;
			}
			if (col_ind[pos_v] < col_ind[pos_u])
			{
				pos_v++;
				continue;
			}
		}
		return result;
	}
	inline T cosine_distance(size_t u, size_t v) const
	{
		if ((cache_norm[u] == 0) || (cache_norm[v] == 0)) return 0;
		return (dotproduct_rows(u, v)) / (cache_norm[u] * cache_norm[v]);
	}

	template<size_t cnt_rows>
	std::list<Score<T>> get_most_similar_rows(size_t u)
	{
		if ((u < 0) || (u >= dim_y)) {throw std::runtime_error("word index out of range in get similar vectors");}
		T * scores = new T[dim_y];
		size_t * positions = new size_t[dim_y];
		#pragma omp parallel for
		for (size_t i = 0; i < dim_y; i++)
		{
			positions[i] = i;
			if (i != u)
			{
				scores[i] = cosine_distance(u, i) * log(0.1 + frequencies[i]);
			}
			else
			{
				scores[i] = 0;
			}
		}
		std::sort(positions, positions + dim_y, Comparator_pos<T>(scores));
		std::list<Score<T>> result;
		size_t cnt = cnt_rows;
		if (cnt > dim_y) cnt = dim_y;
		for (size_t i = 0; i < cnt_rows; i++)
		{
			if (i >= dim_y) break;
			size_t id = positions[dim_y - i - 1];
			if (scores[id] > 0)
				result.push_back(Score<T>(id, scores[id]));
		}

		delete [] scores;
		delete [] positions;
		return result;
	}

	template<size_t cnt_rows>
	std::list<Score<T>> get_most_similar_rows(std::string key)
	{
		if (dic_words.count(key) == 0)
		{
			std::list<Score<T>> empty;
			return empty;
		}
		return get_most_similar_rows<cnt_rows>(dic_words[key]);
	}
	void report_most_similar(std::string query)
	{
		auto most_similar = get_most_similar_rows<10>(query);
		std::cout << "\nmost similar rows to *" << query << "* [" << frequencies[dic_words[query]] << "] are: \n";
		for (auto i : most_similar)
			std::cout << "\t" << i.id << " - " << words[i.id] << " [" << frequencies[i.id] << "] - " << i.score << "\n";
	}
};

void load_word_ids(boost::filesystem::path dir_root)
{
	std::ifstream in((dir_root / boost::filesystem::path("ids")).string());
	if (!in.is_open()) {std::cerr << "can not open file\n";}
	std::string line;
	boost::char_separator<char> sep("\t");
	while ( std::getline( in, line ) )
	{
		boost::tokenizer<boost::char_separator<char> > tokens(line, sep);
		auto beg = tokens.begin();
		std::string key = *beg;
		beg++;
		std::string svalue = *beg;
		size_t value = stoull (svalue);
		dic_words.insert(std::pair<std::string, size_t>(key, value));
		words[value] = key;
	}
	in.close();
}
size_t load_frequencies(boost::filesystem::path dir_root)
{
	std::cerr << "loading frequencies";
	auto cnt_freq =  load_from_raw((dir_root / boost::filesystem::path("freq_per_id")).string(), frequencies);
	return  cnt_freq;
	/*
		std::ifstream in((dir_root / boost::filesystem::path("freq_per_id")).string());
		if (!in.is_open()) {std::cerr<<"can not open file\n";}
		std::string line;
	    size_t id = 0;
		while( std::getline( in, line ) )
		{
	    //	boost::tokenizer<boost::char_separator<char> > tokens(line, sep);
	      //  auto beg=tokens.begin();
	        //std::string svalue=*beg;
	        //size_t id = stoull (svalue);
	        //beg++;
	        //svalue=*beg;
	        size_t value = stoull (line);
	        frequencies[id++]=value;
	    }
	    in.close();
	    */
}

int main(int argc, char* argv[])
{
	std::string dir_root = "/storage/scratch/small_bin/";
	if (argc > 1)
		dir_root = std::string(argv[1]);
	std::cout << "opening " << dir_root << "\n";
	Sparse<float> m;
	m.load(dir_root);
	m.print();
	words.resize(m.dim_y + 1);
	//frequencies.resize(m.dim_y);
	//std::cerr<<"loading word ids\n";
	load_word_ids(dir_root);
	//std::cerr<<"done\n";
	auto cnt_f = load_frequencies(dir_root);
	std::cerr << "loaded " << cnt_f << " frequencies\n";
	//int i = 0;
	//int j = 4;

	//std::cerr << "norm of row "<< i << " = " << m.cache_norm[i] << "\n";
	//std::cerr << "dotproduct of rows "<< i <<" and " << j << " = " << m.dotproduct_rows(i,j)<<"\n";
	//std::cerr << "cosine distance between rows "<< i <<" and " << j << " = " << m.cosine_distance(i,j)<<"\n";
	//auto most_similar= m.get_most_similar_rows<10>(i);
	auto wordlist = load_words("words_of_interest.txt");
	for (auto i : wordlist)
		m.report_most_similar(i);

	return 0;
}


/**
 * \file
 *
 * \copyright
 * Copyright (c) 2012-2019, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/LICENSE.txt
 */

#include <gtest/gtest.h>
#include <algorithm>
#include <boost/variant.hpp>
#include <numeric>
#include <random>

#include "MeshLib/Elements/Element.h"
#include "MeshLib/Mesh.h"
#include "MeshLib/MeshEditing/ConvertToLinearMesh.h"
#include "MeshLib/MeshGenerators/MeshGenerator.h"
#include "MeshLib/MeshGenerators/QuadraticMeshGenerator.h"
#include "MeshLib/Node.h"

using namespace MeshLib;
using namespace std::string_literals;

class ConvertToLinearMesh : public ::testing::Test
{
public:
    ConvertToLinearMesh()
    {
        linear_mesh.reset(
            MeshGenerator::generateRegularHexMesh(1.0, mesh_size));
        quadratic_mesh = createQuadraticOrderMesh(*linear_mesh);
    }

    static constexpr int mesh_size = 5;

    std::unique_ptr<Mesh> linear_mesh;
    std::unique_ptr<Mesh> quadratic_mesh;
};

// Returns the (inverse) permutation vector of b_nodes to a_nodes, or an error
// message.
boost::variant<std::vector<std::size_t>, std::string> compareNodes(
    std::vector<Node*> const& a_nodes, std::vector<Node*> const& b_nodes)
{
    // For each 'a' mesh node find a corresponding 'b' mesh node, not checking
    // the ids, but store the id's in a map.
    std::vector<std::size_t> b_node_id(a_nodes.size());

    std::size_t const nnodes = a_nodes.size();
    for (std::size_t i = 0; i < nnodes; ++i)
    {
        Node* a_node = a_nodes[i];
        auto const b_it =
            std::find_if(begin(b_nodes), end(b_nodes), [&](Node* const b_node) {
                return *a_node ==
                       *b_node;  // coordinate comparison up to epsilon
            });
        if (b_it == end(b_nodes))
        {
            return "Could not find a matching node for node " +
                   std::to_string(a_node->getID()) + ".";
        }

        b_node_id[i] = distance(begin(b_nodes), b_it);
    }

    return b_node_id;
}

// Two elements are equal if their corresponding node coordinates are equal up
// to a (shift) permutation.
// Returns a string in case of error containing a descriptive message.
boost::optional<std::string> equal(Element const& a, Element const& b)
{
    if (typeid(a) != typeid(b))  // => (a.nodes.size == b.nodes.size)
    {
        return "Elements have different types."s;
    }

    auto const a_nodes = a.getNodes();
    auto const b_nodes = b.getNodes();
    auto const nnodes = a.getNumberOfNodes();

    std::size_t first_b_node = 0;
    while (first_b_node < nnodes)
    {
        if (*a_nodes[0] == *b_nodes[first_b_node])
        {
            break;
        }
        ++first_b_node;
    }
    if (first_b_node == nnodes)
    {
        return "Did not find first node of the element " +
               std::to_string(a.getID()) + " in the nodes of the element " +
               std::to_string(b.getID()) + ".";
    }

    for (std::size_t i = 1; i < nnodes; ++i)
    {
        if (!(*a_nodes[i] == *b_nodes[(first_b_node + i) % nnodes]))
        {
            return "node " + std::to_string(i) + " is not equal to the node " +
                   std::to_string((first_b_node + i) % nnodes) + ".";
        }
    }

    return {};  // All nodes are equal.
}

// Returns a vector filled with numbers [0, size) randomly permuted.
std::vector<std::size_t> generateRandomPermutationVector(std::size_t const size)
{
    std::random_device rd;
    std::mt19937 random_generator(rd());

    std::vector<std::size_t> permutation(size);
    std::iota(begin(permutation), end(permutation), 0);
    std::shuffle(begin(permutation), end(permutation), random_generator);
    return permutation;
}

// Test the inverse permutation for being the inverse: p^-1(p(.)) = Id.
// In case of failure an error message is returned.
boost::optional<std::string> inversePermutationIdentityTest(
    std::vector<std::size_t> const& permutation,
    std::vector<std::size_t> const& inverse_permutation)
{
    if (permutation.size() != inverse_permutation.size())
    {
        return "Inverse permutation size differs from the permutation size: " +
               std::to_string(inverse_permutation.size()) + " vs. " +
               std::to_string(permutation.size()) + ".";
    }

    for (std::size_t i = 0; i < permutation.size(); ++i)
    {
        auto const p = permutation[i];
        if (inverse_permutation[p] == i)
        {
            continue;
        }
        return "Inverse permutation element " + std::to_string(p) +
               " does not point to " + std::to_string(i) +
               "-th element but to " + std::to_string(inverse_permutation[p]) +
               ".";
    }
    return {};
}

// Create new nodes in permuted order with new ids.
std::vector<Node*> permuteNodes(std::vector<std::size_t> const& permutation,
                                std::vector<Node*> const& nodes)
{
    std::vector<Node*> permuted_nodes;
    for (std::size_t i = 0; i < permutation.size(); ++i)
    {
        auto const node_p_i = nodes[permutation[i]];
        permuted_nodes.push_back(new Node{node_p_i->getCoords(), i});
    }
    return permuted_nodes;
}

// Tests the nodes of two meshes and returns a string in case of error with
// message, otherwise a permutation vector as in compareNodes().
boost::variant<std::vector<std::size_t>, std::string> compareNodeVectors(
    std::vector<Node*> const& a, std::vector<Node*> const& b)
{
    if (a.size() != b.size())
    {
        return "There are different number of nodes: " +
               std::to_string(a.size()) + " and " + std::to_string(b.size()) +
               ".";
    }

    auto const compare_nodes_result = compareNodes(a, b);
    if (compare_nodes_result.type() == typeid(std::string))
    {
        return "Node comparison failed: " +
               boost::get<std::string>(compare_nodes_result);
    }

    return boost::get<std::vector<std::size_t>>(compare_nodes_result);
}

TEST_F(ConvertToLinearMesh, GeneratedHexMeshRandomizedNodes)
{
    ASSERT_TRUE(linear_mesh != nullptr);
    ASSERT_TRUE(quadratic_mesh != nullptr);

    auto const permutation =
        generateRandomPermutationVector(quadratic_mesh->getNumberOfNodes());

    // Create new nodes in permuted order with new ids.
    auto const permuted_quadratic_nodes =
        permuteNodes(permutation, quadratic_mesh->getNodes());

    //
    // Test the permuted nodes and compute the inverse permutation map.
    //
    auto const compare_nodes_result = compareNodeVectors(
        quadratic_mesh->getNodes(), permuted_quadratic_nodes);
    ASSERT_FALSE(compare_nodes_result.type() == typeid(std::string))
        << "Quadratic mesh nodes comparison with permuted quadratic nodes "
           "failed.\n"
        << boost::get<std::string>(compare_nodes_result);

    auto const& inverse_permutation =
        boost::get<std::vector<std::size_t>>(compare_nodes_result);
    {
        auto const result =
            inversePermutationIdentityTest(permutation, inverse_permutation);
        ASSERT_TRUE(result == boost::none)
            << "Quadratic mesh nodes permutation test failed: " << *result;
    }

    auto clone_element_using_permuted_nodes = [&](Element* const e) {
        Node** nodes = new Node*[e->getNumberOfNodes()];
        for (std::size_t i = 0; i < e->getNumberOfNodes(); ++i)
        {
            auto const q_i = e->getNode(i)->getID();
            auto const r = inverse_permutation[q_i];
            nodes[i] = permuted_quadratic_nodes[r];
        }
        return e->clone(nodes, e->getID());
    };

    // Create new elements in the same order, but using new nodes.
    std::vector<Element*> new_quadratic_elements;
    std::transform(begin(quadratic_mesh->getElements()),
                   end(quadratic_mesh->getElements()),
                   back_inserter(new_quadratic_elements),
                   clone_element_using_permuted_nodes);

    Mesh permuted_nodes_quadratic_mesh{"permuted_quadratic_mesh",
                                       permuted_quadratic_nodes,
                                       new_quadratic_elements};
    //
    // Test the quadratic meshes first.
    //
    {
        ASSERT_EQ(quadratic_mesh->getNumberOfElements(),
                  permuted_nodes_quadratic_mesh.getNumberOfElements());

        auto const nelements = quadratic_mesh->getNumberOfElements();
        for (std::size_t i = 0; i < nelements; ++i)
        {
            auto const& element_a = *quadratic_mesh->getElement(i);
            auto const& element_b =
                *permuted_nodes_quadratic_mesh.getElement(i);
            auto const elements_are_equal = equal(element_a, element_b);
            ASSERT_TRUE(elements_are_equal == boost::none)
                << *elements_are_equal << " For the element " << i << ".";
        }
    }

    {
        auto const converted_mesh = convertToLinearMesh(
            permuted_nodes_quadratic_mesh, "back_to_linear_mesh");

        //
        // Two meshes are equal if all of their nodes have exactly same
        // coordinates.
        //
        auto const compare_nodes_result = compareNodeVectors(
            linear_mesh->getNodes(), converted_mesh->getNodes());
        ASSERT_FALSE(compare_nodes_result.type() == typeid(std::string))
            << "Linear mesh nodes comparison with converted linear mesh nodes "
               "failed.\n"
            << boost::get<std::string>(compare_nodes_result);
        // TODO (naumov) check inverse permutation, but it requires the reduced
        // to base nodes forward permutation vector first.

        // Two meshes are equal if their elements share same nodes.
        ASSERT_EQ(linear_mesh->getNumberOfElements(),
                  converted_mesh->getNumberOfElements());

        auto const linear_mesh_nelements = linear_mesh->getNumberOfElements();
        for (std::size_t i = 0; i < linear_mesh_nelements; ++i)
        {
            auto const& element_a = *linear_mesh->getElement(i);
            auto const& element_b = *converted_mesh->getElement(i);
            auto const elements_are_equal = equal(element_a, element_b);
            ASSERT_TRUE(elements_are_equal == boost::none)
                << *elements_are_equal << " For the element " << i << ".";
        }
    }
}

TEST_F(ConvertToLinearMesh, GeneratedHexMeshBackToLinear)
{
    ASSERT_TRUE(linear_mesh != nullptr);
    ASSERT_TRUE(quadratic_mesh != nullptr);

    auto const converted_mesh =
        convertToLinearMesh(*quadratic_mesh, "back_to_linear_mesh");

    //
    // Two meshes are equal if all of their nodes have exactly same coordinates.
    //
    auto const compare_nodes_result =
        compareNodeVectors(linear_mesh->getNodes(), converted_mesh->getNodes());
    ASSERT_FALSE(compare_nodes_result.type() == typeid(std::string))
        << "Linear mesh nodes comparison with converted linear mesh nodes "
           "failed.\n"
        << boost::get<std::string>(compare_nodes_result);

    // Two meshes are equal if their elements share same nodes.
    ASSERT_EQ(linear_mesh->getNumberOfElements(),
              converted_mesh->getNumberOfElements());

    auto const linear_mesh_nelements = linear_mesh->getNumberOfElements();
    for (std::size_t i = 0; i < linear_mesh_nelements; ++i)
    {
        auto const& linear_mesh_element = *linear_mesh->getElement(i);
        auto const& converted_mesh_element = *converted_mesh->getElement(i);
        auto const elements_are_equal =
            equal(linear_mesh_element, converted_mesh_element);
        ASSERT_TRUE(elements_are_equal == boost::none)
            << *elements_are_equal << " For the element " << i << ".";
    }
}


#include <fstream>
#include <iostream>
#include <sstream>
#include <cmath>

#include "str_util.h"
#include "infer.h"
#include <boost/algorithm/string.hpp>
#include "str_util.h"

using namespace str_util;

Infer::Infer(const string & type, const Pvec<double> & pz, const Pmat<double> & pw_z, int K) :
    type(type), pz(pz), pw_z(pw_z), K(K)
  {

  }

Pmat<double> Infer::predict(const std::vector<std::string> & docs, const std::unordered_map<std::string, int> & w2id) {
	std::vector<Pvec<double>> predictions;
	for (const std::string& line : docs) {
    	std::vector<int> word_idx;
    	std::vector<std::string> words = split(line);
		for (std::string w : words) {
      		w = trim(w);
		    if (w2id.find(w) == w2id.end()) {
        		word_idx.push_back(0);
      		} else {
      			word_idx.push_back(w2id.at(w));
			}
		}
		Doc doc(word_idx);
		Pvec<double> pz_d(K);
		doc_infer(doc, pz_d);
		predictions.push_back(pz_d);
	}
	return Pmat<double>(predictions);
}

void Infer::doc_infer(const Doc& doc, Pvec<double>& pz_d) {
  if (type == "sum_b")
	doc_infer_sum_b(doc, pz_d);
  else if (type == "sub_w")
	doc_infer_sum_w(doc, pz_d);
  else if (type == "mix")
	doc_infer_mix(doc, pz_d);
  else {
	cout << "[Err] unkown infer type:" << type << endl;
	exit(1);
  }
}

// p(z|d) = \sum_b{ p(z|b)p(b|d) }
void Infer::doc_infer_sum_b(const Doc& doc, Pvec<double>& pz_d) {
  pz_d.assign(K, 0);

  if (doc.size() == 1) {
	// doc is a single word, p(z|d) = p(z|w) \propo p(z)p(w|z)
	for (int k = 0; k < K; ++k)
	  pz_d[k] = pz[k] * pw_z[k][doc.get_w(0)];
  }
  else {
	// more than one words
	vector<Biterm> bs;
	doc.gen_biterms(bs);

	int W = pw_z.cols();
	for (int b = 0; b < bs.size(); ++b) {
	  int w1 = bs[b].get_wi();
	  int w2 = bs[b].get_wj();

	  // filter out-of-vocabulary words
	  if (w2 >= W) continue;

	  // compute p(z|b) \propo p(w1|z)p(w2|z)p(z)
	  Pvec<double> pz_b(K);
	  for (int k = 0; k < K; ++k) {
		assert(pw_z[k][w1]>0 && pw_z[k][w2]>0);
		pz_b[k] = pz[k] * pw_z[k][w1] * pw_z[k][w2];
	  }
	  pz_b.normalize();

	  // sum for b, p(b|d) is unifrom
	  for (int k = 0; k < K; ++k)
		pz_d[k] += pz_b[k];
	}
  }

  pz_d.normalize();
}

// p(z|d) = \sum_w{ p(z|w)p(w|d) }
void Infer::doc_infer_sum_w(const Doc& doc, Pvec<double>& pz_d) {
  pz_d.assign(K, 0);

  int W = pw_z.cols();
  const vector<int>& ws = doc.get_ws();

  for (int i = 0; i < ws.size(); ++i) {
	int w = ws[i];
	if (w >= W) continue;

	// compute p(z|w) \propo p(w|z)p(z)
	Pvec<double> pz_w(K);
	for (int k = 0; k < K; ++k)
	  pz_w[k] = pz[k] * pw_z[k][w];

	pz_w.normalize();

	// sum for b, p(b|d) is unifrom
	for (int k = 0; k < K; ++k)
	  pz_d[k] += pz_w[k];
  }
  pz_d.normalize();
}

void Infer::doc_infer_mix(const Doc& doc, Pvec<double>& pz_d) {
  pz_d.resize(K);
  for (int k = 0; k < K; ++k)
	pz_d[k] = pz[k];

  const vector<int>& ws = doc.get_ws();
  int W = pw_z.cols();
  for (int i = 0; i < ws.size(); ++i) {
	int w = ws[i];
	if (w >= W) continue;

	for (int k = 0; k < K; ++k)
	  pz_d[k] *= (pw_z[k][w] * W);
  }

	// sum for b, p(b|d) is unifrom
  pz_d.normalize();
}

// compute p(z|d, w) \proto p(w|z)p(z|d)
void Infer::compute_pz_dw(int w, const Pvec<double>& pz_d, Pvec<double>& p) {
  p.resize(K);

  for (int k = 0; k < K; ++k)
	p[k] = pw_z[k][w] * pz_d[k];

  p.normalize();
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_SWAR_FUNCTIONS_COMPLEX_SIMD_COMMON_SPLATTED_SUM_HPP_INCLUDED
#define NT2_TOOLBOX_SWAR_FUNCTIONS_COMPLEX_SIMD_COMMON_SPLATTED_SUM_HPP_INCLUDED
#include <nt2/toolbox/swar/functions/splatted_sum.hpp>
#include <boost/dispatch/meta/strip.hpp>
#include <nt2/include/functions/sum.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/sdk/complex/meta/as_real.hpp>
#include <nt2/include/functions/imag.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::splatted_sum_, tag::cpu_,
                               (A0)(X),
                               ((simd_<complex_<arithmetic_<A0> >,X>))
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL_REPEAT(1)
    {
      return nt2::splat<A0>(sum(a0));
    }
  };
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::splatted_sum_, tag::cpu_,
                               (A0)(X),
                               ((simd_<imaginary_<arithmetic_<A0> >,X>))
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL_REPEAT(1)
    {
      return nt2::splat<A0>(sum(a0));
    }
  };
   NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::splatted_sum_, tag::cpu_,
                               (A0)(X),
                               ((simd_<dry_<arithmetic_<A0> >,X>))
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL_REPEAT(1)
    {
      return nt2::splat<A0>(sum(a0));
    }
  };
} }
#endif


// Copyright (C) 2020 Christian Brommer, Control of Networked Systems, Universit?t Klagenfurt, Austria
// You can contact the author at <christian.brommer@ieee.org>
//
// All rights reserved.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <gmock/gmock.h>
#include <mars/core_logic.h>
#include <mars/core_state.h>
#include <mars/data_utils/read_pose_data.h>
#include <mars/data_utils/read_sim_data.h>
#include <mars/sensors/imu/imu_sensor_class.h>
#include <mars/type_definitions/buffer_entry_type.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include "include_local/thl_example_data_settings.h"

bool read_yaml_vec_3(std::vector<double>* value, const std::string& parameter, YAML::Node config)
{
  if (config[parameter])
  {
    *value = config[parameter].as<std::vector<double>>();

    std::cout << parameter << ": \t [";
    for (auto const& i : *value)
      std::cout << i << " ";

    std::cout << " ]" << std::endl;
    return true;
  }
  return false;
}

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

  std::string test_data_path = std::string(MARS_LIB_THL_EXAMPLE_DATA_PATH);

  // get config
  YAML::Node config = YAML::LoadFile(test_data_path + "parameter.yaml");

  std::string traj_file_name = config["traj_file_name"].as<std::string>();
  std::cout << "Trajectory File: " << traj_file_name << std::endl;

  std::string pose_file_name = config["pose_file_name"].as<std::string>();
  std::cout << "Pose File: " << pose_file_name << std::endl;

  std::vector<double> imu_n_w;
  std::vector<double> imu_n_bw;
  std::vector<double> imu_n_a;
  std::vector<double> imu_n_ba;

  std::cout << "IMU Noise Parameter: " << std::endl;
  read_yaml_vec_3(&imu_n_w, "imu_n_w", config);
  read_yaml_vec_3(&imu_n_bw, "imu_n_bw", config);
  read_yaml_vec_3(&imu_n_a, "imu_n_a", config);
  read_yaml_vec_3(&imu_n_ba, "imu_n_ba", config);

  // setup propagation sensor
  std::shared_ptr<mars::ImuSensorClass> imu_sensor_sptr = std::make_shared<mars::ImuSensorClass>("IMU");

  // setup the core definition
  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  core_states_sptr.get()->set_propagation_sensor(imu_sensor_sptr);
  core_states_sptr.get()->set_noise_std(Eigen::Vector3d(imu_n_w.data()), Eigen::Vector3d(imu_n_bw.data()),
                                        Eigen::Vector3d(imu_n_a.data()), Eigen::Vector3d(imu_n_ba.data()));

  // setup additional sensors
  // Pose sensor
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose", core_states_sptr);
  pose_sensor_sptr->const_ref_to_nav_ =
      true;  // TODO is set here for now but will be managed by core logic in later versions

  // Define measurement noise
  Eigen::Matrix<double, 6, 1> pose_meas_std;
  pose_meas_std << 0.02, 0.02, 0.02, 2 * (M_PI / 180), 2 * (M_PI / 180), 2 * (M_PI / 180);
  pose_sensor_sptr->R_ = pose_meas_std.cwiseProduct(pose_meas_std);

  // Define initial calibration and covariance
  mars::PoseSensorData pose_init_cal;
  pose_init_cal.state_.p_ip_ = Eigen::Vector3d::Zero();
  pose_init_cal.state_.q_ip_ = Eigen::Quaterniond::Identity();

  // The covariance should enclose the initialization with a 3 Sigma bound
  Eigen::Matrix<double, 6, 1> std;
  std << 0.1, 0.1, 0.1, (10 * M_PI / 180), (10 * M_PI / 180), (10 * M_PI / 180);
  pose_init_cal.sensor_cov_ = std.cwiseProduct(std).asDiagonal();

  pose_sensor_sptr->set_initial_calib(std::make_shared<mars::PoseSensorData>(pose_init_cal));

  // load data
  std::vector<mars::BufferEntryType> measurement_data;

  {  // keep individual measurement data limited to this scope
    std::vector<mars::BufferEntryType> measurement_data_imu;
    mars::ReadSimData(&measurement_data_imu, imu_sensor_sptr, test_data_path + traj_file_name);

    std::vector<mars::BufferEntryType> measurement_data_pose;
    mars::ReadPoseData(&measurement_data_pose, pose_sensor_sptr, test_data_path + pose_file_name);

    measurement_data.insert(measurement_data.end(), measurement_data_imu.begin(), measurement_data_imu.end());
    measurement_data.insert(measurement_data.end(), measurement_data_pose.begin(), measurement_data_pose.end());
  }

  std::sort(measurement_data.begin(), measurement_data.end());

  // generate a single out of order measurement
  // std::swap(measurement_data[31],measurement_data[32]);

  // create the CoreLogic and link the core states
  mars::CoreLogic core_logic(core_states_sptr);

  // Open file for data export
  std::ofstream ofile_core;
  ofile_core.open("/tmp/mars_core_state.csv", std::ios::out);
  ofile_core << std::setprecision(17);

  std::ofstream ofile_pose;
  ofile_pose.open("/tmp/mars_pose_state.csv", std::ios::out);
  ofile_pose << std::setprecision(17);

  // process data
  for (auto k : measurement_data)
  {
    core_logic.ProcessMeasurement(k.sensor_, k.timestamp_, k.data_);

    if (!core_logic.core_is_initialized_)
    {
      // Initialize the first time at which the propagation sensor occures
      if (k.sensor_ == core_logic.core_states_->propagation_sensor_)
      {
        Eigen::Vector3d p_wi_init(0, 0, 5);
        Eigen::Quaterniond q_wi_init = Eigen::Quaterniond::Identity();
        core_logic.Initialize(p_wi_init, q_wi_init);
      }
      else
      {
        continue;
      }
    }

    // Store results in a csv file
    if (k.sensor_ == core_logic.core_states_->propagation_sensor_)
    {
      mars::BufferEntryType latest_result;
      core_logic.buffer_.get_latest_state(&latest_result);
      mars::CoreStateType last_state = static_cast<mars::CoreType*>(latest_result.data_.core_.get())->state_;
      ofile_core << last_state.to_csv_string(latest_result.timestamp_.get_seconds()) << std::endl;
    }

    if (k.sensor_ == pose_sensor_sptr)
    {
      // Repropagation after an out of order update can cause the latest state to be different from the current update
      // sensor. Using get_latest_sensor_handle_state is the safest option.
      mars::BufferEntryType latest_result;
      core_logic.buffer_.get_latest_sensor_handle_state(pose_sensor_sptr, &latest_result);
      mars::PoseSensorStateType last_state = pose_sensor_sptr->get_state(latest_result.data_.sensor_);
      ofile_pose << last_state.to_csv_string(latest_result.timestamp_.get_seconds()) << std::endl;
    }
  }

  ofile_core.close();
  ofile_pose.close();

  mars::BufferEntryType latest_result;
  core_logic.buffer_.get_latest_state(&latest_result);
  mars::CoreStateType last_state = static_cast<mars::CoreType*>(latest_result.data_.core_.get())->state_;

  std::cout << "Last State:" << std::endl;
  std::cout << last_state << std::endl;

  // Define final ground truth values
  Eigen::Vector3d true_p_wi(-20946.817372738657, -3518.039994126535, 8631.1520460773336);
  Eigen::Vector3d true_v_wi(15.924719563070044, -20.483884216740151, 11.455154466026718);
  Eigen::Quaterniond true_q_wi(0.98996033625708202, 0.048830414166879263, -0.02917972697860232, -0.12939345742158029);

  std::cout << "p_wi error [m]: [" << (last_state.p_wi_ - true_p_wi).transpose() << " ]" << std::endl;
  std::cout << "v_wi error [m/s]: [" << (last_state.v_wi_ - true_v_wi).transpose() << " ]" << std::endl;

  Eigen::Quaterniond q_wi_error(last_state.q_wi_.conjugate() * true_q_wi);

  std::cout << "q_wi error [w,x,y,z]: [" << q_wi_error.w() << " " << q_wi_error.vec().transpose() << " ]" << std::endl;

  return 0;
}


//==================================================================================================
/*!
  @file

  @copyright 2012-2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANT_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
/*!
    @ingroup group-constant
    Generates a value of the chosen type from an exact bits pattern.

    Constant generates a value from an exact bits pattern that get splat in the return
    value.

    @par Semantic:

    or any type @c T and any integral constant @c N :

    @code
    T r = Constant<T,N>();
    @endcode

    generates a values which bit pattern can be represent by the integer N.

    @tparam Type  Type of the desired value
    @tparam Bits  Bits pattern to generate as a value

    @return A value of type @c T which bits are exactly equals to @c N
  **/
  template<typename Type, std::uintmax_t Bits> auto Constant();
} }
#endif

#include <boost/simd/constant/definition/constant.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_generator.hpp>
#include <boost/simd/arch/common/simd/constant/constant_generator.hpp>

#endif


//               Copyright Marek Dalewski 2017
// Distributed under the Boost Software License, Version 1.0.
//        (See accompanying file LICENSE.md or copy at
//           http://www.boost.org/LICENSE_1_0.txt)

#include <commander/algorithm.hpp>

#include <boost/test/unit_test.hpp>

#include <tuple>
#include <utility>
#include <vector>

BOOST_AUTO_TEST_CASE( tuple_visit_conjunction )
{
    std::vector<std::tuple<bool, uint32_t>> results;

    auto call_always_true = [&results](const auto& item) -> bool {
        results.emplace_back(true, item);
        return true;
    };
    auto call_not_4 = [&results](const auto& item) -> bool {
        results.emplace_back((uint32_t(item) != 4), item);
        return (uint32_t(item) != 4);
    };
    auto call_not_6 = [&results](const auto& item) -> bool {
        results.emplace_back((uint32_t(item) != 6), item);
        return (uint32_t(item) != 6);
    };

    std::tuple<uint8_t, uint16_t, uint32_t> tuple_of_ints   = {0, 1, 2};
    std::tuple<float, double, long double>  tuple_of_floats = {3.3, 4.4, 5.5};
    std::tuple<uint64_t>                    tuple_uint64_t  = {6};

    {
        results.clear();
        BOOST_CHECK(commander::tuple_visit_conjunction(call_always_true, tuple_of_ints) == true);
        BOOST_CHECK(results.size() == 3);
        BOOST_CHECK(results.at(0) == std::forward_as_tuple(true, 0));
        BOOST_CHECK(results.at(1) == std::forward_as_tuple(true, 1));
        BOOST_CHECK(results.at(2) == std::forward_as_tuple(true, 2));

        results.clear();
        BOOST_CHECK(commander::tuple_visit_conjunction(call_always_true, tuple_of_ints, tuple_of_floats) == true);
        BOOST_CHECK(results.size() == 6);
        BOOST_CHECK(results.at(0) == std::forward_as_tuple(true, 0));
        BOOST_CHECK(results.at(1) == std::forward_as_tuple(true, 1));
        BOOST_CHECK(results.at(2) == std::forward_as_tuple(true, 2));
        BOOST_CHECK(results.at(3) == std::forward_as_tuple(true, 3));
        BOOST_CHECK(results.at(4) == std::forward_as_tuple(true, 4));
        BOOST_CHECK(results.at(5) == std::forward_as_tuple(true, 5));
    }

    {
        results.clear();
        BOOST_CHECK(commander::tuple_visit_conjunction(call_not_4, tuple_of_ints) == true);
        BOOST_CHECK(results.size() == 3);
        BOOST_CHECK(results.at(0) == std::forward_as_tuple(true, 0));
        BOOST_CHECK(results.at(1) == std::forward_as_tuple(true, 1));
        BOOST_CHECK(results.at(2) == std::forward_as_tuple(true, 2));

        results.clear();
        BOOST_CHECK(commander::tuple_visit_conjunction(call_not_4, tuple_of_ints, tuple_of_floats) == false);
        BOOST_CHECK(results.size() == 5);
        BOOST_CHECK(results.at(0) == std::forward_as_tuple(true, 0));
        BOOST_CHECK(results.at(1) == std::forward_as_tuple(true, 1));
        BOOST_CHECK(results.at(2) == std::forward_as_tuple(true, 2));
        BOOST_CHECK(results.at(3) == std::forward_as_tuple(true, 3));
        BOOST_CHECK(results.at(4) == std::forward_as_tuple(false, 4));
    }

    {
        results.clear();
        BOOST_CHECK(commander::tuple_visit_conjunction(call_always_true, tuple_uint64_t) == true);
        BOOST_CHECK(results.size() == 1);
        BOOST_CHECK(results.at(0) == std::forward_as_tuple(true, 6));

        results.clear();
        BOOST_CHECK(commander::tuple_visit_conjunction(call_always_true, tuple_uint64_t, tuple_of_ints) == true);
        BOOST_CHECK(results.size() == 4);
        BOOST_CHECK(results.at(0) == std::forward_as_tuple(true, 6));
        BOOST_CHECK(results.at(1) == std::forward_as_tuple(true, 0));
        BOOST_CHECK(results.at(2) == std::forward_as_tuple(true, 1));
        BOOST_CHECK(results.at(3) == std::forward_as_tuple(true, 2));

        results.clear();
        BOOST_CHECK(commander::tuple_visit_conjunction(call_always_true, tuple_of_ints, tuple_uint64_t) == true);
        BOOST_CHECK(results.size() == 4);
        BOOST_CHECK(results.at(0) == std::forward_as_tuple(true, 0));
        BOOST_CHECK(results.at(1) == std::forward_as_tuple(true, 1));
        BOOST_CHECK(results.at(2) == std::forward_as_tuple(true, 2));
        BOOST_CHECK(results.at(3) == std::forward_as_tuple(true, 6));
    }

    {
        results.clear();
        BOOST_CHECK(commander::tuple_visit_conjunction(call_not_6, tuple_uint64_t) == false);
        BOOST_CHECK(results.size() == 1);
        BOOST_CHECK(results.at(0) == std::forward_as_tuple(false, 6));

        results.clear();
        BOOST_CHECK(commander::tuple_visit_conjunction(call_not_6, tuple_uint64_t, tuple_of_ints) == false);
        BOOST_CHECK(results.size() == 1);
        BOOST_CHECK(results.at(0) == std::forward_as_tuple(false, 6));

        results.clear();
        BOOST_CHECK(commander::tuple_visit_conjunction(call_not_6, tuple_of_ints, tuple_uint64_t) == false);
        BOOST_CHECK(results.size() == 4);
        BOOST_CHECK(results.at(0) == std::forward_as_tuple(true, 0));
        BOOST_CHECK(results.at(1) == std::forward_as_tuple(true, 1));
        BOOST_CHECK(results.at(2) == std::forward_as_tuple(true, 2));
        BOOST_CHECK(results.at(3) == std::forward_as_tuple(false, 6));
    }
}


/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
* \file     social_force.cpp
* \author   Collin Johnson
*
* Definition of interaction_social_forces and trajectory_social_forces.
*/

#include <mpepc/evaluation/social_force.h>
#include <mpepc/evaluation/interaction.h>
#include <core/line.h>
#include <boost/optional.hpp>
#include <tuple>

// #define DEBUG_PASSING

namespace vulcan
{
namespace mpepc
{

struct social_forces_state_t
{
    double radius;
    position_t position;
    Vector heading;
    Vector velocity;

    social_forces_state_t(void)
    : radius(1.0)
    , heading(2)
    , velocity(2)
    {
    }
};

// get<0> = blame, get<1> = force
std::tuple<double, double> forces_from_agent(const topo_agent_t& agent,
                                             const social_forces_state_t& state,
                                             const social_forces_params_t& params);
boost::optional<passing_object_t> is_passing_object(const topo_agent_t& agent,
                                                    const math::Rectangle<float>& robotBoundary,
                                                    double speed,
                                                    double heading);
void assign_ids_to_passing_objects(std::vector<social_forces_t>& forces);

social_forces_t interaction_social_forces(const interaction_t& interaction, const social_forces_params_t& params)
{
    social_forces_t forces;
    forces.timestamp = interaction.timestamp;
    forces.pose = interaction.pose;
    forces.isInteracting = !interaction.agents.empty();

    social_forces_state_t state;
    state.position = interaction.pose.toPoint();
    state.heading[0] = std::cos(forces.pose.theta);
    state.heading[1] = std::sin(forces.pose.theta);

    state.velocity = state.heading * interaction.velocity.linear;

    forces.blame = 0.0;
    forces.force = 0.0;

    double agentBlame = 0.0;
    double agentForce = 0.0;

    auto robotBoundary = params.robotBoundary;
    robotBoundary.rotate(interaction.pose.theta);
    robotBoundary.translate(interaction.pose.x, interaction.pose.y);

    for(auto& agent : interaction.agents)
    {
        std::tie(agentBlame, agentForce) = forces_from_agent(agent, state, params);

        if(agentBlame > forces.blame)
        {
            forces.blame = agentBlame;
            forces.blameObj = position_t(agent.state.x, agent.state.y);
        }

        if(agentForce > forces.force)
        {
            forces.force = agentForce;
            forces.forceObj = position_t(agent.state.x, agent.state.y);
        }

        if(agent.areaId == interaction.areaId)
        {
            if(auto passingObj = is_passing_object(agent,
                                                   robotBoundary,
                                                   interaction.velocity.linear,
                                                   interaction.pose.theta))
            {
                forces.passingObj.push_back(*passingObj);
            }
        }
    }

    return forces;
}

std::vector<social_forces_t> trajectory_social_forces(const std::vector<interaction_t>& interactions,
                                                      const social_forces_params_t& params)
{
    std::vector<social_forces_t> forces;

    std::transform(interactions.begin(), interactions.end(), std::back_inserter(forces), [&](const auto& i) {
        return interaction_social_forces(i, params);
    });

    assign_ids_to_passing_objects(forces);

    return forces;
}

std::tuple<double, double> forces_from_agent(const topo_agent_t& agent,
                                             const social_forces_state_t& state,
                                             const social_forces_params_t& params)
{
    Vector agentDir(2);
    agentDir[0] = agent.state.x - state.position.x;
    agentDir[1] = agent.state.y - state.position.y;

    double distToAgent = distance_between_points(agent.state.x,
                                                       agent.state.y,
                                                       state.position.x,
                                                       state.position.y);
    distToAgent -= agent.radius + params.robotRadius;
    distToAgent = std::max(0.0, distToAgent);

    double cosPhi = -arma::as_scalar(arma::trans(agentDir) * state.heading);
    double anisotropicScale = params.lambda_p + ((1.0 - params.lambda_p) * (0.5 * (1.0 + cosPhi)));
    double socialForce = params.a_p * std::exp(-distToAgent / params.b_p) * anisotropicScale;

    Point<double> futurePosition(state.position.x + (state.velocity[0] * params.tau),
                                       state.position.y + (state.velocity[1] * params.tau));
    Line<double> robotTraj(state.position, futurePosition);
    Point<double> agentPosition(agent.state.x, agent.state.y);
    Point<double> projectedAgentPosition = closest_point_on_line_segment(agentPosition, robotTraj);
    distToAgent = distance_between_points(agentPosition, projectedAgentPosition);

    double blame = 1.0 / (1.0 + std::exp(distToAgent));

    return std::make_tuple(blame, socialForce);
}

boost::optional<passing_object_t> is_passing_object(const topo_agent_t& agent,
                                                    const math::Rectangle<float>& robotBoundary,
                                                    const double speed,
                                                    const double heading)
{
    // If not moving, not actually passing anything!
    if(speed < 0.25)
    {
        return boost::none;
    }

    auto agentPosition = Point<float>(agent.state.x, agent.state.y);
    auto nearestPoint = robotBoundary.closestPointOnBoundary(agentPosition);
    double distToAgent = distance_between_points(agentPosition, nearestPoint.first) - agent.radius;

    double agentSpeed = std::sqrt((agent.state.xVel * agent.state.xVel) + (agent.state.yVel * agent.state.yVel));
    double agentHeading = std::atan2(agent.state.yVel, agent.state.xVel);
    bool isOncoming = angle_diff_abs(agentHeading, heading) > M_PI_2;
    double relativeSpeed = isOncoming ? speed + agentSpeed : speed - agentSpeed;

    // If inside the robot, it must be passing VERY close!
    if((distToAgent < 0.0) || robotBoundary.contains(agentPosition))
    {
#ifdef DEBUG_PASSING
        std::cout << "Hitting something: " << robotBoundary << " agent: " << agentPosition << " radius: "
            << agent.radius << " dist: " << distToAgent << '\n';
#endif // DEBUG_PASSING
        return passing_object_t(agentPosition, 0.0, relativeSpeed, math::RectSide::inside);
    }

    // If the closest point is an endpoint, then not moving along one of the sides
    if((nearestPoint.first == robotBoundary.bottomLeft) || (nearestPoint.first == robotBoundary.bottomRight)
        || (nearestPoint.first == robotBoundary.topRight) || (nearestPoint.first == robotBoundary.topLeft))
    {
        return boost::none;
    }

    if((nearestPoint.second == math::RectSide::left) || (nearestPoint.second == math::RectSide::right))
    {
#ifdef DEBUG_PASSING
        std::cout << "Passing something: " << robotBoundary << " agent: " << agentPosition << " radius: "
            << agent.radius << " dist: " << distToAgent << '\n';
#endif // DEBUG_PASSING

        // Store which side of the agent we passed on using the RectSide. If moving in same direction, then
        // the pass is opposite the robot side. If opposite directions, the agent left is our left too.
        // If we moving the same direction, but our relative speed is higher, then the agent side we pass on is
        // the opposite side of the closest point to the robot boundary
        if(!isOncoming && (relativeSpeed > 0))
        {
            nearestPoint.second = (nearestPoint.second == math::RectSide::left) ? math::RectSide::right
                : math::RectSide::left;
        }

        return passing_object_t(agentPosition, distToAgent, relativeSpeed, nearestPoint.second);
    }

    return boost::none;
}

void assign_ids_to_passing_objects(std::vector<social_forces_t>& forces)
{
    int nextId = 1;

    // Go through each set of forces. If consecutive forces have passing objects, then they need the same id.
    for(std::size_t n = 2; n < forces.size(); ++n)
    {
        // If last was empty and now we have some passing, then we've moved on to the next id
        if(forces[n - 2].passingObj.empty() && forces[n - 1].passingObj.empty() && !forces[n].passingObj.empty())
        {
            ++nextId;
        }

        for(auto& obj : forces[n].passingObj)
        {
            obj.id = nextId;
        }
    }

    std::cout << "INFO: social_forces: Found " << nextId << " passing events.\n";
}

} // namespace mpepc
} // namespace vulcan


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_DISTANCE_SPHERICAL_HPP
#define BOOST_GEOMETRY_STRATEGIES_DISTANCE_SPHERICAL_HPP

#include <boost/geometry/strategies/distance/comparable.hpp>
#include <boost/geometry/strategies/distance/detail.hpp>
#include <boost/geometry/strategies/distance/services.hpp>
#include <boost/geometry/strategies/detail.hpp>

#include <boost/geometry/strategies/normalize.hpp>
#include <boost/geometry/strategies/relate/spherical.hpp>

#include <boost/geometry/strategies/spherical/azimuth.hpp>

#include <boost/geometry/strategies/spherical/distance_cross_track.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track_box_box.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track_point_box.hpp>
#include <boost/geometry/strategies/spherical/distance_haversine.hpp>
#include <boost/geometry/strategies/spherical/distance_segment_box.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace distance
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

// TODO: azimuth and normalize getters would not be needed if distance_segment_box was implemented differently
//       right now it calls disjoint algorithm details.

template <typename RadiusTypeOrSphere, typename CalculationType>
class spherical
    : public strategies::relate::detail::spherical<RadiusTypeOrSphere, CalculationType>
{
    using base_t = strategies::relate::detail::spherical<RadiusTypeOrSphere, CalculationType>;

public:
    spherical() = default;

    template <typename RadiusOrSphere>
    explicit spherical(RadiusOrSphere const& radius_or_sphere)
        : base_t(radius_or_sphere)
    {}

    // azimuth

    static auto azimuth()
    {
        return strategy::azimuth::spherical<CalculationType>();
    }

    // distance

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_pp_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::haversine
                <
                    typename base_t::radius_type, CalculationType
                >(base_t::radius());
    }

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_ps_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::cross_track
            <
                CalculationType,
                strategy::distance::haversine<typename base_t::radius_type, CalculationType>
            >(base_t::radius());
    }

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_pb_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::cross_track_point_box
            <
                CalculationType,
                strategy::distance::haversine<typename base_t::radius_type, CalculationType>
            >(base_t::radius());
    }

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_sb_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::spherical_segment_box
            <
                CalculationType,
                strategy::distance::haversine<typename base_t::radius_type, CalculationType>
            >(base_t::radius());
    }

    template <typename Geometry1, typename Geometry2>
    auto distance(Geometry1 const&, Geometry2 const&,
                  detail::enable_if_bb_t<Geometry1, Geometry2> * = nullptr) const
    {
        return strategy::distance::cross_track_box_box
            <
                CalculationType,
                strategy::distance::haversine<typename base_t::radius_type, CalculationType>
            >(base_t::radius());
    }

    // normalize

    template <typename Geometry>
    static auto normalize(Geometry const&,
                          std::enable_if_t
                            <
                                util::is_point<Geometry>::value
                            > * = nullptr)
    {
        return strategy::normalize::spherical_point();
    }
};

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

template
<
    typename RadiusTypeOrSphere = double,
    typename CalculationType = void
>
class spherical
    : public strategies::distance::detail::spherical<RadiusTypeOrSphere, CalculationType>
{
    using base_t = strategies::distance::detail::spherical<RadiusTypeOrSphere, CalculationType>;

public:
    spherical() = default;

    template <typename RadiusOrSphere>
    explicit spherical(RadiusOrSphere const& radius_or_sphere)
        : base_t(radius_or_sphere)
    {}
};

namespace services
{

template <typename Geometry1, typename Geometry2>
struct default_strategy
    <
        Geometry1, Geometry2,
        spherical_equatorial_tag, spherical_equatorial_tag
    >
{
    using type = strategies::distance::spherical<>;
};

template <typename R, typename CT>
struct strategy_converter<strategy::distance::haversine<R, CT> >
{
    template <typename S>
    static auto get(S const& s)
    {
        return strategies::distance::spherical<R, CT>(s.radius());
    }
};

template <typename CT, typename PPS>
struct strategy_converter<strategy::distance::cross_track<CT, PPS> >
    : strategy_converter<PPS>
{};

template <typename CT, typename PPS>
struct strategy_converter<strategy::distance::cross_track_point_box<CT, PPS> >
    : strategy_converter<PPS>
{};

template <typename CT, typename PPS>
struct strategy_converter<strategy::distance::spherical_segment_box<CT, PPS> >
    : strategy_converter<PPS>
{};

template <typename CT, typename PPS>
struct strategy_converter<strategy::distance::cross_track_box_box<CT, PPS> >
    : strategy_converter<PPS>
{};

template <typename R, typename CT>
struct strategy_converter<strategy::distance::comparable::haversine<R, CT> >
{
    template <typename S>
    static auto get(S const& s)
    {
        return strategies::distance::detail::make_comparable(
                strategies::distance::spherical<R, CT>(s.radius()));
    }
};

template <typename CT, typename PPS>
struct strategy_converter<strategy::distance::comparable::cross_track<CT, PPS> >
    : strategy_converter<PPS>
{};

} // namespace services

}} // namespace strategies::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_DISTANCE_SPHERICAL_HPP


#include <cmath>
#include <string>
#include <sstream>
#include <fstream>
#include <iostream>
#include <algorithm>

//#include <boost/regex.hpp>

#include "utility.hpp"

namespace
{
  bool both_are_spaces (char lhs, char rhs)
  {
    return (lhs == rhs) && (lhs == ' ');
  }
}

bool berthaingen::set_mol12_at_dist (berthaingen::molecule & mol1,
    berthaingen::molecule & mol2, int atom1, int atom2,
    double dist, berthaingen::molecule & mol)
{
  if ((atom1 <= mol1.get_atomsize()) &&
      (atom2 <= mol2.get_atomsize()))
  {
    berthaingen::atom a1 = mol1.get_atom(atom1 - 1);
    berthaingen::atom a2 = mol2.get_atom(atom2 - 1);

    // Maybe a 3D point clss ... is needed
    double len = pow((a1.get_x() - a2.get_x()), 2.0) +
       pow((a1.get_y() - a2.get_y()), 2.0) +
       pow((a1.get_z() - a2.get_z()), 2.0);
    len = sqrt(len);

    double xvers = (a1.get_x() - a2.get_x())/len;
    double yvers = (a1.get_y() - a2.get_y())/len;
    double zvers = (a1.get_z() - a2.get_z())/len;

    mol1.center (a1.get_x(), a1.get_y(), a1.get_z());
    mol2.center (a2.get_x(), a2.get_y(), a2.get_z());

    mol2.center (xvers*dist, yvers*dist, zvers*dist);

    mol = mol1;

    mol.add_fragment(mol2);

    return true;
  }

  return false;
}

bool berthaingen::is_float (const std::string & s)
{
  std::istringstream iss(s);
  float f;

  iss >> std::noskipws >> f;

  return iss.eof() && !iss.fail();
}

bool berthaingen::is_integer(const std::string & s)
{
  if(s.empty() || ((!isdigit(s[0])) &&
        (s[0] != '-') && (s[0] != '+')))
    return false ;

  char * p ;
  strtol(s.c_str(), &p, 10) ;

  return (*p == 0) ;
}

void berthaingen::tokenize (const std::string & str,
    std::vector<std::string> & tokens,
    const std::string & delimiters)
{
  // Skip delimiters at beginning.
  std::string::size_type lastPos =
    str.find_first_not_of(delimiters, 0);
  std::string::size_type pos =
    str.find_first_of(delimiters, lastPos);

  while (std::string::npos != pos ||
      std::string::npos != lastPos)
  {
    tokens.push_back(str.substr(lastPos, pos - lastPos));
    lastPos = str.find_first_not_of(delimiters, pos);
    pos = str.find_first_of(delimiters, lastPos);
  }
}

void berthaingen::multispace_to_single (std::string & str)
{
  std::string::iterator new_end =
    std::unique(str.begin(), str.end(), both_are_spaces);
  str.erase(new_end, str.end());

  //boost::regex_replace(str, boost::regex("[' ']{2,}"), " ");
}

void berthaingen::rtrim(std::string & s, const std::string & delimiters)
{
  s.erase(s.find_last_not_of( delimiters ) + 1 );
}

void berthaingen::ltrim(std::string & s, const std::string & delimiters)
{
  s.erase(0, s.find_first_not_of( delimiters ) );
}

bool berthaingen::split_atom_and_basis (
    const std::vector<std::string> & setfname,
    std::map<berthaingen::ptable::element, std::vector<std::string> > &
    set_map, std::stringstream & errmsg)
{
  std::vector<std::string>::const_iterator iter = setfname.begin();
  for (; iter != setfname.end(); ++iter)
  {
    std::vector<std::string> vctstring;
    berthaingen::tokenize (*iter, vctstring, ":");
    if (vctstring.size() == 2)
    {
      berthaingen::ptable::element e =
        berthaingen::ptable::symbol_to_element(vctstring[0].c_str());

      if (e == berthaingen::ptable::NO_ELEMENT)
      {
        errmsg << "Error in element: " << vctstring[0];
        return false;
      }

      std::vector<std::string> lines;
      std::ifstream fp;
      fp.open(vctstring[1].c_str());
      if (fp.is_open())
      {
        while (!fp.eof())
        {
          std::string line;
          std::getline (fp, line);
          if (line.find_first_not_of("\t\n ") != std::string::npos)
          {
            multispace_to_single (line);
            ltrim(line, " \t");
            rtrim(line, " \t");

            lines.push_back(line);
          }
        }
      }
      fp.close();

      if (lines.size() == 0)
      {
        errmsg << "Error in element " << vctstring[0] <<
          " file " << vctstring[1] << "is empty ";
        return false;
      }

      if (set_map.find(e) == set_map.end())
      {
        set_map[e] = lines;
      }
      else
      {
        errmsg << "Element " << vctstring[0] << " many times present ";
        return false;
      }
    }
    else
    {
      errmsg << "Error in set: " << *iter;
      return false;
    }
  }

  return true;
}