/**
 * 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/>.
 */

// Note: Boost 1.52 and later is required for use of the Boost.Polygon
//   Voronoi Library.  Many thanks to Andrii Sydorchuk for contributing
//   this high-quality Voronoi Diagram library to Boost.
#include <algorithm>
#include <map>
#include <utility>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/multi/geometries/multi_point.hpp>
#include <boost/polygon/voronoi.hpp>
#include <boost/polygon/voronoi_builder.hpp>
#include <boost/polygon/voronoi_diagram.hpp>
#include <wx/stopwatch.h>
#include "GalWeight.h"
#include "../GenUtils.h"
#include "../GenGeomAlgs.h"
#include "../GdaShape.h"
#include "../logger.h"
#include "VoronoiUtils.h"

using namespace boost::polygon;

struct Point {
	int a;
	int b;
	Point (int x, int y) : a(x), b(y) {}
};

struct Segment {
	Point p0;
	Point p1;
	Segment (int x1, int y1, int x2, int y2) : p0(x1, y1), p1(x2, y2) {}
};

namespace boost {
	namespace polygon {

		template <>
		struct geometry_concept<Point> { typedef point_concept type; };

		template <>
		struct point_traits<Point> {
			typedef int coordinate_type;

			static inline coordinate_type get(const Point& point,
											  orientation_2d orient) {
				return (orient == HORIZONTAL) ? point.a : point.b;
			}
		};

		template <>
		struct geometry_concept<Segment> { typedef segment_concept type; };

		template <>
		struct segment_traits<Segment> {
			typedef int coordinate_type;
			typedef Point point_type;

			static inline point_type get(const Segment& segment,
										 direction_1d dir) {
				return dir.to_int() ? segment.p1 : segment.p0;
			}
		};
	}  // polygon
}  // boost

/**
 Note: Input is double centroids, but we then scale up to large integers
 to create the Voronoi diagram.  The output of the Voronoi diagram
 is large doubles.
 */

namespace Gda {
	namespace VoronoiUtils {
		typedef voronoi_builder<int> VB;
		typedef voronoi_diagram<double> VD;

		std::list<int>* getCellList(
							const VD::cell_type& cell,
							std::map<std::pair<int,int>,
							std::list<int>* >& pt_to_id_list,
							std::vector<std::pair<int,int> >& int_pts);
		bool isVertexOutsideBB(const VD::vertex_type& vertex,
							   const double& xmin, const double& ymin,
							   const double& xmax, const double& ymax);
		bool clipEdge(const VD::edge_type& edge,
					  std::vector<std::pair<int,int> >& int_pts,
					  const double& xmin, const double& ymin,
					  const double& xmax, const double& ymax,
					  double& x0, double& y0, double& x1, double& y1);
		bool clipInfiniteEdge(const VD::edge_type& edge,
							  std::vector<std::pair<int,int> >& int_pts,
							  const double& xmin, const double& ymin,
							  const double& xmax, const double& ymax,
							  double& x0, double& y0, double& x1, double& y1);
		bool clipFiniteEdge(const VD::edge_type& edge,
							std::vector<std::pair<int,int> >& int_pts,
							const double& xmin, const double& ymin,
							const double& xmax, const double& ymax,
							double& x0, double& y0, double& x1, double& y1);
	}
}

/** Input: double precision x/y coordinates, indexed by observation record id
 Output: list of list of duplicates
 */
void Gda::VoronoiUtils::FindPointDuplicates(const std::vector<double>& x,
											  const std::vector<double>& y,
										std::list<std::list<int> >& duplicates)
{
	typedef std::pair<int,int> int_pair;
	int num_obs = x.size();
	double x_orig_min=0, x_orig_max=0;
	double y_orig_min=0, y_orig_max=0;
	SampleStatistics::CalcMinMax(x, x_orig_min, x_orig_max);
	SampleStatistics::CalcMinMax(y, y_orig_min, y_orig_max);
	double orig_scale = GenUtils::max<double>(x_orig_max-x_orig_min,
											  y_orig_max-y_orig_min);
	if (orig_scale == 0) orig_scale = 1;
	double big_dbl = 1073741824; // 2^30
	double p = (big_dbl/orig_scale);

	std::map<int, std::list<int> > dups;
	std::map<int, std::list<int> >::iterator dups_iter;
	std::map<int_pair, int> pt_map;
	std::map<int_pair, int>::iterator map_iter;
	std::vector<int> x_int(num_obs);
	std::vector<int> y_int(num_obs);
	bool duplicates_exist = false;
	for (int i=0; i<num_obs; i++) {
		x_int[i] = (int) ((x[i]-x_orig_min)*p);
		y_int[i] = (int) ((y[i]-y_orig_min)*p);
		int_pair key(std::make_pair(x_int[i], y_int[i]));
		map_iter = pt_map.find(key);
		if (map_iter == pt_map.end()) {
			pt_map[key] = i;
		} else {
			duplicates_exist = true;
			int ind1 = map_iter->second;
			int ind2 = i;
			dups_iter = dups.find(ind1);
			if (dups_iter == dups.end()) {
				std::list<int> l;
				l.push_back(ind1);
				l.push_back(ind2);
				dups[ind1] = l;
			} else {
				dups_iter->second.push_back(ind2);
			}
		}
	}
	duplicates.clear();
	if (duplicates_exist) {
		for (dups_iter = dups.begin(); dups_iter != dups.end(); dups_iter++) {
			duplicates.push_back(dups_iter->second);
		}
	}
}

/** If success, returns true. Else, if returns false, then duplicates or
   near duplicates were found and duplicate_ind1 and duplicate_ind2 will
   indicate which two points are near duplicates. */
bool Gda::VoronoiUtils::MakePolygons(const std::vector<double>& x,
									   const std::vector<double>& y,
									   std::vector<GdaShape*>& polys,
									   double& voronoi_bb_xmin,
									   double& voronoi_bb_ymin,
									   double& voronoi_bb_xmax,
									   double& voronoi_bb_ymax)
{
	LOG_MSG("Entering Gda::VoronoiUtils::MakePolygons");
	using namespace boost::polygon;
	typedef std::pair<int,int> int_pair;

	int num_obs = x.size();
	polys.clear();
	polys.resize(num_obs);
	double x_orig_min=0, x_orig_max=0;
	double y_orig_min=0, y_orig_max=0;
	SampleStatistics::CalcMinMax(x, x_orig_min, x_orig_max);
	SampleStatistics::CalcMinMax(y, y_orig_min, y_orig_max);
	double orig_scale = GenUtils::max<double>(x_orig_max-x_orig_min,
											  y_orig_max-y_orig_min);
	if (orig_scale == 0) orig_scale = 1;
	double big_dbl = 1073741824; // 2^30
	double p = (big_dbl/orig_scale);

	std::map<int, std::list<int> > dups;
	std::map<int, std::list<int> >::iterator dups_iter;
	std::map<int_pair, int> pt_map;
	std::map<int_pair, int>::iterator map_iter;
	std::vector<int> x_int(num_obs);
	std::vector<int> y_int(num_obs);
	bool duplicates_exist = false;
	std::list<std::list<int> > duplicates;
	for (int i=0; i<num_obs; i++) {
		x_int[i] = (int) ((x[i]-x_orig_min)*p);
		y_int[i] = (int) ((y[i]-y_orig_min)*p);
		int_pair key(std::make_pair(x_int[i], y_int[i]));
		map_iter = pt_map.find(key);
		if (map_iter == pt_map.end()) {
			pt_map[key] = i;
		} else {
			duplicates_exist = true;
			int ind1 = map_iter->second;
			int ind2 = i;
			dups_iter = dups.find(ind1);
			if (dups_iter == dups.end()) {
				std::list<int> l;
				l.push_back(ind1);
				l.push_back(ind2);
				dups[ind1] = l;
			} else {
				dups_iter->second.push_back(ind2);
			}
		}
	}
	if (duplicates_exist) {
		for (dups_iter = dups.begin(); dups_iter != dups.end(); dups_iter++) {
			duplicates.push_back(dups_iter->second);
		}
	}

	VD vd;
	wxStopWatch sw_vd;
	VB vb;
	std::vector<int_pair> int_pts(num_obs);
	for (int i=0; i<num_obs; i++) {
		int_pts[i].first = x_int[i];
		int_pts[i].second = y_int[i];
	}
	for (int i=0; i<num_obs; i++) {
		int index = vb.insert_point(x_int[i], y_int[i]);
	}
	vb.construct(&vd);
	LOG_MSG(wxString::Format("Voronoi diagram construction on %d points "
							 "took %ld ms", num_obs, sw_vd.Time()));

	// Add 2% offset to the bounding rectangle
	const double bb_pad = 0.02;
	// note data has been translated to origin and scaled
	double bbox_xmin = -bb_pad*big_dbl;
	double bbox_xmax = (x_orig_max-x_orig_min)*p + bb_pad*big_dbl;
	double bbox_ymin = -bb_pad*big_dbl;
	double bbox_ymax = (y_orig_max-y_orig_min)*p + bb_pad*big_dbl;

	voronoi_bb_xmin = (bbox_xmin / p) + x_orig_min;
	voronoi_bb_xmax = (bbox_xmax / p) + x_orig_min;
	voronoi_bb_ymin = (bbox_ymin / p) + y_orig_min;
	voronoi_bb_ymax = (bbox_ymax / p) + y_orig_min;

	wxStopWatch sw_vd_processing;
	int cell_cnt = 0;
	int max_pts = 1000;
	wxRealPoint* pts = new wxRealPoint[max_pts];
	for (VD::const_cell_iterator it = vd.cells().begin();
		 it != vd.cells().end(); ++it) {
		bool boundary_cell = false;
		int edge_cnt = 0;
		const VD::cell_type &cell = *it;

		int_pair key = std::make_pair(x_int[cell.source_index()],
									  y_int[cell.source_index()]);
		int ind = pt_map.find(key)->second;

		const VD::edge_type *edge = cell.incident_edge();
		double x_init = 0;
		double y_init = 0;
		do {
			if (!edge->is_finite() || !edge->is_primary()) {
				//LOG_MSG("cell intersects boundary");
				boundary_cell = true;
				break;
			}
			// The following ensures that the same edge is always clipped.
			// This ensurues that adjacent polygons have the exact same
			// shared-edge descriptions.
			double edge_x0, edge_y0, edge_x1, edge_y1;
			bool intersects_e = false;
			if (edge < edge->twin()) {
				intersects_e = clipEdge(*edge, int_pts, bbox_xmin, bbox_ymin,
										bbox_xmax, bbox_ymax,
										edge_x0, edge_y0, edge_x1, edge_y1);
			} else {
				intersects_e = clipEdge(*edge->twin(), int_pts, bbox_xmin,
										bbox_ymin, bbox_xmax, bbox_ymax,
										edge_x0, edge_y0, edge_x1, edge_y1);
			}
			if (!intersects_e) {
				boundary_cell = true;
				break;
			}
			if (edge_cnt-1 > max_pts) {
				delete [] pts;
				max_pts = 2*edge_cnt;
				pts = new wxRealPoint[max_pts];
			}
			double x0 = (edge_x0 / p) + x_orig_min;
			double y0 = (edge_y0 / p) + y_orig_min;
			double x1 = (edge_x1 / p) + x_orig_min;
			double y1 = (edge_y1 / p) + y_orig_min;
			if (edge_cnt == 0) {
				x_init == x0;
				y_init == y0;
			}
			//wxString msg;
			//msg << "edge: (" << x0 << ", " << y0 << ") -> (";
			//msg << x1 << ", " << y1 << ")";
			//LOG_MSG(msg);
			pts[edge_cnt].x = x0;
			pts[edge_cnt].y = y0;
			edge_cnt++;
			edge = edge->next();
			if (edge == cell.incident_edge()) {
				if (x1 == x_init && y1 == y_init) {
					pts[edge_cnt].x = x1;
					pts[edge_cnt].y = y1;
					edge_cnt++;
				} else {
					boundary_cell = true;
					break;
				}
			}
		} while (edge != cell.incident_edge());

		if (!boundary_cell) {
			polys[ind] = new GdaPolygon(edge_cnt, pts);
			//wxString msg;
			//msg << "added non-boundary polygon with ";
			//msg << edge_cnt-1 << " edges";
			//LOG_MSG(msg);
		} else {
			// boundary cell, need to determine clipped polygon
			edge = cell.incident_edge();
			edge_cnt = 0;

			using boost::geometry::model::d2::point_xy;
			using boost::geometry::append;
			using boost::geometry::make;
			boost::geometry::model::multi_point<point_xy<double> > h_pts;
			typedef boost::geometry::model::polygon<point_xy<double> > my_polygon;
			typedef boost::geometry::ring_type<my_polygon>::type ring_type;
			my_polygon hull;

			do {
				// The following ensures that the same edge is always clipped.
				// This ensurues that adjacent polygons have the exact same
				// shared-edge descriptions.
				double edge_x0, edge_y0, edge_x1, edge_y1;
				bool intersects_e = false;
				if (edge < edge->twin()) {
					intersects_e = clipEdge(*edge, int_pts,
											bbox_xmin, bbox_ymin,
											bbox_xmax, bbox_ymax,
											edge_x0, edge_y0, edge_x1, edge_y1);
				} else {
					intersects_e = clipEdge(*edge->twin(), int_pts,
											bbox_xmin, bbox_ymin,
											bbox_xmax, bbox_ymax,
											edge_x0, edge_y0, edge_x1, edge_y1);
				}
				if (intersects_e) {
					double x0 = (edge_x0 / p) + x_orig_min;
					double y0 = (edge_y0 / p) + y_orig_min;
					append(h_pts, make<point_xy<double> >(x0, y0));
					double x1 = (edge_x1 / p) + x_orig_min;
					double y1 = (edge_y1 / p) + y_orig_min;
					append(h_pts, make<point_xy<double> >(x1, y1));
				}
				edge_cnt++;
				edge = edge->next();
			} while (edge != cell.incident_edge());

			// make sure that the cell's internal point is also within the
			// convex hull.
			{
				double x0 =
					(((double) x_int[cell.source_index()]) / p) + x_orig_min;
				double y0 =
					(((double) y_int[cell.source_index()]) / p) + y_orig_min;
				append(h_pts, make<point_xy<double> >(x0, y0));
			}

			boost::geometry::convex_hull(h_pts, hull);

				ring_type outer_ring = hull.outer();
			int pts_cnt = 0;
			for (ring_type::iterator it=outer_ring.begin();
				 it != outer_ring.end(); it++) {
				double x = boost::geometry::get<0>(*it);
				double y = boost::geometry::get<1>(*it);
				pts[pts_cnt].x = x;
				pts[pts_cnt].y = y;
				pts_cnt++;
			}
			polys[ind] = new GdaPolygon(pts_cnt, pts);
		}
	}

	if (duplicates_exist) {
		// Must fill in missing entries in poly vector with copies
		// of duplicate shapes.
		for (dups_iter = dups.begin();
			 dups_iter != dups.end(); dups_iter++) {
			int head_id = dups_iter->first;
			for (std::list<int>::iterator iter=dups_iter->second.begin();
				 iter != dups_iter->second.end(); iter++) {
				if (*iter == head_id) continue;
				polys[*iter] = new GdaPolygon(*(GdaPolygon*)polys[head_id]);
			}
		}
	}

	if (pts) delete [] pts;

	//LOG_MSG(wxString::Format("Voronoi diagram processing on %d points "
	//						 "took %ld ms", num_obs, sw_vd_processing.Time()));
	//LOG_MSG(wxString::Format("#obs: %d, #voronoi cells: %d", num_obs,
	//						 polys.size()));

	LOG_MSG("Exiting Gda::VoronoiUtils::MakePolygons");
	return true;
}

std::list<int>* Gda::VoronoiUtils::getCellList(
				const VD::cell_type& cell,
				std::map<std::pair<int,int>, std::list<int>* >& pt_to_id_list,
				std::vector<std::pair<int,int> >& int_pts)
{
	std::map<std::pair<int,int>, std::list<int>* >::iterator iter;
	//iter = pt_to_id_list.find(std::make_pair(cell.point0().x(),
	//										 cell.point0().y()));
	iter = pt_to_id_list.find(int_pts[cell.source_index()]);
	if (iter == pt_to_id_list.end()) {
		return 0;
	}
	return iter->second;
}

bool Gda::VoronoiUtils::isVertexOutsideBB(const VD::vertex_type& vertex,
											const double& xmin,
											const double& ymin,
											const double& xmax,
											const double& ymax)
{
	double x = vertex.x();
	double y = vertex.y();
	return (x < xmin || x > xmax || y < ymin || y > ymax);
}

/** Clip both infinite and finite edges to bounding rectangle.
 return true if intersection or if edge is contained within bounding box,
 otherwise return false */
bool Gda::VoronoiUtils::clipEdge(const VD::edge_type& edge,
								   std::vector<std::pair<int,int> >& int_pts,
								   const double& xmin, const double& ymin,
								   const double& xmax, const double& ymax,
								   double& x0, double& y0,
								   double& x1, double& y1)
{
	if (edge.is_finite()) {
		return clipFiniteEdge(edge, int_pts, xmin, ymin, xmax, ymax,
							  x0, y0, x1, y1);
	} else {
		return clipInfiniteEdge(edge, int_pts, xmin, ymin, xmax, ymax,
								x0, y0, x1, y1);
	}
}

/** Clip infinite edge to bounding rectangle */
bool Gda::VoronoiUtils::clipInfiniteEdge(const VD::edge_type& edge,
									std::vector<std::pair<int,int> >& int_pts,
									const double& xmin, const double& ymin,
									const double& xmax, const double& ymax,
									double& x0, double& y0,
									double& x1, double& y1)
{
    const VD::cell_type& cell1 = *edge.cell();
    const VD::cell_type& cell2 = *edge.twin()->cell();
    double origin_x, origin_y, direction_x, direction_y;
    // Infinite edges could not be created by two segment sites.
    if (cell1.contains_point() && cell2.contains_point()) {
		double p1_x = (double) int_pts[cell1.source_index()].first;
		double p1_y = (double) int_pts[cell1.source_index()].second;
		double p2_x = (double) int_pts[cell2.source_index()].first;
		double p2_y = (double) int_pts[cell2.source_index()].second;
		origin_x = ((p1_x + p2_x) * 0.5);
		origin_y = ((p1_y + p2_y) * 0.5);
		direction_x = (p1_y - p2_y);
		direction_y = (p2_x - p1_x);
    } else {
		// This case should never happen for point maps.
		LOG_MSG("Warning! one clipInfiniteEdge cells contains a segment!");
		return false;
    }
    double side = xmax - xmin;
    double koef =
		side / (std::max)(fabs(direction_x), fabs(direction_y));
    if (edge.vertex0() == NULL) {
		x0 = origin_x - direction_x * koef;
		y0 = origin_y - direction_y * koef;
    } else {
		x0 = edge.vertex0()->x();
		y0 = edge.vertex0()->y();
    }
    if (edge.vertex1() == NULL) {
		x1 = origin_x + direction_x * koef;
		y1 = origin_y + direction_y * koef;
    } else {
		x1 = edge.vertex1()->x();
		y1 = edge.vertex1()->y();
    }
	return GenGeomAlgs::ClipToBB(x0, y0, x1, y1, xmin, ymin, xmax, ymax);
}

/** Clip finite edge to bounding rectangle */
bool Gda::VoronoiUtils::clipFiniteEdge(const VD::edge_type& edge,
									std::vector<std::pair<int,int> >& int_pts,
									const double& xmin, const double& ymin,
									const double& xmax, const double& ymax,
									double& x0, double& y0,
									double& x1, double& y1)
{
	// we know that edge is finite, so both vertex0 and vertex1 are defined
	x0 = edge.vertex0()->x();
	y0 = edge.vertex0()->y();
	x1 = edge.vertex1()->x();
	y1 = edge.vertex1()->y();
	return GenGeomAlgs::ClipToBB(x0, y0, x1, y1, xmin, ymin, xmax, ymax);
}

/** If false returned, then an unexpected error.  Otherwise, neighbor map
 created successfully.  The presence of duplicates is indicated in
 duplicates_exists and the list of duplicates is filled in.
 */
bool Gda::VoronoiUtils::PointsToContiguity(const std::vector<double>& x,
									const std::vector<double>& y,
									bool queen,
									std::vector<std::set<int> >& nbr_map)
{
	LOG_MSG("Entering Gda::VoronoiUtils::PointsToContiguity");
	typedef std::pair<int,int> int_pair;
	typedef std::list<int> id_list;

	int num_obs = x.size();
	double x_orig_min=0, x_orig_max=0;
	double y_orig_min=0, y_orig_max=0;
	SampleStatistics::CalcMinMax(x, x_orig_min, x_orig_max);
	SampleStatistics::CalcMinMax(y, y_orig_min, y_orig_max);
	double orig_scale = GenUtils::max<double>(x_orig_max-x_orig_min,
											  y_orig_max-y_orig_min);
	if (orig_scale == 0) orig_scale = 1;
	double big_dbl = 1073741824; // 2^30
	double p = (big_dbl/orig_scale);

	// Add 2% offset to the bounding rectangle
	const double bb_pad = 0.02;
	// note data has been translated to origin and scaled
	double bb_xmin = -bb_pad*big_dbl;
	double bb_xmax = (x_orig_max-x_orig_min)*p + bb_pad*big_dbl;
	double bb_ymin = -bb_pad*big_dbl;
	double bb_ymax = (y_orig_max-y_orig_min)*p + bb_pad*big_dbl;

	std::map<int_pair, id_list* > pt_to_id_list;
	// for each unique point, the list of cells at that point
	std::map<int_pair, id_list* >::iterator pt_to_id_list_iter;

	std::map<int_pair, std::set<id_list* > > pt_to_nbr_sets;
	// for each unique point, the set of lists of points that are neighbors
	std::map<int_pair, std::set<id_list* > >::iterator pt_to_nbr_sets_iter;

	std::vector<int_pair> int_pts(num_obs);
	for (int i=0; i<num_obs; i++) {
		int_pts[i].first = (int) ((x[i]-x_orig_min)*p);
		int_pts[i].second = (int) ((y[i]-y_orig_min)*p);
	}

	for (int i=0; i<num_obs; i++) {
		pt_to_id_list_iter = pt_to_id_list.find(int_pts[i]);
		if (pt_to_id_list_iter == pt_to_id_list.end()) {
			pt_to_id_list[int_pts[i]] = new id_list;
		}
		pt_to_id_list[int_pts[i]]->push_back(i);
	}

	nbr_map.clear();
	nbr_map.resize(num_obs);

	VD vd;
	wxStopWatch sw_vd;
	VB vb;
	for (int i=0; i<num_obs; i++) {
		int index = vb.insert_point(int_pts[i].first, int_pts[i].second);
	}
	vb.construct(&vd);

	wxStopWatch sw_vd_processing;
	for (VD::const_cell_iterator it = vd.cells().begin();
		 it != vd.cells().end(); ++it) {
		const VD::cell_type &cell = *it;

		int_pair key = int_pts[cell.source_index()];

		std::set<id_list* >& nbr_set = pt_to_nbr_sets[key];

		const VD::edge_type* edge = cell.incident_edge();
		typedef std::list<const VD::vertex_type*> v_list;
		v_list verts;
		do {
			id_list* nbr_list = getCellList(*(edge->twin()->cell()),
											pt_to_id_list, int_pts);
			if (!nbr_list) {
				return false;
			}

			double x0, y0, x1, y1;
			if (clipEdge(*edge, int_pts,
						 bb_xmin, bb_ymin, bb_xmax, bb_ymax,
						 x0, y0, x1, y1)) {
				nbr_set.insert(nbr_list);
			}

			if (queen) { // add all cells that share each edge vertex
				if (edge->vertex0() &&
					!isVertexOutsideBB(*edge->vertex0(), bb_xmin, bb_ymin,
									   bb_xmax, bb_ymax)) {
						verts.push_back(edge->vertex0());
					}
				if (edge->vertex1() &&
					!isVertexOutsideBB(*edge->vertex1(), bb_xmin, bb_ymin,
									   bb_xmax, bb_ymax)) {
					verts.push_back(edge->vertex1());
				}
			}
			edge = edge->next();
		} while (edge != cell.incident_edge());

		// add all neighboring cells.  List will be empty if !queen
		for (v_list::iterator it = verts.begin(); it != verts.end(); it++) {
			const VD::edge_type *edge =
				(*it)->incident_edge();
			do {
				id_list* nbr_list = getCellList(*(edge->cell()),
												 pt_to_id_list, int_pts);
				if (!nbr_list) {
					return false;
				}
				nbr_set.insert(nbr_list);

				edge = edge->rot_next();
			} while (edge != (*it)->incident_edge());
		}
	}

	for (int i=0; i<num_obs; i++) {
		std::set<id_list* >& nbr_set = pt_to_nbr_sets[int_pts[i]];
		//LOG_MSG(wxString::Format("obs %d has %d neighbors",
		//        i, nbr_set.size()));

		for (std::set<id_list* >::iterator set_iter = nbr_set.begin();
			 set_iter != nbr_set.end(); set_iter++) {
			for (id_list::iterator list_iter = (*set_iter)->begin();
				 list_iter != (*set_iter)->end(); list_iter++) {
				if (*list_iter != i) nbr_map[i].insert(*list_iter);
			}
		}
		// for each set of points, make every point a neighbor of the
		// other points in the set, excluding self
		id_list* pl = pt_to_id_list[int_pts[i]];
		for (id_list::iterator it=pl->begin(); it!=pl->end(); it++) {
			if (*it != i) nbr_map[i].insert(*it);
		}
	}

	for (pt_to_id_list_iter = pt_to_id_list.begin();
		 pt_to_id_list_iter != pt_to_id_list.end();
		 pt_to_id_list_iter++) {
		delete pt_to_id_list_iter->second;
	}

	LOG_MSG(wxString::Format("Voronoi diagram processing on %d points "
							 "took %ld ms", num_obs, sw_vd_processing.Time()));

	LOG_MSG("Exiting Gda::VoronoiUtils::PointsToContiguity");
	return true;
}

GalElement* Gda::VoronoiUtils::NeighborMapToGal(
										std::vector<std::set<int> >& nbr_map)
{
	if (nbr_map.size() == 0) return 0;
	GalElement* gal = new GalElement[nbr_map.size()];
	if (!gal) return 0;
	for (int i=0, iend=nbr_map.size(); i<iend; i++) {
		gal[i].SetSizeNbrs(nbr_map[i].size());
		long cnt = 0;
		for (std::set<int>::iterator it=nbr_map[i].begin();
			 it != nbr_map[i].end(); it++) {
			gal[i].SetNbr(cnt++, *it);
		}
	}
	return gal;
}


#include "FiniteElementMesh.h"

#include <Eigen/Dense>
#include <random>

template<class T>
struct LatticeMesh : public FiniteElementMesh<T>
{
    using Base = FiniteElementMesh<T>;

    // from AnimatedMesh
    using Base::m_meshElements;
    using Base::m_particleX;
    using Base::initializeUSD;
    using Base::initializeTopology;
    using Base::initializeParticles;
    using Vector2 = typename Base::Vector2;

    // from FiniteElementMesh
    using Base::initializeUndeformedConfiguration;
    using Base::m_stepEndTime;

    std::array<int, 2> m_cellSize; // dimensions in grid cells
    T m_gridDX;

    const int m_pinchRadius;

    std::vector<Vector2> m_particleUndeformedX;
    std::vector<int> m_leftHandleIndices;
    std::vector<int> m_rightHandleIndices;
    Vector2 m_leftHandleDisplacement;
    Vector2 m_rightHandleDisplacement;

    LatticeMesh()
        :Base(1.e2, 1., 4., .05), m_pinchRadius(1)
    {
        m_leftHandleDisplacement  = Vector2(-.5,  1.);
        m_rightHandleDisplacement = Vector2( .5, -1.);
    }

    void initialize()
    {
        initializeUSD("MembraneStretch3.usda");

        // Create a Cartesian lattice topology
        for(int cell_i = 0; cell_i < m_cellSize[0]; cell_i++)
        for(int cell_j = 0; cell_j < m_cellSize[1]; cell_j++){
            m_meshElements.emplace_back(
                std::array<int, 3>{
                    gridToParticleID(cell_i  , cell_j  ),
                    gridToParticleID(cell_i+1, cell_j  ),
                    gridToParticleID(cell_i+1, cell_j+1)
                }
            );
            m_meshElements.emplace_back(
                std::array<int, 3>{
                    gridToParticleID(cell_i  , cell_j  ),
                    gridToParticleID(cell_i+1, cell_j+1),
                    gridToParticleID(cell_i  , cell_j+1)
                }
            );
        }
        initializeTopology();

        // Also initialize the associated particles
        for(int node_i = 0; node_i <= m_cellSize[0]; node_i++)
        for(int node_j = 0; node_j <= m_cellSize[1]; node_j++)
            m_particleX.emplace_back(m_gridDX * (T)node_i, m_gridDX * (T)node_j);
        initializeParticles();

        // Check particle indexing in mesh
        for(const auto& element: m_meshElements)
            for(const auto vertex: element)
                if(vertex < 0 || vertex >= m_particleX.size())
                    throw std::logic_error("mismatch between mesh vertex and particle array");

        // Initialize rest shape matrices and particle mass
        initializeUndeformedConfiguration();

        // Also record rest shape
        m_particleUndeformedX = m_particleX;

        // Identify particles on left and right handles
        for(int node_j = 0; node_j <= m_cellSize[1]; node_j++){
            m_leftHandleIndices.push_back(gridToParticleID(0, node_j));
            m_rightHandleIndices.push_back(gridToParticleID(m_cellSize[0], node_j));
        }
    }

    void applyPerturbation()
    {
        std::random_device rd;  //Will be used to obtain a seed for the random number engine
        std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd()
        std::uniform_real_distribution<> dis(-1.0, +1.0);

        for (auto& X : m_particleX)
            for (int d = 0; d < 2; d++)
                X[d] += dis(gen);
    }

    void clearConstrainedParticles(std::vector<Vector2>& x) override
    {
        for(const auto v: m_leftHandleIndices)
            x[v] = Vector2::Zero();
        for(const auto v: m_rightHandleIndices)
            x[v] = Vector2::Zero();
    }

    void setBoundaryConditions() override
    {
        T effectiveTime = std::min<T>(m_stepEndTime, 1.0);

        for(const auto v: m_leftHandleIndices){
            m_particleX[v] = m_particleUndeformedX[v] + effectiveTime * m_leftHandleDisplacement;
        }
        for(const auto v: m_rightHandleIndices){
            m_particleX[v] = m_particleUndeformedX[v] + effectiveTime * m_rightHandleDisplacement;
        }
    }

private:
    inline int gridToParticleID(const int i, const int j) const { return i * (m_cellSize[1]+1) + j; }
};

int main(int argc, char *argv[])
{
    LatticeMesh<float> simulationMesh;
    simulationMesh.m_cellSize = { 40, 40 };
    simulationMesh.m_gridDX = 0.05;
    simulationMesh.m_nFrames = 30;
    simulationMesh.m_subSteps = 1;
    simulationMesh.m_frameDt = 1.0;

    // Initialize the simulation example
    simulationMesh.initialize();

    // Output the initial shape of the mesh
    simulationMesh.writeFrame(0);

    // Perform the animation, output results at each frame
    for(int frame = 1; frame <= simulationMesh.m_nFrames; frame++){
        simulationMesh.simulateFrame(frame);
        if (frame == 7)
            simulationMesh.applyPerturbation();
        simulationMesh.writeFrame(frame);
    }

    // Write the entire timeline to USD
    simulationMesh.writeUSD();

    return 0;
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

class EdgeProjectXYZRGBDPoseOnly: public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point): _point(point) {}

    virtual void computeError(){
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> (_vertices[0]);
        _error = _measurement - pose->estimate().map(_point);
    }

    virtual void linearizeOplus(){
        g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> (_vertices[0]);
        g2o::SE3Quat T(pose->estimate());
        Eigen::Vector3d xyz_trans = T.map(_point);
        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];

        _jacobianOplusXi(0, 0) = 0;
        _jacobianOplusXi(0, 0) = -z;
        _jacobianOplusXi(0, 0) = y;
        _jacobianOplusXi(0, 0) = -1;
        _jacobianOplusXi(0, 0) = 0;
        _jacobianOplusXi(0, 0) = 0;

        _jacobianOplusXi(0, 0) = z;
        _jacobianOplusXi(0, 0) = 0;
        _jacobianOplusXi(0, 0) = -x;
        _jacobianOplusXi(0, 0) = 0;
        _jacobianOplusXi(0, 0) = -1;
        _jacobianOplusXi(0, 0) = 0;

        _jacobianOplusXi(0, 0) = -y;
        _jacobianOplusXi(0, 0) = x;
        _jacobianOplusXi(0, 0) = 0;
        _jacobianOplusXi(0, 0) = 0;
        _jacobianOplusXi(0, 0) = 0;
        _jacobianOplusXi(0, 0) = -1;
    }

    bool read(istream &in) {}
    bool write(ostream &out) const {}
    protected:
    Eigen::Vector3d _point;
};

void feature_matching(const Mat& img1, const Mat& img2,
    std::vector<KeyPoint>& keypoints1,
    std::vector<KeyPoint>& keypoints2,
    std::vector< DMatch >& matches)
    {
        Mat descriptors_1, descriptors_2;
        Ptr<FeatureDetector> detector = ORB::create();
        Ptr<DescriptorExtractor> descriptor = ORB::create();

        // compute FAST corners
        detector->detect(img1, keypoints1);
        detector->detect(img2, keypoints2);

        // compute BRIEF descriptor
        descriptor->compute(img1, keypoints1, descriptors_1);
        descriptor->compute(img2, keypoints2, descriptors_2);

        Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create(4);
        /*
        FLANNBASED = 1,
        BRUTEFORCE = 2,
        BRUTEFORCE_L1 = 3,
        BRUTEFORCE_HAMMING = 4,
        BRUTEFORCE_HAMMINGLUT = 5,
        BRUTEFORCE_SL2 = 6 */

        vector<DMatch> allmatches;
        matcher->match(descriptors_1, descriptors_2, allmatches);

        // Filter all match points
        // Find maximum and minimum distance
        double min_dist = 1000000, max_dist = 0;
        for ( int i = 0; i < descriptors_1.rows; i++ )
        {
            double dist = allmatches[i].distance;
            if ( dist < min_dist ) min_dist = dist;
            if ( dist > max_dist ) max_dist = dist;
        }

        cout << "max distance: " << max_dist << endl;
        cout << "min distance: " << min_dist << endl;

        for ( int i = 0; i < descriptors_1.rows; i++ )
        {
            if ( allmatches[i].distance <= max ( 2*min_dist, 30.0 ) )
            {
                matches.push_back ( allmatches[i] );
            }
        }
    }

void pose_estimation_3d3d(const vector<Point3f> &pts1,
    const vector<Point3f> &pts2, Mat &R, Mat &t){
        Point3f p1, p2; // Center of mass to reduce variance
        int N = pts1.size();
        for(int i = 0;i<N;i++){
            p1 += pts1[i];
            p2 += pts2[i];
        }
        p1 /= N;
        p2 /= N;

        vector<Point3f> q1(N), q2(N);
        for(int i = 0;i<N;i++){
            q1[i] = pts1[i] - p1;
            q2[i] = pts2[i] - p2;
        }

        Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
        for(int i = 0;i<N;i++){
            W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
        }

        cout << "W " << W << endl;

        // SVD on W
        Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );

        Eigen::Matrix3d U = svd.matrixU();
        Eigen::Matrix3d V = svd.matrixV();

        cout << "Matrix U: " << U << endl;
        cout << "Matrix V: " << V << endl;

        Eigen::Matrix3d R_ = U * (V.transpose());
        Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);
        R = (Mat_<double>(3,3) <<
           R_(0, 0), R_(0, 1), R_(0, 2),
           R_(1, 0), R_(1, 1), R_(1, 2),
           R_(2, 0), R_(2, 1), R_(2, 2));
        t = (Mat_<double>(3,1) << t_(0, 0), t_(1, 0), t_(2, 0));
    }

void bundleAdjustment(const vector<Point3f> &pts1,
    const vector<Point3f> &pts2, Mat &R, Mat &t){

        typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3>> Block;

        // Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>();
        Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>();
        Block *solver_ptr = new Block(linearSolver);

        g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

        g2o::SparseOptimizer optimizer;
        optimizer.setAlgorithm( solver );

        // vertex
        g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
        pose->setId(0);
        pose->setEstimate( g2o::SE3Quat(
            Eigen::Matrix3d::Identity(),
            Eigen::Vector3d( 0,0,0 )));

        optimizer.addVertex( pose );

        // Add unary edges
        for(int i = 0;i<pts1.size();i++){
            EdgeProjectXYZRGBDPoseOnly* edge = new EdgeProjectXYZRGBDPoseOnly(
                    Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z) );
            edge->setId(i+1);
            edge->setVertex(0, dynamic_cast<g2o::VertexSE3Expmap*> (pose));          // Dynamic cast
            edge->setMeasurement(Eigen::Vector3d(pts1[i].x, pts1[i].y, pts1[i].z));  // Reprojection error
            edge->setInformation(Eigen::Matrix3d::Identity()*1e4);
            optimizer.addEdge(edge);
        }

        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        optimizer.setVerbose( true );
        optimizer.initializeOptimization();
        optimizer.optimize(10);
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        // Routine
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
        cout<<"optimization costs time: "<<time_used.count()<<" seconds."<<endl;

        cout<<endl<<"after optimization:"<<endl;
        cout<<"T="<<endl<<Eigen::Isometry3d( pose->estimate() ).matrix()<<endl;

    }

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

     if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }

    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    feature_matching ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"Total "<<matches.size() <<" Matching Points" <<endl;

    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );
    Mat d2 = imread ( argv[4], CV_LOAD_IMAGE_UNCHANGED );

    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts1_3d, pts2_3d;
    // vector<Point2f> pts_2d;
    for ( DMatch m:matches )
    {
        ushort depth1 = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        ushort depth2 = d2.ptr<unsigned short> (int ( keypoints_2[m.trainIdx].pt.y )) [ int ( keypoints_2[m.trainIdx].pt.x ) ];
        if ( depth1 == 0 || depth2 == 0)
            continue;
        float dd1 = depth1/5000.0;
        float dd2 = depth2/5000.0;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        Point2d p2 = pixel2cam ( keypoints_2[m.trainIdx].pt, K );
        pts1_3d.push_back ( Point3f ( p1.x*dd1, p1.y*dd1, dd1 ) );  // Find feature point's 3D coordinates from Depth image
        pts2_3d.push_back ( Point3f ( p2.x*dd2, p2.y*dd2, dd2 ) );
    }

    cout<<"3d-2d pairs: "<<pts1_3d.size() <<endl;

    Mat R, t;
    pose_estimation_3d3d ( pts1_3d, pts2_3d, R, t );
    cout<<"ICP via SVD results: "<<endl;
    cout<<"R = "<<R<<endl;
    cout<<"t = "<<t<<endl;
    cout<<"R_inv = "<<R.t() <<endl;
    cout<<"t_inv = "<<-R.t() *t<<endl;

    // Bundle Adjustment

    bundleAdjustment( pts1_3d, pts2_3d, R, t );

    cout<<"ICP via BA results: "<<endl;
    cout<<"R = "<<R<<endl;
    cout<<"t = "<<t<<endl;
    cout<<"R_inv = "<<R.t() <<endl;
    cout<<"t_inv = "<<-R.t() *t<<endl;

    // verify p1 = R*p2 + t
    for ( int i=0; i<5; i++ )
    {
        cout<<"p1 = "<<pts1_3d[i]<<endl;
        cout<<"p2 = "<<pts2_3d[i]<<endl;
        cout<<"(R*p2+t) = "<<
            R * (Mat_<double>(3,1)<<pts2_3d[i].x, pts2_3d[i].y, pts2_3d[i].z) + t
            <<endl;
        cout<<endl;
    }

    return 0;
}


#include "mtao/opengl/Window.h"
#include <iostream>
#include <Eigen/Geometry>
#include "imgui.h"
#include <memory>
#include <algorithm>
#include "mtao/geometry/mesh/boundary_facets.h"
#include "mtao/geometry/mesh/read_obj.hpp"
#include "mtao/geometry/bounding_box.hpp"
#include "mtao/geometry/mesh/write_obj.hpp"
#include "mtao/opengl/drawables.h"
#include <mtao/types.h>
#include <Corrade/Containers/Array.h>
#include <Corrade/Containers/ArrayView.h>
#include <igl/read_triangle_mesh.h>

#include <Magnum/Shaders/VertexColor.h>
#include <Magnum/GL/Renderer.h>
#include <Corrade/Utility/Arguments.h>
#include <mtao/opengl/objects/grid.h>
#include <mtao/geometry/grid/grid.h>
#include <mtao/geometry/grid/staggered_grid.hpp>
#include <mtao/geometry/prune_vertices.hpp>
#include <mandoline/construction/construct_imgui.hpp>
#include <mandoline/construction/remesh_self_intersections.hpp>
#include <mandoline/construction/preprocess_mesh.hpp>
#include <mandoline/mesh3.hpp>
#include <mandoline/tools/cutmesh_info.hpp>
#include "validation/cutmesh_validation.hpp"
#include <spdlog/spdlog.h>

using namespace mtao::opengl;

class MeshViewer : public mtao::opengl::Window3 {
  public:
    enum class Mode : int { Smoothing,
                            LSReinitialization };
    Mode mode = Mode::LSReinitialization;

    char output_filename[128] = "output.cutmesh";
    std::string_view output_view;
    float permeability = 100.0;
    float timestep = 1000.0;
    bool animate = false;
    float bbox_offset = .1;
    using Vec = mtao::VectorX<GLfloat>;
    using Vec3 = mtao::Vec3f;
    Vec data;
    Vec data_original;
    Vec dx;
    Vec dy;
    Vec signs;
    Eigen::AlignedBox<float, 3> bbox, orig_bbox;
    Eigen::SparseMatrix<float> L;
    bool use_cube = false;
    int adaptive_level = 0;
    mtao::ColVecs3d V;
    mtao::ColVecs3i F;
    std::vector<std::array<int, 2>> edges;
    std::map<std::array<int, 2>, std::vector<std::array<int, 2>>> mapped_edges;
    std::optional<std::array<int, 2>> edge_choice = {};

    std::optional<mandoline::construction::CutmeshGenerator_Imgui> constructor;
    std::optional<mandoline::CutCellMesh<3>> ccm;

    MeshViewer(const Arguments &args) : Window3(args), output_view(output_filename, mtao::types::container_size(output_filename)) {
        Corrade::Utility::Arguments myargs;
        myargs.addArgument("filename").parse(args.argc, args.argv);
        std::string filename = myargs.value("filename");
        Eigen::MatrixXd VV;
        Eigen::MatrixXi FF;
        igl::read_triangle_mesh(filename, VV, FF);

        //std::tie(V,F) = mtao::geometry::mesh::read_objD(filename);
        V = VV.transpose();
        F = FF.transpose();
        std::cout << "V/E/F " << V.cols() << "/" << mtao::geometry::mesh::boundary_facets(F).cols() << "/" << F.cols() << std::endl;
        mesh.setTriangleBuffer(V.cast<float>(), F.cast<unsigned int>());
        orig_bbox = bbox = mtao::geometry::bounding_box(V.cast<float>().eval());
        mtao::Vec3f trans_e = -((bbox.min() + bbox.max()) / 2).cast<float>();
        Magnum::Math::Vector3<float> trans(trans_e.x(), trans_e.y(), trans_e.z());

        std::tie(V, F) = mtao::geometry::prune(V, F, 0);
        std::tie(V, F) = mandoline::construction::preprocess_mesh(V, F);

        constructor = mandoline::construction::CutmeshGenerator_Imgui::create(V, F);

        mesh.translate(trans);
        edge_mesh.translate(trans);
        vertex_mesh.translate(trans);
        grid.translate(trans);
        float s = 1. / bbox.sizes().maxCoeff();
        mesh.scale(Magnum::Math::Vector3<float>(s));
        edge_mesh.scale(Magnum::Math::Vector3<float>(s));
        vertex_mesh.scale(Magnum::Math::Vector3<float>(s));
        grid.scale(Magnum::Math::Vector3<float>(s));
        mv_drawable = new mtao::opengl::Drawable<Magnum::Shaders::MeshVisualizer>{ mesh, _wireframe_shader, drawables() };

        edge_drawable = new mtao::opengl::Drawable<Magnum::Shaders::Flat3D>{ grid, _flat_shader, drawables() };
        edge_drawable->activate_triangles({});
        edge_drawable->activate_edges();

        edge_boundary_drawable = new mtao::opengl::Drawable<Magnum::Shaders::Flat3D>{ edge_mesh, _flat_shader, edge_drawables };
        point_drawable = new mtao::opengl::Drawable<Magnum::Shaders::Flat3D>{ vertex_mesh, _flat_shader, edge_drawables };
        edge_boundary_drawable->activate_triangles({});
        edge_boundary_drawable->activate_edges();

        edge_boundary_drawable->data().color = Magnum::Color4(1, 0, 0, 1);

        edge_mesh.setParent(&root());
        vertex_mesh.setVertexBuffer(mtao::Vec3f(1, 1, 1));
        point_drawable->deactivate();
        point_drawable->activate_points();
        vertex_mesh.setParent(&root());
        edge_boundary_drawable->set_visibility(false);
        grid.setParent(&root());

        mesh.setParent(&root());
        update();
    }
    void update() {
        if (constructor) {
            //mtao::geometry::grid::Grid3f g(std::array<int,3>{{NI,NJ,NK}});
            constructor->update_grid();
            constructor->update_vertices(V);
            auto sg = constructor->staggered_grid();
            mtao::geometry::grid::Grid3d g = sg.vertex_grid();
            grid.set(g);
        }
    }
    void update_edges() {
        mtao::ColVecs3f VV = ccm->vertices().cast<float>();
        if (edge_choice) {
            if (auto it = mapped_edges.find(*edge_choice); it != mapped_edges.end()) {
                auto &&edges = it->second;
                if (edges.size() > 0) {
                    auto E = mtao::eigen::stl2eigen(edges);

                    edge_mesh.setEdgeBuffer(E.cast<unsigned int>().eval());
                    std::set<int> Vidx;
                    std::copy(E.data(), E.data() + E.size(), std::inserter(Vidx, Vidx.end()));
                    mtao::ColVecs3f V(3, Vidx.size());
                    for (auto &&[i, j] : mtao::iterator::enumerate(Vidx)) {
                        V.col(i) = VV.col(j).cast<float>();
                    }
                    vertex_mesh.setVertexBuffer(V);

                    edge_boundary_drawable->set_visibility(true);
                    return;
                }
            }
        }
        if (edges.size() > 0) {
            auto E = mtao::eigen::stl2eigen(edges);
            vertex_mesh.setVertexBuffer(VV.cast<float>());

            edge_mesh.setEdgeBuffer(E.cast<unsigned int>().eval());

            edge_boundary_drawable->set_visibility(true);
            return;
        }
        edge_boundary_drawable->set_visibility(false);
    }

    void make_ccm() {
        ccm = constructor->generate();
        if (ccm) {
            spdlog::info("Made CCM!!");
            mandoline::tools::print_region_info(*ccm);

#if defined(MANDOLINE_HANDLE_SELF_INTERSECTIONS)
            auto regions_vec = input_mesh_regions(*ccm);
            std::set<int> regions;
            std::copy(regions_vec.data(), regions_vec.data() + regions_vec.size(), std::inserter(regions, regions.end()));
            std::vector<std::set<std::array<int,3>>> Fs(regions.size());
            for(int i = 0; i < regions_vec.cols(); ++i) {
                auto R = regions_vec.col(i);
                auto f = ccm->origF().col(i);
                for(int j = 0; j < regions_vec.rows(); ++j) {
                    Fs[R(j)].emplace(std::array<int,3>{{f(0),f(1),f(2)}});
                }
            }

            spdlog::info("Input mesh had {} regions", regions.size());
            std::copy(regions.begin(),regions.end(),std::ostream_iterator<int>(std::cout,","));
            std::cout << std::endl;
            for(auto&& [idx,fs]: mtao::iterator::enumerate(Fs)) {
                std::stringstream name;
                name << "objfile" << idx << ".obj";
                mtao::geometry::mesh::write_objD(ccm->origV(),mtao::eigen::stl2eigen(fs), name.str());
            }
#endif
        }
        {// check exterior grid valences
    spdlog::warn("gui side Exterior grid face size: {}", ccm->exterior_grid().num_faces());
            if(!exterior_cell_valence_counts(*ccm)) {
                std::cout << "Bad exterior cell valences!" << std::endl;
            }
        }
        {
            auto g = ccm->Base::vertex_grid();
            auto s = g.shape();
            std::cout << s[0] << ":" << s[1] << ":" << s[2] << std::endl;
        }

        std::vector<mtao::ColVecs3i> Fs;
        mtao::ColVecs3f V = ccm->vertices().cast<float>();
        for (auto &&f : ccm->faces()) {
            if (f.is_mesh_face()) {
                Fs.push_back(f.triangulate_fan());
            }
        }
        if (Fs.size() > 0) {
            auto F = mtao::eigen::hstack_iter(Fs.begin(), Fs.end()).cast<unsigned int>().eval();
            mesh.setTriangleBuffer(V, F);
            edge_mesh.setVertexBuffer(V);

            using E = std::array<int, 2>;
            edges.clear();
            mapped_edges.clear();
            for (int i = 0; i < ccm->cut_edge_size(); ++i) {
                auto e = ccm->cut_edge(i);
                auto va = ccm->masked_vertex(e.indices[0]);
                auto vb = ccm->masked_vertex(e.indices[1]);
                auto mask = va.mask() & vb.mask();
                if (mask.active()) {
                    edges.emplace_back(e.indices);
                    for (int i = 0; i < 3; ++i) {
                        if (mask[i]) {
                            mapped_edges[E{ { i, *mask[i] } }].emplace_back(edges.back());
                        }
                    }
                }
            }
            update_edges();
        }
    }
    void gui() override {

        if (mv_drawable) {
            mv_drawable->gui();
        }
        if (edge_drawable) {
            edge_drawable->gui();
        }
        if (ImGui::InputFloat("Boundingbox Offset", &bbox_offset)) {
            mtao::Vec3f C = (orig_bbox.min() + orig_bbox.max()) / 2;
            mtao::Vec3f s = orig_bbox.sizes() / 2;
            bbox.min() = C - (1 + bbox_offset) * s;
            bbox.max() = C + (1 + bbox_offset) * s;
            update();
        }
        if (ImGui::Button("Reset BBox")) {
            bbox = orig_bbox;
            update();
        }
        if (ImGui::CollapsingHeader("Cutmesh Generation")) {
            if (constructor) {
                if (constructor->gui()) {
                    update();
                }
            }
            if (ImGui::Button("Make CCM")) {
                make_ccm();
            }
        }
        {
            bool active = bool(edge_choice);
            if (ImGui::Checkbox("Edge choice", &active)) {
                std::cout << "Checked!" << std::endl;
                if (active) {
                    edge_choice = std::array<int, 2>{ { 0, 0 } };
                } else {
                    edge_choice.reset();
                    update_edges();
                }
            }
            if (active) {
                bool changed = false;
                changed |= ImGui::InputInt("Axis", edge_choice->begin());
                changed |= ImGui::InputInt("Plane", edge_choice->begin() + 1);
                if (changed) {
                    auto &&[a, b] = *edge_choice;
                    a = std::clamp<int>(a, 0, 2);
                    b = std::clamp<int>(b, 0, ccm->vertex_shape()[a] - 1);
                    update_edges();
                }
            }
        }
        ImGui::InputText("Filename", output_filename, 128);
        if (ImGui::Button("Save")) {
            if (ccm) {
                ccm->write(std::string(output_view));
            }
        }
    }
    void draw() override {
        Magnum::GL::Renderer::setPointSize(10.);
        Magnum::GL::Renderer::disable(Magnum::GL::Renderer::Feature::FaceCulling);
        Window3::draw();
        Magnum::GL::Renderer::disable(Magnum::GL::Renderer::Feature::DepthTest);
        camera().draw(edge_drawables);
        Magnum::GL::Renderer::enable(Magnum::GL::Renderer::Feature::DepthTest);
    }

  private:
    Magnum::Shaders::MeshVisualizer _wireframe_shader{ supportsGeometryShader() ? Magnum::Shaders::MeshVisualizer::Flag::Wireframe : Magnum::Shaders::MeshVisualizer::Flag{} };
    Magnum::Shaders::Flat3D _flat_shader;
    Magnum::SceneGraph::DrawableGroup3D edge_drawables;
    mtao::opengl::objects::Mesh<3> mesh;
    mtao::opengl::objects::Grid<3> grid;
    mtao::opengl::objects::Mesh<3> edge_mesh;
    mtao::opengl::objects::Mesh<3> vertex_mesh;
    mtao::opengl::Drawable<Magnum::Shaders::MeshVisualizer> *mv_drawable = nullptr;
    mtao::opengl::Drawable<Magnum::Shaders::Flat3D> *edge_drawable = nullptr;
    mtao::opengl::Drawable<Magnum::Shaders::Flat3D> *edge_boundary_drawable = nullptr;
    mtao::opengl::Drawable<Magnum::Shaders::Flat3D> *point_drawable = nullptr;
};

MAGNUM_APPLICATION_MAIN(MeshViewer)


///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//                          *** w90.hpp ***                                  //
//                                                                           //
// class to manage methods relating to wannier90 hamiltonian                 //
// templatized over dimension of the mesh                                    //
//                                                                           //
// created November 27, 2017                                                 //
// copyright Christopher N. Singh Binghamton University Physics              //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

#ifndef W90_hpp
#define W90_hpp

#include <cassert>
#include <Eigen/Dense>
#include <fstream>
#include <string>
#include <sstream>
#include <vector>

class W90
{
public:
	W90(std::string hr_file);

	Eigen::MatrixXcd fft(const Eigen::Vector3d& k);

private:
	Eigen::ArrayXXd hr;

	inline std::complex<double> tij(int nn, const Eigen::Vector3d& k);
};

#endif /* W90_hpp */


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef SNPTEST_REGRESSION_INDEPENDENT_INDEPENDENTPARAMETERDISTRIBUTION_HPP
#define SNPTEST_REGRESSION_INDEPENDENT_INDEPENDENTPARAMETERDISTRIBUTION_HPP

#include <memory>
#include <unordered_map>
#include <Eigen/Core>
#include <boost/ptr_container/ptr_vector.hpp>
#include <boost/ptr_container/ptr_map.hpp>
#include "metro/UnivariateLogDensity.hpp"
#include "metro/SmoothFunction.hpp"

namespace metro {
	struct IndependentParameterDistribution: public SmoothFunction {
	public:
		typedef std::auto_ptr< IndependentParameterDistribution > UniquePtr ;
		typedef Eigen::VectorXd Point ;
		typedef Eigen::VectorXd Vector ;
		typedef Eigen::MatrixXd Matrix ;
		typedef Eigen::Matrix< int, Eigen::Dynamic, Eigen::Dynamic > IntegerMatrix ;

		static UniquePtr create( std::vector< std::string > const& parameter_names ) ;

	public:
		IndependentParameterDistribution( std::vector< std::string > const& parameter_names ) ;

		void set_prior( std::string const& name, UnivariateLogDensity::UniquePtr pdf ) ;
		int number_of_parameters() const ;
		Vector const& parameters() const { return m_parameters ; }

		void evaluate_at(
			Point const& parameters,
			int const numberOfDerivatives = 2
		) ;
		void evaluate( int const numberOfDerivatives = 2 ) ;
		double get_value_of_function() const ;
		Vector get_value_of_first_derivative() const ;
		Matrix get_value_of_second_derivative() const ;

		std::string get_summary() const ;

	private:
		std::size_t m_number_of_parameters ;
		std::vector< std::string > const m_parameter_names ;
		typedef std::map< std::string, std::size_t > ParameterNameIndices ;
		ParameterNameIndices m_parameter_name_indices ;
		Vector m_parameters ;
		boost::ptr_vector< UnivariateLogDensity > m_densities ;
	} ;
}

#endif


//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Flight Systems and Automatic Control group,
//       TU Darmstadt, nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================

#ifndef HECTOR_POSE_ESTIMATION_FILTER_EKF_INL
#define HECTOR_POSE_ESTIMATION_FILTER_EKF_INL

#include <hector_pose_estimation/filter/ekf.h>
#include <boost/utility/enable_if.hpp>

#include <ros/console.h>

namespace hector_pose_estimation {
namespace filter {

template <class ConcreteModel, typename Enabled>
bool EKF::Predictor_<ConcreteModel, Enabled>::predict(double dt) {
  this->model_->getExpectedDiff(x_diff, state(), dt);
  this->model_->getStateJacobian(A, state(), dt, this->init_);
  this->model_->getSystemNoise(Q, state(), dt, this->init_);

  ROS_DEBUG_STREAM_NAMED("ekf.prediction", "dt * f(x) = [" << x_diff.transpose() << "]");
  ROS_DEBUG_STREAM_NAMED("ekf.prediction", "dt * Q    = [" << std::endl << Q  << "]");
  ROS_DEBUG_STREAM_NAMED("ekf.prediction", "dt * A    = [" << std::endl << A << "]");

//  state().P0() = A * state().P() * A.transpose() + Q;
//  state().P0().assertSymmetric();
//  state().x0() = x_pred;

  this->init_ = false;
  return true;
}

template <class ConcreteModel, typename Enabled>
bool EKF::Corrector_<ConcreteModel, Enabled>::correct(const typename ConcreteModel::MeasurementVector &y, const typename ConcreteModel::NoiseVariance &R) {
  this->model_->getExpectedValue(y_pred, state());
  this->model_->getStateJacobian(C, state(), this->init_);

  ROS_DEBUG_STREAM_NAMED("ekf.correction", "x_prior  = [" << state().getVector().transpose() << "]");
  ROS_DEBUG_STREAM_NAMED("ekf.correction", "P_prior  = [" << std::endl << state().getCovariance() << "]");
  ROS_DEBUG_STREAM_NAMED("ekf.correction", "y        = [" << y.transpose() << "]");
  ROS_DEBUG_STREAM_NAMED("ekf.correction", "R        = [" << std::endl << R << "]");

  ROS_DEBUG_STREAM_NAMED("ekf.correction", "h(x)     = [" << y_pred.transpose() << "]");
  ROS_DEBUG_STREAM_NAMED("ekf.correction", "C        = [" << std::endl << C << "]");

  CP.noalias() = C * state().P();
  S.noalias()  = CP * C.transpose() + R;
  K.noalias() = CP.transpose() * S.inverse();
  state().P().noalias() -= K * CP;
  state().P().assertSymmetric();

  error.noalias() = y - y_pred;
  this->model_->limitError(error);
  update.noalias() = K * error;

  state().update(update);

  ROS_DEBUG_STREAM_NAMED("ekf.correction", "S        = [" << std::endl << S << "]");
  ROS_DEBUG_STREAM_NAMED("ekf.correction", "K        = [" << std::endl << K << "]");
  ROS_DEBUG_STREAM_NAMED("ekf.correction", "error    = [" << error.transpose() << "]");

  ROS_DEBUG_STREAM_NAMED("ekf.correction", "x_post   = [" << state().getVector().transpose() << "]");
  ROS_DEBUG_STREAM_NAMED("ekf.correction", "P_post   = [" << std::endl << state().getCovariance() << "]");

  this->init_ = false;
  return true;
}

} // namespace filter
} // namespace hector_pose_estimation

#endif // HECTOR_POSE_ESTIMATION_FILTER_EKF_INL


#ifndef EULERDEFUALTBASE_HPP
#define EULERDEFAULTBASE_HPP

#include <string>

#include <armadillo>

#include "Conv2D/Mesh.hpp"

/**
 * @brief A class implementing Euler advection on 2D geometries
 */
class EulerDefaultBase
{
public:
    /**
     * @brief Default constructor
     */
    EulerDefaultBase() = default;

    /**
     * @brief Default destructor
     */
    virtual ~EulerDefaultBase() = default;

    /**
     * @brief Assign the unstructured, triangular mesh describing the geometry
     */
    void setMesh(const Mesh &mesh)
    {
        mesh_ = mesh;
        parseBoundaryGroups();
    }

    /**
     * @brief Set up the specific heat ratio
     */
    void setSpecificHeatRatio(double gamma)
    {
        gamma_ = gamma;
    }

    /**
     * @brief Set up the gas constant
     */
    void setGasConstant(double R)
    {
        R_ = R;
    }

    /**
     * @brief Set up the free stream Mach number
     */
    void setFreeFlowMachNumber(double MInf)
    {
        MInf_ = MInf;
    }

    /**
     * @brief Set up the free stream static pressure
     */
    void setFreeFlowStaticPressure(double pInf)
    {
        pInf_ = pInf;
    }

    /**
     * @brief Set up the CFL number for timestepping
     */
    void setCFLNumber(double CFL)
    {
        CFL_ = CFL;
    }

    /**
     * @brief Set up the initial state to free stream state
     *
     * This routine should not be called unless the mesh, specific heat ratio,
     * gas constant, free stream mach number and free stream static pressure
     * has been already set for the class instance
     */
    void setInitialState();

    /**
     * @brief Set up the initial state from the values in file
     *
     * This routine should not be called unless the mesh, specific heat ratio,
     * gas constant, free stream mach number and free stream static pressure
     * has been already set for the class instance
     *
     * @param[in] File name containing the initial data
     */
    void setInitialState(const std::string fileName);

    /**
     * @brief Compute Roe flux
     *
     * This routine should not be called unless the specific heat ratio has
     * aready been set
     *
     * @param[in]  UL State on the left
     * @param[in]  UR State on the right
     * @param[in]  n  Normal pointing from left to right
     * @param[out] F  Computed numerical flux
     * @param[out] s  Computed maximum wave speed across interface
     */
    void computeRoeFlux(const arma::vec &UL, const arma::vec &UR,
                        const arma::rowvec &n, arma::vec &F, double &s) const;

    /**
     * @brief Specify how to compute flux at bottom boundary
     *
     * By default compute Roe flux w.r.t. the free stream state. However, this
     * virtual function can be overriden. Examples include: applyFreeStreamBC,
     * applyInvisidWallBC, applyInflowBC, applyOutflowBC.
     *
     * @param[in]  UInt Interior state
     * @param[in]  n    Normal pointing out from mesh
     * @param[out] U    Computed boundary state
     * @param[out] F    Computed numerical flux
     * @param[out] s    Computed maximum wave speed across interface
     */
    virtual void computeBottomFlux(const arma::vec &UInt, const arma::rowvec &n,
                                   arma::vec &U, arma::vec &F,
                                   double &s) const;

    /**
     * @brief Specify how to compute flux at right boundary
     *
     * By default compute Roe flux w.r.t. the free stream state. However, this
     * virtual function can be overriden. Examples include: applyFreeStreamBC,
     * applyInvisidWallBC, applyInflowBC, applyOutflowBC.
     *
     * @param[in]  UInt Interior state
     * @param[in]  n    Normal pointing out from mesh
     * @param[out] U    Computed boundary state
     * @param[out] F    Computed numerical flux
     * @param[out] s    Computed maximum wave speed across interface
     */
    virtual void computeRightFlux(const arma::vec &UInt,  const arma::rowvec &n,
                                  arma::vec &U, arma::vec &F,
                                  double &s) const;

    /**
     * @brief Specify how to compute flux at top boundary
     *
     * By default compute Roe flux w.r.t. the free stream state. However, this
     * virtual function can be overriden. Examples include: applyFreeStreamBC,
     * applyInvisidWallBC, applyInflowBC, applyOutflowBC.
     *
     * @param[in]  UInt Interior state
     * @param[in]  n    Normal pointing out from mesh
     * @param[out] U    Computed boundary state
     * @param[out] F    Computed numerical flux
     * @param[out] s    Computed maximum wave speed across interface
     */
    virtual void computeTopFlux(const arma::vec &UInt, const arma::rowvec &n,
                                arma::vec &U, arma::vec &F,
                                double &s) const;

    /**
     * @brief Specify how to compute flux at left boundary
     *
     * By default compute Roe flux w.r.t. the free stream state. However, this
     * virtual function can be overriden. Examples include: applyFreeStreamBC,
     * applyInvisidWallBC, applyInflowBC, applyOutflowBC.
     *
     * @param[in]  UInt Interior state
     * @param[in]  n    Normal pointing out from mesh
     * @param[out] U    Computed boundary state
     * @param[out] F    Computed numerical flux
     * @param[out] s    Computed maximum wave speed across interface
     */
    virtual void computeLeftFlux(const arma::vec &UInt, const arma::rowvec &n,
                                 arma::vec &U, arma::vec &F,
                                 double &s) const;

    /**
     * @brief Run the first order solver for specified number of iterations
     *
     * This routine should not be called unless the mesh, specific heat ratio,
     * gas constant, free stream mach number and free stream static pressure
     * has been already set for the class instance, and an initial state has
     * been set
     *
     * @param[in] numIter      Number of iterations to run
     * @param[in] residualFile Name of the file to store the residuals
     */
    void runFirstOrderSolver(arma::uword numIter,
                             const std::string &residualFile);

    /**
     * @brief Run the first order solver until specified tolerance is met
     *
     * This routine should not be called unless the mesh, specific heat ratio,
     * gas constant, free stream mach number and free stream static pressure
     * has been already set for the class instance, and an initial state has
     * been set
     *
     * @param[in] tolerance    Tolerance level
     * @param[in] residualFile Name of the file to store the residuals
     */
    void runFirstOrderSolver(double tolerance,
                             const std::string &residualFile);

    /**
     * @brief Run the second order solver for specified number of iterations
     *
     * This routine should not be called unless the mesh, specific heat ratio,
     * gas constant, free stream mach number and free stream static pressure
     * has been already set for the class instance, and an initial state has
     * been set
     *
     * @param[in] numIter      Number of iterations to run
     * @param[in] residualFile Name of the file to store the residuals
     */
    void runSecondOrderSolver(arma::uword numIter,
                              const std::string &residualFile);

    /**
     * @brief Run the second order solver until specified tolerance is met
     *
     * This routine should not be called unless the mesh, specific heat ratio,
     * gas constant, free stream mach number and free stream static pressure
     * has been already set for the class instance, and an initial state has
     * been set
     *
     * @param[in] tolerance    Tolerance level
     * @param[in] residualFile Name of the file to store the residuals
     */
    void runSecondOrderSolver(double tolerance,
                              const std::string &residualFile);

    /**
     * @brief Write validation values to file
     *
     * The lift coefficent, drag coefficient and the entropy error are computed
     * and written to file
     *
     * This routine should not be called unless the mesh, specific heat ratio,
     * free stream mach number and free stream static pressure has been already
     * set for the class instance, and a state has been set (either through
     * specifying initial value or after running the solvers)
     *
     * @param[in] fileName Name of the output file
     */
    void writeValidationValuesTofile(const std::string &fileName) const;

    /**
     * @brief Write pressure coefficients to file
     *
     * Write the pressure coefficients with x coordinates of the midpoints
     * of the faces at the bottom boundary
     *
     * This routine should not be called unless the mesh, specific heat ratio,
     * free stream mach number and free stream static pressure has been already
     * set for the class instance, and a state has been set (either through
     * specifying initial value or after running the solvers)
     *
     * @param[in] fileName Name of the output file
     */
    void writePressureCoefficientsToFile(const std::string &fileName) const;

    /**
     * @brief Write Mach numbers to file
     *
     * Write the Mach numbers to a VTK file
     *
     * This routine should not be called unless the mesh and the state of
     * class instance is set (either through specifying initial value or after
     * running the solvers)
     *
     * @param[in] fileName Name of the output file
     */
    void writeMachNumbersToFile(const std::string &fileName) const;

    /**
     * @brief Write the state to file
     *
     * This routine should not be called unless some state has been set (either
     * through specifying initial value or after running the solvers)
     *
     * @param[in] fileName Name of the output file
     */
    void writeStateToFile(const std::string &fileName) const;

protected:
    /**
     * @brief Apply free stream boundary condition
     * @param[in]  UInt Interior state
     * @param[in]  n    Normal pointing out from the domain
     * @param[out] U    Constructed boundary state
     * @param[out] F    Computed numeric flux
     * @param[out] s    Computed maximum wavespeed
     */
    void applyFreeStreamBC(const arma::vec &UInt, const arma::rowvec &n,
                           arma::vec &U, arma::vec &F, double &s) const;

    /**
     * @brief Apply inviscid boundary condition
     * @param[in]  UInt Interior state
     * @param[in]  n    Normal pointing out from the domain
     * @param[out] U    Constructed boundary state
     * @param[out] F    Computed numeric flux
     * @param[out] s    Computed maximum wavespeed
     */
    void applyInvisidWallBC(const arma::vec &UInt, const arma::rowvec &n,
                            arma::vec &U, arma::vec &F, double &s) const;

    /**
     * @brief Apply inflow boundary condition
     * @param[in]  UInt Interior state
     * @param[in]  n    Normal pointing out from the domain
     * @param[out] U    Constructed boundary state
     * @param[out] F    Computed numeric flux
     * @param[out] s    Computed maximum wavespeed
     */
    void applyInflowBC(const arma::vec &UInt, const arma::rowvec &n,
                       arma::vec &U, arma::vec &F, double &s) const;

    /**
     * @brief Apply outflow boundary condition
     * @param[in]  UInt Interior state
     * @param[in]  n    Normal pointing out from the domain
     * @param[out] U    Constructed boundary state
     * @param[out] F    Computed numeric flux
     * @param[out] s    Computed maximum wavespeed
     */
    void applyOutflowBC(const arma::vec &UInt, const arma::rowvec &n,
                        arma::vec &U, arma::vec &F, double &s) const;

private:
    /**
     * @brief The mesh specifying the geometry
     */
    Mesh mesh_;

    /**
     * @brief Specific heat ratio
     */
    double gamma_;

    /**
     * @brief Gas constant
     */
    double R_;

    /**
     * @brief Free stream Mach number
     */
    double MInf_;

    /**
     * @brief Free stream static pressure
     */
    double pInf_;

    /**
     * @brief CFL number
     */
    double CFL_;

    /**
     * @brief Total stagnation inflow pressure
     */
    double pt_;

    /**
     * @brief Total stagnation inflow temperature
     */
    double Tt_;

    /**
     * @brief Free stream state
     */
    arma::vec Ufree_;

    /**
     * @brief Full state
     */
    arma::mat U_;

    /**
     * @brief Which boundary group is at the bottom
     */
    arma::uword bottomBGroup_;

    /**
     * @brief How many rows to go through B2E, Bn etc. to reach bottom boundary group
     */
    arma::uword bottomBGroupOffset_;

    /**
     * @brief Which boundary group is at the right
     */
    arma::uword rightBGroup_;

    /**
     * @brief How many rows to go through B2E, Bn etc. to reach right boundary group
     */
    arma::uword rightBGroupOffset_;

    /**
     * @brief Which boundary group is at the top
     */
    arma::uword topBGroup_;

    /**
     * @brief How many rows to go through B2E, Bn etc. to reach top boundary group
     */
    arma::uword topBGroupOffset_;

    /**
     * @brief Which boundary group is at the left
     */
    arma::uword leftBGroup_;

    /**
     * @brief How many rows to go through B2E, Bn etc. to reach left boundary group
     */
    arma::uword leftBGroupOffset_;

    /**
     * @brief Compute the free stream state
     */
    void computeFreeStreamState();

    /**
     * @brief Parse boundary groups and figure out which boundary is where
     *
     * In particular, set up a correspondance between boundary location and
     * boundary group numbers, e.g. if the boundary groups are specified in
     * the order: bottom, top, left, right, then this routine sets up
     * bottomBGroup = 0, topBGroup = 1, leftBGroup = 2 and rightBGroup = 3
     */
    void parseBoundaryGroups();

    /**
     * @brief Compute first order residual
     * @param[in]  U Current state
     * @param[out] R Computed residual tallies
     * @param[out] S Computed wave speed tallies
     * @return Infinity norm of the residual
     */
    double computeFirstOrderResidual(const arma::mat &U, arma::mat &R,
                                     arma::vec &S) const;

    /**
     * @brief Compute state gradients
     * @param[in]  U Current state
     * @param[out] G Computed gradients
     */
    void computeGradients(const arma::mat &U, arma::mat &G) const;

    /**
     * @brief Compute second order residual
     * @param[in]  U Current state
     * @param[in]  G Current gradients
     * @param[out] R Computed residual tallies
     * @param[out] S Computed wave speed tallies
     * @return Infinity norm of the residual
     */
    double computeSecondOrderResidual(const arma::mat &U, const arma::mat &G,
                                      arma::mat &R, arma::vec &S) const;

    /**
     * @brief Compute lift coefficient
     * @return Value of the lift coefficient
     */
    double liftCoefficient() const;

    /**
     * @brief Compute drag coefficient
     * @return Value of the drag coefficient
     */
    double dragCoefficient() const;

    /**
     * @brief Compute entropy error
     * @return Value of the entropy error
     */
    double entropyError() const;
};

#endif // EULERDEFAULTBASE_HPP


/*
 * Copyright (c) 2010-2012 Steffen Kieß
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#ifndef MATH_VECTOR3_HPP_INCLUDED
#define MATH_VECTOR3_HPP_INCLUDED

// Math::DiagMatrix3<T> is a 3-dimensional vector

#include <Core/Util.hpp>
#include <Core/Assert.hpp>

#include <Math/Forward.hpp>
#include <Math/Abs.hpp>

#include <complex>

#include <boost/type_traits/is_convertible.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/utility/enable_if.hpp>

//#define VECTOR3_USE_ARRAY
//#define VECTOR3_USE_PADDING

namespace Math {
  template <typename T> class
#ifdef VECTOR3_USE_PADDING
  __attribute__ ((aligned (4 * boost::alignment_of<T>::value)))
#endif
  Vector3 {
#ifdef VECTOR3_USE_ARRAY
    T data_[3];
#else
    T x_, y_, z_;
#endif
    //T _padding;

    class PrivateType {
      friend class Vector3;
      NVCC_HOST_DEVICE PrivateType () {}
    };

  public:
    NVCC_HOST_DEVICE Vector3 () {}
#if !defined (__CUDACC__)
#if defined (__clang__) || GCC_VERSION_IS_ATLEAST(4, 6)
#pragma GCC diagnostic push
#endif
#pragma GCC diagnostic ignored "-Wconversion"
#pragma GCC diagnostic ignored "-Wfloat-conversion"
#endif
#ifdef VECTOR3_USE_ARRAY
    //NVCC_HOST_DEVICE explicit Vector3 (T v) { data_[0] = v; data_[1] = v; data_[2] = v; }
    NVCC_HOST_DEVICE Vector3 (T x, T y, T z) { data_[0] = x; data_[1] = y; data_[2] = z; }
    template <typename U> NVCC_HOST_DEVICE Vector3 (Vector3<U> v, UNUSED typename boost::enable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) { data_[0] = v.x (); data_[1] = v.y (); data_[2] = v.z (); }
    template <typename U> NVCC_HOST_DEVICE explicit Vector3 (Vector3<U> v, UNUSED typename boost::disable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) { data_[0] = (T) v.x (); data_[1] = (T) v.y (); data_[2] = (T) v.z (); }
#else
    //NVCC_HOST_DEVICE explicit Vector3 (T v) : x_ (v), y_ (v), z_ (v) {}
    NVCC_HOST_DEVICE Vector3 (T x, T y, T z) : x_ (x), y_ (y), z_ (z) {}
    template <typename U> NVCC_HOST_DEVICE Vector3 (Vector3<U> v, UNUSED typename boost::enable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) : x_ (v.x ()), y_ (v.y ()), z_ (v.z ()) {}
    template <typename U> NVCC_HOST_DEVICE explicit Vector3 (Vector3<U> v, UNUSED typename boost::disable_if<boost::is_convertible<U, T>, PrivateType>::type dummy = PrivateType ()) : x_ (v.x ()), y_ (v.y ()), z_ (v.z ()) {}
#endif
#if !defined (__CUDACC__)
#if defined (__clang__) || GCC_VERSION_IS_ATLEAST(4, 6)
#pragma GCC diagnostic pop
#endif
#endif

#ifndef __OPENCL_VERSION__
    NVCC_HOST_DEVICE const T& x () const {
#ifdef VECTOR3_USE_ARRAY
      return data_[0];
#else
      return x_;
#endif
    }

    NVCC_HOST_DEVICE const T& y () const {
#ifdef VECTOR3_USE_ARRAY
      return data_[1];
#else
      return y_;
#endif
    }

    NVCC_HOST_DEVICE const T& z () const {
#ifdef VECTOR3_USE_ARRAY
      return data_[2];
#else
      return z_;
#endif
    }

    NVCC_HOST_DEVICE T& x () {
#ifdef VECTOR3_USE_ARRAY
      return data_[0];
#else
      return x_;
#endif
    }

    NVCC_HOST_DEVICE T& y () {
#ifdef VECTOR3_USE_ARRAY
      return data_[1];
#else
      return y_;
#endif
    }

    NVCC_HOST_DEVICE T& z () {
#ifdef VECTOR3_USE_ARRAY
      return data_[2];
#else
      return z_;
#endif
    }

#else
#define DEFQ(x, qual) qual T& x () qual { return x##_; }
#define DEFA(qual) DEFQ(x, qual) DEFQ(y, qual) DEFQ(z, qual)
#define DEFC(qual) DEFA(qual) DEFA(const qual)
    DEFC(__private) DEFC(__local) DEFC(__constant) DEFC(__global)
#undef DEFC
#undef DEFA
#undef DEFQ
#endif

    NVCC_HOST_DEVICE const T& operator[] (size_t i) const {
#ifdef VECTOR3_USE_ARRAY
      ASSERT (i >= 0 && i < 3);
      return data_[i];
#else
      if (i == 0)
        return x_;
      else if (i == 1)
        return y_;
      else if (i == 2)
        return z_;
      else
#if !(defined (__CUDACC__) && defined (__CUDA_ARCH__)) && !defined (__OPENCL_VERSION__) // TODO
        ABORT ();
#else
        return x_; // TODO
#endif
#endif
    }

    NVCC_HOST_DEVICE T& operator[] (size_t i) {
#ifdef VECTOR3_USE_ARRAY
      ASSERT (i >= 0 && i < 3);
      return data_[i];
#else
      if (i == 0)
        return x_;
      else if (i == 1)
        return y_;
      else if (i == 2)
        return z_;
      else
#if !(defined (__CUDACC__) && defined (__CUDA_ARCH__)) && !defined (__OPENCL_VERSION__) // TODO
        ABORT ();
#else
        return x_; // TODO
#endif
#endif
    }
  };

  // Operations on Vector3

#define RTS(op) DECLTYPE ((*(T*)NULL) op (*(U*)NULL))
#define RT(op) Vector3<RTS(op)>
#ifdef __CUDACC__ // Workaround BugPlayground/nvcc-5.0-templates-1.cu
  template <typename T> static ERROR_ATTRIBUTE ("should not be called") Vector3<T> helperGetVector3Type (T t);
#define RT2(op) DECLTYPE (helperGetVector3Type ((*(T*)NULL) op (*(U*)NULL)))
#define RETURN_RT(op) typedef RT2(op) Ty; return Ty
#else
#define RETURN_RT(op) return RT(op)
#endif
  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(+) operator+ (Vector3<T> v1, Vector3<U> v2) {
    RETURN_RT(+) (v1.x () + v2.x (), v1.y () + v2.y (), v1.z () + v2.z ());
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline Vector3<T>& operator+= (Vector3<T>& v1, Vector3<U> v2) {
    v1.x () += v2.x (); v1.y () += v2.y (); v1.z () += v2.z ();
    return v1;
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(-) operator- (Vector3<T> v1, Vector3<U> v2) {
    RETURN_RT(-) (v1.x () - v2.x (), v1.y () - v2.y (), v1.z () - v2.z ());
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline Vector3<T>& operator-= (Vector3<T>& v1, Vector3<U> v2) {
    v1.x () -= v2.x (); v1.y () -= v2.y (); v1.z () -= v2.z ();
    return v1;
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(*) operator* (Vector3<T> v, U scalar) {
    RETURN_RT(*) (v.x () * scalar, v.y () * scalar, v.z () * scalar);
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(*) operator* (T scalar, Vector3<U> v) {
    RETURN_RT(*) (scalar * v.x (), scalar * v.y (), scalar * v.z ());
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline Vector3<T>& operator*= (Vector3<T>& v, U scalar) {
    v.x () *= scalar; v.y () *= scalar; v.z () *= scalar;
    return v;
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(/) operator/ (Vector3<T> v, U scalar) {
    RETURN_RT(/) (v.x () / scalar, v.y () / scalar, v.z () / scalar);
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline Vector3<T>& operator/= (Vector3<T>& v, U scalar) {
    v.x () /= scalar; v.y () /= scalar; v.z () /= scalar;
    return v;
  }

  template <typename T> NVCC_HOST_DEVICE inline bool operator== (Vector3<T> v1, Vector3<T> v2) {
    return v1.x () == v2.x () && v1.y () == v2.y () && v1.z () == v2.z ();
  }
  template <typename T> NVCC_HOST_DEVICE inline bool operator!= (Vector3<T> v1, Vector3<T> v2) {
    return !(v1 == v2);
  }

  template <typename T> struct Abs2Impl<Vector3<T> > {
    static DECLTYPE(Math::abs2 (*(T*)0)) apply (Vector3<T> v) {
      return Math::abs2 (v.x ()) + Math::abs2 (v.y ()) + Math::abs2 (v.z ());
    }
  };

  // dot product
  template <typename T, typename U> NVCC_HOST_DEVICE inline RTS(*) operator* (Vector3<T> v1, Vector3<U> v2) {
    return v1.x () * v2.x () + v1.y () * v2.y () + v1.z () * v2.z ();
  }

  template <typename T, typename U> NVCC_HOST_DEVICE inline RT(*) crossProduct (Vector3<T> v1, Vector3<U> v2) {
    RETURN_RT(*) (v1.y () * v2.z () - v1.z () * v2.y (),
                  v1.z () * v2.x () - v1.x () * v2.z (),
                  v1.x () * v2.y () - v1.y () * v2.x ());
  }

  template <typename T, typename U> inline RTS(*) crossProductAbs (Vector3<T> v1, Vector3<U> v2) {
    return Math::abs (crossProduct (v1, v2));
  }

#undef RTS
#undef RT
#ifdef __CUDACC__ // Workaround BugPlayground/nvcc-5.0-templates-1.cu
#undef RT2
#undef RETURN_RT
#endif

  // Unary +/-
  template <typename T> NVCC_HOST_DEVICE inline Vector3<T> operator+ (Vector3<T> v) {
    return Vector3<T> (+v.x (), +v.y (), +v.z ());
  }
  template <typename T> NVCC_HOST_DEVICE inline Vector3<T> operator- (Vector3<T> v) {
    return Vector3<T> (-v.x (), -v.y (), -v.z ());
  }

  template <typename F> Vector3<F> inline real (Vector3<std::complex<F> > v) {
    return Vector3<F> (real (v.x ()), real (v.y ()), real (v.z ()));
  }

  template <typename F> Vector3<F> inline imag (Vector3<std::complex<F> > v) {
    return Vector3<F> (imag (v.x ()), imag (v.y ()), imag (v.z ()));
  }

  template <typename F> Vector3<std::complex<F> > inline conj (Vector3<std::complex<F> > v) {
    return Vector3<std::complex<F> > (conj (v.x ()), conj (v.y ()), conj (v.z ()));
  }
}

#endif // !MATH_VECTOR3_HPP_INCLUDED


// Copyright (c) Facebook, Inc. and its affiliates.

// This source code is licensed under the MIT license found in the
// LICENSE file in the root directory of this source tree.
#ifndef TORCHROT_H
#define TORCHROT_H

#include <assert.h>
#include <fstream>
#include <iostream>
#include <string>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include "dtt.h"
#include <torch/torch.h>

#ifdef __cplusplus
extern "C" {
#endif /* __cplusplus */

#define C_TORCH_EXPORT __attribute__((visibility("default")))

C_TORCH_EXPORT Eigen::Quaternionf tensor4ToQuat(torch::Tensor T);
C_TORCH_EXPORT Eigen::Matrix3f tensor33ToMatrix(torch::Tensor T);
C_TORCH_EXPORT Eigen::AngleAxisf tensor3ToAngleAxis(torch::Tensor T);
C_TORCH_EXPORT Eigen::Vector4f quatToVector(Eigen::Quaternionf q);

C_TORCH_EXPORT torch::Tensor normalizeQuaternion(torch::Tensor q);
C_TORCH_EXPORT torch::Tensor invertQuaternion(torch::Tensor q);
C_TORCH_EXPORT torch::Tensor quatToAxis(torch::Tensor q);
C_TORCH_EXPORT torch::Tensor quatToAngle(torch::Tensor q);
C_TORCH_EXPORT torch::Tensor quatToMatrix(torch::Tensor q);
C_TORCH_EXPORT torch::Tensor quatToRotvec(torch::Tensor q);
C_TORCH_EXPORT torch::Tensor matrixToQuat(torch::Tensor m);
C_TORCH_EXPORT torch::Tensor rotvecToQuat(torch::Tensor r);
C_TORCH_EXPORT torch::Tensor quaternionMultiply(torch::Tensor q1,
                                                torch::Tensor q2);

#endif

#ifdef __cplusplus
} /* extern "C" */
#endif /* __cplusplus */


//==============================================================================
//         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 BOOST_SIMD_BITWISE_FUNCTIONS_SCALAR_RROL_HPP_INCLUDED
#define BOOST_SIMD_BITWISE_FUNCTIONS_SCALAR_RROL_HPP_INCLUDED
#include <boost/simd/bitwise/functions/rrol.hpp>
#include <boost/simd/include/functions/scalar/unary_minus.hpp>
#include <boost/simd/include/functions/scalar/ror.hpp>
#include <boost/simd/include/functions/scalar/rol.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( rrol_, tag::cpu_, (A0)(A1)
                            , (scalar_< arithmetic_<A0> >)
                              (scalar_< int_<A1> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return (a1 > 0)? rol(a0, a1) :ror(a0, boost::simd::neg(a1));
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( rrol_, tag::cpu_, (A0)(A1)
                            , (scalar_< arithmetic_<A0> >)
                              (scalar_< uint_<A1> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(2)
    {
      return  rol(a0, a1);
    }
  };
} } }

#endif