#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
//#include <CGAL/Exact_predicates_exact_constructions_kernel_with_sqrt.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Projection_traits_xy_3.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_with_info_2.h>
#include <CGAL/Triangulation_vertex_base_with_id_2.h>
#include <CGAL/Min_circle_2.h>
#include <CGAL/Min_circle_2_traits_2.h>
#include <CGAL/basic.h>
#include <CGAL/QP_models.h>
#include <CGAL/QP_functions.h>
#include <CGAL/Gmpz.h>
#include "UnionFind.h"

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/bipartite.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

#include <iostream>
#include <cassert>
#include <vector>
#include <queue>
#include <stack>
#include <cmath>
#include <valarray>

// LP/QP
typedef CGAL::Gmpq ET;
// program and solution types
typedef CGAL::Quadratic_program<ET> Program;
typedef CGAL::Quadratic_program_solution<ET> Solution;

// General CGAL
typedef CGAL::Exact_predicates_exact_constructions_kernel EK;
//typedef CGAL::Exact_predicates_exact_constructions_kernel_with_sqrt KR;
typedef CGAL::Exact_predicates_inexact_constructions_kernel IK;
//typedef CGAL::Min_circle_2_traits_2<KR> Traits;
//typedef CGAL::Min_circle_2<Traits> Min_circle;

// Change when use different kernels!
typedef long long lint;
typedef IK::Point_2 Pt;
typedef IK::Point_3 Pt3;
typedef IK::Ray_2 Ray;
typedef IK::Vector_2 Vec;
typedef IK::Segment_2 Seg;

// Triangulation
typedef int ind_t; // not a good one...

//typedef CGAL::Triangulation_vertex_base_with_info_2<ind_t, IK>	Vb;
//typedef CGAL::Triangulation_data_structure_2<Vb>	Tds;
//typedef CGAL::Delaunay_triangulation_2<IK>		Delaunay;
//typedef Delaunay::Finite_faces_iterator		Face_iterator;
//typedef Delaunay::Finite_edges_iterator		Edge_iterator;
//typedef Delaunay::Vertex_handle		Vertex_handle;
//typedef Delaunay::Point		DPt;

// Boost
typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::undirectedS,
	boost::no_property, boost::property < boost::edge_weight_t, lint > > Graph;
typedef boost::property_map < Graph, boost::edge_weight_t >::type WeightMap;
typedef boost::graph_traits < Graph >::edge_descriptor Edge;
typedef boost::graph_traits < Graph >::edge_iterator EdgeIt;
typedef boost::graph_traits < Graph >::out_edge_iterator OutEdgeIt;
typedef boost::graph_traits < Graph >::vertex_descriptor Vertex;
//typedef std::pair<int, int> E;

#define _o std::cout
#define _l std::endl
#define _i std::cin
#define _e std::cerr

double floor_to_double(const CGAL::Quotient<ET>& x)
{
	double a = std::floor(CGAL::to_double(x));
	while (a > x) a -= 1;
	while (a + 1 <= x) a += 1;
	return a;
}

double ceil_to_double(const CGAL::Quotient<ET>& x)
{
	double a = std::ceil(CGAL::to_double(x));
	while (a < x) a += 1;
	while (a - 1 >= x) a -= 1;
	return a;
}

struct MyRay {
	Ray r;
	int idx;
	MyRay(Ray r=Ray(), int i=0):r(r),idx(i){}
	MyRay(Pt src, Pt next, int i) {
		r = Ray(src, next);
		idx = i;
	}
};
bool CompareRayByY0(const MyRay& r1, const MyRay& r2) {
	return r1.r.source().y() > r2.r.source().y();
}
int whoWins(const std::vector<MyRay>& rays, int oldone, int newone) {
	Vec vo = rays[oldone].r.to_vector();
	Vec vn = rays[newone].r.to_vector();
	if (vo.y() >= 0 && vn.y() >= 0) {
		return oldone;
	}
	if (vo.y() < 0 && vn.y() < 0) {
		return newone;
	}
	if (vo.y() >= 0 && vn.y() < 0) {
		return (vo.y() * vn.x() + vo.x() * vn.y() > 0) ? newone : oldone;
	}
	if (vo.y() < 0 && vn.y() >= 0) {
		return (vo.y() * vn.x() + vo.x() * vn.y() > 0) ? oldone : newone;
	}
	return -1;
}
int rayChallenge(Ray& rold, Ray& rnew) {
	// rold.y0 > rnew.y0
	// return 0 if old one wins
	// return 1 if new one wins
	Vec vo = rold.to_vector();
	Vec vn = rnew.to_vector();
	if (vo.y() >= 0 && vn.y() >= 0) {
		return 0;
	}
	if (vo.y() < 0 && vn.y() < 0) {
		return 1;
	}
	if (vo.y() >= 0 && vn.y() < 0) {
		Seg so(rold.source(), rold.source() + vo);
		Seg sn(rnew.source(), Pt(rnew.source().x() + vn.x(), rnew.source().y() - vn.y()));
		CGAL::Comparison_result cr = CGAL::compare_slopes(so, sn);
		//_e << " _" << cr << "_ ";
		return (cr == CGAL::LARGER ? 1 : 0);
		//return (vo.y() * vn.x() + vo.x() * vn.y() > 0) ? 1 : 0;
	}
	if (vo.y() < 0 && vn.y() >= 0) {
		Seg so(rold.source(), Pt(rold.source().x() + vo.x(), rold.source().y() - vo.y()));
		Seg sn(rnew.source(), rnew.source() + vn);
		CGAL::Comparison_result cr = CGAL::compare_slopes(sn, so);
		//_e << " _" << cr << "_ ";
		return (cr == CGAL::LARGER ? 0 : 1);
	}
	return 0;
}
void Motorcycles() {
	int n; _i >> n;
	_e << n << _l;
	std::vector<MyRay> rays(n);
	for (int i = 0; i < n; i++) {
		lint y0, x1, y1;
		_i >> y0 >> x1 >> y1;
		rays[i] = MyRay(Pt(0, y0), Pt(x1, y1), i);
	}
	std::sort(rays.begin(), rays.end(), CompareRayByY0);

	std::stack<MyRay> st_rays;
	st_rays.push(rays[0]);
	for (int i = 1; i < n; i++) {
		MyRay ray = rays[i];
		while (!st_rays.empty()) {
			MyRay topray = st_rays.top();
			if (!CGAL::do_intersect(ray.r, topray.r)) {
				st_rays.push(ray);
				//_e << ray.idx << " in stack\n";
				break;
			}
			else {
				//_e << topray.idx << ":" << topray.r.to_vector() << "---" << ray.idx << ":" << ray.r.to_vector();
				int who = rayChallenge(topray.r, ray.r);
				//_e << " \twinner:" << ((who==0)?topray.idx:ray.idx) << _l;
				if (who == 0) break;
				else {
					//_e << topray.idx << " out of stack\n";
					st_rays.pop();
				}
			}
		}
		if (st_rays.empty()) {
			st_rays.push(ray);
			//_e << ray.idx << " in empty stack\n";
		}
	}
	//_e << _l;
	std::vector<int> alives;
	while (!st_rays.empty()) {
		alives.push_back(st_rays.top().idx);
		st_rays.pop();
	}
	std::sort(alives.begin(), alives.end());
	std::vector<int>::iterator ait = alives.begin();
	for (; ait != alives.end(); ++ait) {
		_o << *ait << " ";
	}
	_o << _l;
	return;
}

void testUnionFind();

int main(int argc, char** argv) {
	std::ios_base::sync_with_stdio(false);
	if (argc > 1) {
		std::string str("C:\\M.K.S.H\\ETH\\AlgoLab\\week6\\potw\\motorcycles\\");
		str += std::string(argv[1]) + ".in";
		freopen(str.c_str(), "r", stdin);
	}
	else freopen("C:\\M.K.S.H\\ETH\\AlgoLab\\week6\\potw\\motorcycles\\test1.in", "r", stdin);
	freopen("C:\\M.K.S.H\\ETH\\AlgoLab\\week6\\potw\\motorcycles\\out_my.out", "w", stdout);
	freopen("C:\\M.K.S.H\\ETH\\AlgoLab\\week6\\potw\\motorcycles\\err_my.out", "w", stderr);
	int t;
	_i >> t;
	while (t--) {
		Motorcycles();
	}
	return 0;
}

void testUnionFind() {
	UnionFind uf(9);
	uf.unionSets(1, 3);
	uf.unionSets(1, 9);
	uf.unionSets(5, 6);
	uf.unionSets(8, 9);
	uf.unionSets(7, 8);
	uf.unionSets(5, 8);
	_e << uf.toString() << _l;
	return;
}

// Week 8: Germs / 100 pts
// Nearest neighbor
void Germs(int n) {
	typedef CGAL::Triangulation_vertex_base_with_info_2<int, IK>	Vb;
	typedef CGAL::Triangulation_data_structure_2<Vb>	Tds;
	typedef CGAL::Delaunay_triangulation_2<IK, Tds>		Delaunay;
	typedef Delaunay::Finite_faces_iterator		Face_iterator;
	typedef Delaunay::Finite_edges_iterator		Edge_iterator;
	typedef Delaunay::Finite_vertices_iterator	Vertex_iterator;
	typedef Delaunay::Vertex_handle		Vertex_handle;
	typedef Delaunay::Point				DPt;
	typedef Delaunay::Vertex_circulator Vertex_circulator;

	int l, b, r, t;
	_i >> l >> b >> r >> t;

	std::vector< std::pair<Pt, int> > pts(n);
	std::vector<lint> mind2(n);
	for (int i = 0; i < n; i++) {
		int x, y;
		_i >> x >> y;
		lint dl = std::abs(x - l);
		lint dr = std::abs(x - r);
		lint db = std::abs(y - b);
		lint dt = std::abs(y - t);
		lint md = std::min(std::min(dl, dr), std::min(db, dt));
		mind2[i] = md*md * 4;
		pts[i] = std::make_pair(Pt(x, y), i);
	}
	lint fd2, md2, ld2;
	if (n > 1) {
		Delaunay D;
		D.insert(pts.begin(), pts.end());

		Vertex_iterator vit = D.finite_vertices_begin(), vend = D.finite_vertices_end();
		for (; vit != vend; ++vit) {
			int uind = vit->info();
			Pt up = vit->point();
			lint md2 = mind2[uind];
			Vertex_circulator vc(D.incident_vertices(vit)), done(vc);
			do {
				if (D.is_infinite(vc)) continue;
				Pt vp = vc->point();
				lint d2 = CGAL::squared_distance(vp, up);
				md2 = std::min(d2, md2);
			} while (++vc != done);
			mind2[uind] = md2;
		}
		std::sort(mind2.begin(), mind2.end());
	}

	fd2 = mind2[0], md2 = mind2[n / 2], ld2 = mind2[n - 1];
	double tf = std::sqrt((std::sqrt(fd2) - 1) / 2);
	double tm = std::sqrt((std::sqrt(md2) - 1) / 2);
	double tl = std::sqrt((std::sqrt(ld2) - 1) / 2);
	int tfi = std::ceil(tf);
	int tmi = std::ceil(tm);
	int tli = std::ceil(tl);
	_o << tfi << " " << tmi << " " << tli << _l;
	return;

}

// Week 8: H1N1 / incorrect 100pts?
// Find the maximum width of the path from outside to the vertex
void H1N1(int n) {
	// Boost
	typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::undirectedS,
		boost::no_property, boost::property < boost::edge_weight_t, long > > Graph;
	typedef boost::property_map < Graph, boost::edge_weight_t >::type WeightMap;
	typedef boost::graph_traits < Graph >::edge_descriptor Edge;
	typedef boost::graph_traits < Graph >::edge_iterator EdgeIt;
	typedef boost::graph_traits < Graph >::out_edge_iterator OutEdgeIt;
	typedef boost::graph_traits < Graph >::vertex_descriptor Vertex;
	// CGAL
	typedef CGAL::Triangulation_vertex_base_2<IK>	Vb;
	typedef CGAL::Triangulation_face_base_with_info_2<int, IK>	Fb;
	typedef CGAL::Triangulation_data_structure_2<Vb, Fb>	Tds;
	typedef CGAL::Delaunay_triangulation_2<IK, Tds>		Delaunay;
	typedef Delaunay::Finite_faces_iterator		Face_iterator;
	typedef Delaunay::Face_handle				Face_handle;
	typedef Delaunay::Finite_edges_iterator		Edge_iterator;
	typedef Delaunay::Edge_circulator			Edge_circulator;
	typedef Delaunay::Finite_vertices_iterator	Vertex_iterator;
	typedef Delaunay::Vertex_handle				Vertex_handle;
	typedef Delaunay::Point						DPt;
	typedef Delaunay::Vertex_circulator			Vertex_circulator;

	int m;

	std::vector<Pt> infected(n);
	for (int i = 0; i < n; i++) {
		int x, y;
		_i >> x >> y;
		infected[i] = Pt(x, y);
	}
	Delaunay D;
	D.insert(infected.begin(), infected.end());

	Graph G(1); // only infinite face
	WeightMap wmap = boost::get(boost::edge_weight, G); // weight = width^2 = r^2*4
	int newind = 1; // gradually add faces

	Face_iterator fit = D.finite_faces_begin(), fend = D.finite_faces_end();
	for (; fit != fend; ++fit) {
		fit->info() = 0; // or it could be anything
	}

	fit = D.finite_faces_begin(), fend = D.finite_faces_end();
	for (; fit != fend; ++fit) {
		Vertex u;
		if (!fit->info()) {
			u = boost::add_vertex(G);
			fit->info() = newind++;
		}
		else u = fit->info();
		// circulate its 3 neighbors to add edges to the graph
		for (int nb = 0; nb < 3; nb++) {
			Face_handle fh = fit->neighbor(nb);
			if (D.is_infinite(fh)) {
				// infinite face: add edge to vertex 0
				Vertex v = 0;
				Edge e; bool su;
				Vertex_handle vh1 = fit->vertex(fit->cw(nb)), vh2 = fit->vertex(fit->ccw(nb));
				lint d2 = CGAL::squared_distance(vh1->point(), vh2->point());
				boost::tie(e, su) = boost::add_edge(u, v, G);
				wmap[e] = d2;
			}
			else {
				//  if the face hasn't been visited, add a new vertex and fill fh->info()
				Vertex v;
				if (!fh->info()) {
					fh->info() = newind++;
					v = boost::add_vertex(G);
				}
				else v = fh->info();

				// continue if the edge already exists
				Edge e; bool su;
				boost::tie(e, su) = boost::edge(u, v, G);
				if (su) continue;

				Vertex_handle vh1 = fit->vertex(fit->cw(nb)), vh2 = fit->vertex(fit->ccw(nb));
				lint d2 = CGAL::squared_distance(vh1->point(), vh2->point());
				boost::tie(e, su) = boost::add_edge(u, v, G);
				wmap[e] = d2;
			}
		}
	}

	const lint BIG = 10000000000000000;
	typedef std::pair<lint, Vertex> DistV;
	int nv = boost::num_vertices(G);
	std::vector<DistV> vdist(nv);
	for (int i = 0; i < nv; i++) vdist[i] = std::make_pair(-1, i);

	std::vector<bool> selected(nv, false); // whether the width of a vertex has been optimized
	std::vector<lint> distmap(nv, -1); // the maximum width towards each vertex
	OutEdgeIt eit, eend;
	for (boost::tie(eit, eend) = boost::out_edges(0, G); eit != eend; eit++) {
		Vertex v = boost::target(*eit, G);
		vdist[v].first = std::max(vdist[v].first, (lint)wmap[*eit]);
		distmap[v] = std::max(distmap[v], (lint)wmap[*eit]);
	}
	vdist[0].first = 0;

	_e << "BFS " << _l;
	// BFS for max min edge
	std::priority_queue < DistV > pq(vdist.begin(), vdist.end());
	selected[0] = true;
	distmap[0] = BIG;
	while (!pq.empty()) {
		DistV dv = pq.top();
		pq.pop();
		lint d = dv.first;
		Vertex u = dv.second;
		if (selected[u]) continue; // see the pq.push() below, one vertex could be pushed multiple times
		OutEdgeIt eit, eend;
		for (boost::tie(eit, eend) = boost::out_edges(u, G); eit != eend; eit++)
		{
			Vertex v = boost::target(*eit, G);
			if (!selected[v]) {
				// the updating rule: d[v] = max(d[v], min(w[e], d[u]))
				lint curw = std::max((long)distmap[v], std::min(wmap[*eit], (long)distmap[u]));
				distmap[v] = curw;
				pq.push(std::make_pair(distmap[v], v));
			}
		}
		selected[u] = true;
	}

	_i >> m;
	for (int j = 0; j < m; j++) {
		int x, y; lint r2;
		_i >> x >> y >> r2;
		Pt person(x, y);
		Vertex_handle vh = D.nearest_vertex(person);
		if (CGAL::squared_distance(person, vh->point()) < r2) {
			_o << "n"; continue;
		}
		Face_handle fh = D.locate(person);
		if (D.is_infinite(fh)) {
			_o << "y"; continue;
		}
		Vertex u = fh->info();
		if (r2 * 4 > distmap[u]) _o << "n";
		else _o << "y";
	}
	_o << _l;
}

// Week 8: Graypes / 50->50->50->100
// Delaunay, find shortest edge
// Use Vertex_iterator to iterate through the vertices -> O(1)
// rather than nearest_vertex() -> O(log n)
// Tho I didn't they would kill an O(nlogn) algo..
// Don't play with constant when there is a method to improve asymptotic
void Graypes(int n) {
	typedef CGAL::Triangulation_vertex_base_with_info_2<int, IK>	Vb;
	typedef CGAL::Triangulation_data_structure_2<Vb>	Tds;
	typedef CGAL::Delaunay_triangulation_2<IK, Tds>		Delaunay;
	typedef Delaunay::Finite_faces_iterator		Face_iterator;
	typedef Delaunay::Finite_edges_iterator		Edge_iterator;
	typedef Delaunay::Finite_vertices_iterator	Vertex_iterator;
	typedef Delaunay::Vertex_handle		Vertex_handle;
	typedef Delaunay::Point				DPt;
	typedef Delaunay::Vertex_circulator Vertex_circulator;

	std::vector< std::pair<Pt, int> > apes(n);
	for (int i = 0; i < n; i++) {
		int x, y;
		_i >> x >> y;
		apes[i] = std::make_pair(Pt(x, y), i);
	}
	//std::random_shuffle(apes.begin(), apes.end());

	Delaunay D;
	D.insert(apes.begin(), apes.end());

	const lint BIG = 10000000000000000;
	//std::vector<int> runto(n, -1);
	//std::vector<lint> d2runto(n, BIG);
	lint totalmind2 = BIG;

	Vertex_iterator vit = D.finite_vertices_begin(), vend = D.finite_vertices_end();
	for (; vit != vend; ++vit) {
		Pt p = vit->point();

		Vertex_circulator vc = D.incident_vertices(vit), done(vc);
		do {
			if (D.is_infinite(vc))continue;
			Pt q = vc->point();
			lint d2pq = CGAL::squared_distance(p, q);
			totalmind2 = std::min(totalmind2, d2pq);
		} while (++vc != done);
	}

	/* O(nlogn) because of nearest_vertex()
	for (int i = 0; i < n; i++) {
	//lint mind2 = d2runto[i];
	//int prunto = runto[i];
	lint mind2 = BIG;
	Pt p = apes[i].first;
	int pind = apes[i].second;
	Vertex_handle vh = D.nearest_vertex(p);
	Vertex_circulator vc = D.incident_vertices(vh), done(vc);
	do {
	if (D.is_infinite(vc)) continue;
	Pt q = vc->point();
	int qind = vc->info();
	if (qind < pind) continue;
	lint d2pq = CGAL::squared_distance(p, q);
	if (d2pq < mind2) {
	mind2 = d2pq;
	//prunto = qind;
	}
	//else if (d2pq == mind2) {
	//	Pt prevbest = apes[prunto].first;
	//	if (q.x() < prevbest.x() || (q.x() == prevbest.x() && q.y() < prevbest.y())) {
	//		prunto = qind;
	//	}
	//}
	} while (++vc != done);
	//if (prunto != runto[i]) {
	//	runto[i] = prunto;
	//	d2runto[i] = mind2;
	//	if (mind2 < d2runto[prunto]) {
	//		runto[prunto] = i;
	//		d2runto[prunto] = mind2;
	//	}
	//	else if (mind2 == d2runto[prunto]) {
	//		Pt p2 = apes[runto[prunto]].first;
	//		if (p.x() < p2.x() || (p.x() == p2.x() && p.y() < p2.y())) {
	//			runto[prunto] = i;
	//		}
	//	}
	//}
	totalmind2 = std::min(totalmind2, mind2);
	}
	*/

	lint time = std::ceil(50 * std::sqrt((double)(totalmind2)));
	_o << time << _l;
	return;
}

// Week 11 PotW: Sith / 25->50->100pts
// Delaunay + find connected components
// Update connected components: Union Find
int calcBiggestComponent(const std::vector<int>& comp, int ncomp) {
	int n = comp.size();
	std::vector<int> counter(ncomp);
	for (int i = 0; i < n; i++) {
		counter[comp[i]]++;
	}
	int largest = 0;
	for (int j = 0; j < ncomp; j++) {
		largest = std::max(largest, counter[j]);
	}
	return largest;
}
void Comp2Par(const std::vector<int>& comp, int ncomp, std::vector<int>& par) {
	int n = comp.size();
	std::vector<int> first(ncomp, -1);
	for (int i = 0; i < n; i++) {
		if (first[comp[i]] == -1)
			first[comp[i]] = i;
		par[i] = first[comp[i]];
	}
}
void Sith() {
	typedef CGAL::Triangulation_vertex_base_with_info_2<int, IK>	Vb;
	typedef CGAL::Triangulation_data_structure_2<Vb>	Tds;
	typedef CGAL::Delaunay_triangulation_2<IK, Tds>		Delaunay;
	typedef Delaunay::Finite_faces_iterator		Face_iterator;
	typedef Delaunay::Finite_edges_iterator		Edge_iterator;
	typedef Delaunay::Finite_vertices_iterator	Vertex_iterator;
	typedef Delaunay::Vertex_handle		Vertex_handle;
	typedef Delaunay::Point				DPt;
	typedef Delaunay::Vertex_circulator Vertex_circulator;

	typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::undirectedS,
		boost::no_property, boost::property < boost::edge_weight_t, double > > Graph;
	typedef boost::property_map < Graph, boost::edge_weight_t >::type WeightMap;
	typedef boost::graph_traits < Graph >::edge_descriptor Edge;
	typedef boost::graph_traits < Graph >::vertex_descriptor Vertex;

	int n, r;
	_i >> n >> r;
	_e << "testcase: " << n << " " << r << _l;
	lint r2 = (lint)r*r;
	std::vector< std::pair<Pt, int> > p(n);
	// invert the input
	// so I choose planets [0, k) rather than [k, end)
	for (int i = 0; i < n; i++) {
		int x, y;
		_i >> x >> y;
		p[n - 1 - i] = std::make_pair(Pt(x, y), n - 1 - i);
	}

	Delaunay D;
	int nv = n / 2;
	D.insert(p.begin(), p.begin() + nv);
	Graph G(nv);
	for (int i = 0; i < nv; i++) {
		Vertex_handle vh = D.nearest_vertex(p[i].first);
		Pt pt = vh->point();
		int ipt = vh->info();
		Vertex_circulator vc = D.incident_vertices(vh), done(vc);
		do {
			if (D.is_infinite(vc)) continue;
			Pt pin = vc->point();
			int ipin = vc->info();
			if (ipin < ipt) continue; // i --> j, add edge when i < j
			lint d2 = CGAL::squared_distance(pt, pin);
			if (d2 <= r2) {
				boost::add_edge(ipt, ipin, G);
			}
		} while (++vc != done);
	}

	std::vector<int> comp(nv), par(nv);
	int ncomp = boost::connected_components(G, &comp[0]);
	int biggestComp = calcBiggestComponent(comp, ncomp);
	Comp2Par(comp, ncomp, par);
	UnionFind uf(par);

	for (int last = nv; last < n; last++) {
		int largestPossible = n - last - 1;
		if (biggestComp >= largestPossible) break;
		Vertex vlast = boost::add_vertex(G);
		Pt plast = p[last].first; // p[last].second == last
		D.insert(p.begin() + last, p.begin() + last + 1);
		int ipt = last;
		Vertex_handle vh = D.nearest_vertex(plast);
		Vertex_circulator vc = D.incident_vertices(vh), done(vc);
		uf.append();
		do {
			if (D.is_infinite(vc)) continue;
			Pt pin = vc->point();
			int ipin = vc->info();
			lint d2 = CGAL::squared_distance(plast, pin);
			if (d2 <= r2) {
				boost::add_edge(ipt, ipin, G);
				uf.unionSets(ipt, ipin);
			}
		} while (++vc != done);

		//std::vector<int> ccomp(last+1);
		//int nccomp = boost::connected_components(G, &ccomp[0]);
		//int largest = calcBiggestComponent(ccomp, nccomp);
		int largest = uf.getMaxSize();
		largest = std::min(largest, largestPossible);
		biggestComp = std::max(biggestComp, largest);
	}
	_e << "result: " << biggestComp << _l;
	_o << biggestComp << _l;
	return;

}

// Week 12: Radiation / 60->80->100
// LP + bin_search?? 2^300??
// LP (with Gmpz not Gmpq) + exp_search+bin_search
// they are exact number types and don't worry about 2^300
struct PolyTerm {
	int xd;
	int yd;
	int zd;
	int idx;
	PolyTerm(int x = 0, int y = 0, int z = 0, int idx = 0) :xd(x), yd(y), zd(z), idx(idx) {}
};
void calcPolyTerm(int deg, std::vector<PolyTerm> &terms) {
	//int balls = deg + 2;
	terms.clear();
	int idx = 0;
	int d = deg;
	for (int d = 0; d <= deg; d++) {
		int balls = d + 2;
		for (int i = 0; i < balls - 1; i++) {
			for (int j = i + 1; j < balls; j++) {
				PolyTerm pt(i, j - i - 1, d + 1 - j, idx);
				terms.push_back(pt);
				idx++;
			}
		}
	}
	return;
}
typedef CGAL::Gmpz ETZ;
typedef CGAL::Quadratic_program<ETZ> ProgramZ;
typedef CGAL::Quadratic_program_solution<ETZ> SolutionZ;
ETZ findPoly(int deg, std::vector<Pt3> &hcell, std::vector<Pt3> &tcell, std::vector< std::vector<ETZ> >& hpowers, std::vector< std::vector<ETZ> >& tpowers) {

	ProgramZ lp(CGAL::SMALLER, false, 0, false, 0);
	std::vector<PolyTerm> terms;
	calcPolyTerm(deg, terms);

	int h = hcell.size(), t = tcell.size();
	int nterms = terms.size();
	const int delta = nterms;
	for (int j = 0; j < nterms; j++) {
		//lp.set_c(j, 1);
		int xind = terms[j].xd, yind = 31 + terms[j].yd, zind = 62 + terms[j].zd;
		for (int i = 0; i < h; i++) {
			ETZ xyz = hpowers[i][xind] * hpowers[i][yind] * hpowers[i][zind];
			lp.set_a(j, i, xyz);
		}
		for (int i = 0; i < t; i++) {
			ETZ xyz = tpowers[i][xind] * tpowers[i][yind] * tpowers[i][zind];
			lp.set_a(j, h + i, -xyz);
		}
	}
	lp.set_c(delta, -1);
	//lp.set_l(delta, true, 0);
	lp.set_u(delta, true, 1);
	for (int i = 0; i < h; i++) {
		lp.set_b(i, 0);
		lp.set_a(delta, i, 0);
	}
	for (int i = 0; i < t; i++) {
		lp.set_a(delta, h + i, 1);
		lp.set_b(h + i, 0);
	}

	SolutionZ s = CGAL::solve_linear_program(lp, ETZ());
	//if (s.is_optimal() && s.objective_value() < 0)
	//	return true;
	//return false;
	ETZ result = s.objective_value().numerator() / s.objective_value().denominator();
	return result;
}
void calcPowers(std::vector<Pt3> & cell, std::vector< std::vector<ETZ> >& powers) {
	int n = cell.size();
	_e << "powers!" << _l;
	for (int i = 0; i < n; i++) {
		powers[i] = std::vector<ETZ>(93);
		ETZ xp(1), yp(1), zp(1);
		int cx = (int)cell[i].x();
		int cy = (int)cell[i].y();
		int cz = (int)cell[i].z();
		for (int d = 0; d <= 30; d++) {
			powers[i][d] = xp;
			powers[i][31 + d] = yp;
			powers[i][62 + d] = zp;
			if (d == 30) break;
			xp *= cx;
			yp *= cy;
			zp *= cz;
			//_e << xp << " " << yp << " " << zp << _l;
		}
	}
}
void Radiation() {
	int h, t; _i >> h >> t;
	std::vector<Pt3> hcell(h), tcell(t);
	for (int i = 0; i < h; i++) {
		int x, y, z; _i >> x >> y >> z;
		hcell[i] = Pt3(x, y, z);
		//_e << hcell[i] << _l;
	}
	for (int j = 0; j < t; j++) {
		int x, y, z; _i >> x >> y >> z;
		tcell[j] = Pt3(x, y, z);
		//_e << tcell[j] << _l;
	}
	if (h == 0 || t == 0) {
		_o << 0 << _l;
		return;
	}

	std::vector< std::vector<ETZ> > hpowers(h), tpowers(t);
	calcPowers(hcell, hpowers);
	calcPowers(tcell, tpowers);
	_e << "calcpowers!" << _l;

	//std::vector<bool> canFinds(31, false);
	std::vector<ETZ> res(31, 0);
	int st = 0, ed = 1;
	int low, up, mid;
	while (true) {
		_e << "d=" << ed << _l;
		//canFinds[ed] = canFindPoly(ed, hcell, tcell, hpowers, tpowers);
		res[ed] = findPoly(ed, hcell, tcell, hpowers, tpowers);
		if (res[ed] < 0) {
			low = st;
			up = ed;
			mid = (low + up) / 2;
			break;
		}
		else {
			if (ed == 30) {
				_o << "Impossible!\n";
				return;
			}
			st = ed;
			ed = std::min(30, ed * 2);
		}
	}

	// binary search
	while (true) {
		_e << "d=" << mid << _l;
		if (up - low <= 1) {
			if (res[low]<0 || findPoly(low, hcell, tcell, hpowers, tpowers)<0) {
				_o << low << _l;
				return;
			}
			else if (res[up]<0 || findPoly(up, hcell, tcell, hpowers, tpowers)<0) {
				_o << up << _l;
				return;
			}
		}

		res[mid] = findPoly(mid, hcell, tcell, hpowers, tpowers);
		if (res[mid]<0) {
			up = mid;
			mid = (low + up) / 2;
		}
		else {
			low = mid + 1;
			mid = (low + up) / 2;
		}
	}

	_e << _l;
	_o << "Impossible!" << _l;
	return;

}

// Week 13: Clues / 100pts
// Delaunay + is_bipartite + connected_components
void Clues() {
	typedef CGAL::Triangulation_vertex_base_with_info_2<int, IK>	Vb;
	typedef CGAL::Triangulation_data_structure_2<Vb>	Tds;
	typedef CGAL::Delaunay_triangulation_2<IK, Tds>		Delaunay;
	typedef Delaunay::Finite_faces_iterator		Face_iterator;
	typedef Delaunay::Finite_edges_iterator		Edge_iterator;
	typedef Delaunay::Finite_vertices_iterator	Vertex_iterator;
	typedef Delaunay::Vertex_handle		Vertex_handle;
	typedef Delaunay::Point				DPt;
	typedef Delaunay::Vertex_circulator Vertex_circulator;

	typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::undirectedS,
		boost::no_property, boost::property < boost::edge_weight_t, double > > Graph;
	typedef boost::property_map < Graph, boost::edge_weight_t >::type WeightMap;
	typedef boost::graph_traits < Graph >::edge_descriptor Edge;
	typedef boost::graph_traits < Graph >::vertex_descriptor Vertex;

	int n, m, r;
	_i >> n >> m >> r;
	lint r2 = (lint)r*r;
	std::vector< std::pair<Pt, int> > stations(n);
	std::vector< std::pair<Pt, Pt> > clues(m);
	for (int i = 0; i < n; i++) {
		int x, y; _i >> x >> y;
		stations[i] = std::make_pair(Pt(x, y), i);
	}
	for (int j = 0; j < m; j++) {
		int xa, ya, xb, yb; _i >> xa >> ya >> xb >> yb;
		clues[j] = std::make_pair(Pt(xa, ya), Pt(xb, yb));
	}

	Graph G(n);
	std::vector<int> components(n);

	Delaunay D;
	D.insert(stations.begin(), stations.end());
	Vertex_iterator vit;
	bool violated = false;
	for (vit = D.finite_vertices_begin(); vit != D.finite_vertices_end(); ++vit) {
		Vertex_circulator vc = D.incident_vertices(vit), done(vc);
		int idx = vit->info();
		std::vector< std::pair<Pt, int> > candidates;
		candidates.push_back(std::make_pair(vit->point(), vit->info()));
		do {
			if (!D.is_infinite(vc)) {
				int vcidx = vc->info();
				//if (vcidx < idx) continue;
				if (CGAL::squared_distance(vc->point(), vit->point()) > r2) continue;
				//boost::add_edge(idx, vcidx, G);
				candidates.push_back(std::make_pair(vc->point(), vcidx));
			}
		} while (++vc != done);
		int ncand = candidates.size();
		for (int i = 1; i < ncand; i++) {
			Pt pi = candidates[i].first;
			for (int j = i + 1; j < ncand; j++) {
				Pt pj = candidates[j].first;
				if (CGAL::squared_distance(pi, pj) <= r2) {
					violated = true;
					break;
				}
			}
		}
		if (violated) break;
		for (int j = 1; j < ncand; j++) {
			int idxj = candidates[j].second;
			boost::add_edge(idx, idxj, G);
		}
	}

	if (violated) {
		for (int j = 0; j < m; j++) {
			_o << "n";
		}
		_o << _l;
		_e << "violated" << _l;
		return;
	}

	bool isBipartite = boost::is_bipartite(G);
	if (!isBipartite) {
		for (int j = 0; j < m; j++) {
			_o << "n";
		}
		_o << _l;
		_e << "n" << _l;
		return;
	}

	boost::connected_components(G, &components[0]);
	for (int i = 0; i < n; i++) {
		_e << components[i] << " ";
	}_e << _l;
	for (int j = 0; j < m; j++) {
		Pt a = clues[j].first, b = clues[j].second;
		if (CGAL::squared_distance(a, b) <= r2) {
			_o << "y";
			continue;
		}
		Vertex_handle vha = D.nearest_vertex(a);
		Vertex_handle vhb = D.nearest_vertex(b);
		// not reachable to any station
		if (CGAL::squared_distance(vha->point(), a) > r2
			|| CGAL::squared_distance(vhb->point(), b) > r2) {
			_o << "n";
			continue;
		}
		// not in the same component
		int idxa = vha->info(), idxb = vhb->info();
		if (components[idxa] == components[idxb]) {
			_o << "y";
			//_e << vha->point() << "|" << vhb->point() << _l;
			//_e << components[idxa] << " " << components[idxb] << _l;
		}
		else _o << "n";
	}
	_o << _l;
	return;

}

// Week 13: Goldfinger / 100 pts
// LP + Delaunay + bin_search
typedef CGAL::Delaunay_triangulation_2<IK>		Delaunay;
typedef Delaunay::Finite_faces_iterator		Face_iterator;
typedef Delaunay::Finite_edges_iterator		Edge_iterator;
typedef Delaunay::Vertex_handle		Vertex_handle;
typedef Delaunay::Point		DPt;
bool isSolvable(int a, std::vector<Pt> &psensor, std::vector<int> &esensor,
		std::vector<Pt> &pmpe, std::vector<lint> &r2mpe, int Imax, std::vector< std::vector<lint> >& d2) {
	int n = psensor.size();
	int m = pmpe.size();
	bool isRestricted = r2mpe[0] > 0;

	Program lp(CGAL::LARGER, true, 0, false);
	for (int i = 0; i < n; i++) {
		for (int j = 0; j < a; j++) {
			//lint d2 = CGAL::squared_distance(pmpe[j], psensor[i]);
			if (!isRestricted || d2[i][j] < r2mpe[j])
				lp.set_a(j, i, (ET)1 / d2[i][j]);
		}
		lp.set_b(i, esensor[i]);
	}
	for (int j = 0; j < a; j++) {
		lp.set_a(j, n, -1);
		lp.set_c(j, 1);
	}
	lp.set_b(n, -Imax);

	Solution s = CGAL::solve_linear_program(lp, ET());
	if (s.is_optimal()) {
		//_o << a << _l;
		return true;
	}
	else {
		return false;
	}

}
void Goldfinger() {
	int n, m, h, Imax;
	_i >> n >> m >> h >> Imax;
	std::vector<Pt> psensor(n);
	std::vector<Pt> pmpe(m);
	std::vector<Pt> phench(h);
	std::vector<int> esensor(n);
	lint BIG = (1 << 55);
	std::vector<lint> r2mpe(m, -1);
	std::vector< std::vector<lint> > d2(n);

	for (int i = 0; i < n; i++) {
		int x, y, e; _i >> x >> y >> e;
		psensor[i] = Pt(x, y);
		esensor[i] = e;
	}
	for (int j = 0; j < m; j++) {
		int x, y; _i >> x >> y;
		pmpe[j] = Pt(x, y);
	}
	for (int k = 0; k < h; k++) {
		int x, y; _i >> x >> y;
		phench[k] = Pt(x, y);
	}

	Delaunay D;
	D.insert(phench.begin(), phench.end());
	if (h > 0) {
		for (int j = 0; j < m; j++) {
			Vertex_handle vh = D.nearest_vertex(pmpe[j]);
			r2mpe[j] = CGAL::squared_distance(vh->point(), pmpe[j]);
		}
	}

	for (int i = 0; i < n; i++) {
		d2[i] = std::vector<lint>(m);
		for (int j = 0; j < m; j++) {
			d2[i][j] = CGAL::squared_distance(psensor[i], pmpe[j]);
		}
	}

	if (isSolvable(1, psensor, esensor, pmpe, r2mpe, Imax, d2)) {
		_o << 1 << _l;
		return;
	}

	// exp. search for an interval
	std::vector<int> solvables(m + 1, 0); // 0 = not touched, 1 = solvable, -1 = unsolvable
										  //solvables[m] = 1;
	solvables[1] = -1;
	int st = 1, end = 2;
	while (end <= m) {
		_e << "end=" << end << _l;
		if (solvables[end] == 1 || isSolvable(end, psensor, esensor, pmpe, r2mpe, Imax, d2)) {
			solvables[end] = 1;
			break;
		}
		else {
			solvables[end] = -1;
			if (end == m) break;
			st = end;
			end = std::min(end * 2, m);
		}
	}
	if (solvables[m] == -1) {
		_o << "impossible" << _l;
		return;
	}

	// binary search for the best
	int low = st, up = end, a = (low + up) / 2;
	while (true) {
		_e << "a=" << a << _l;
		if (up - low <= 1) {
			if (solvables[low] == 1 || isSolvable(low, psensor, esensor, pmpe, r2mpe, Imax, d2)) {
				_o << low << _l;
				return;
			}
			if (solvables[up] == 1 || isSolvable(up, psensor, esensor, pmpe, r2mpe, Imax, d2)) {
				_o << up << _l;
				return;
			}
			break; // impossible
		}

		bool solvable = isSolvable(a, psensor, esensor, pmpe, r2mpe, Imax, d2);
		if (solvable) {
			solvables[a] = 1;
			up = a;
			a = (low + up) / 2;
		}
		else {
			solvables[a] = -1;
			low = a;
			a = (low + up) / 2;
		}

	}

	_o << "impossible" << _l;
	return;

}

// Week 3: First Hit / 99->100
// For 5555 pts total!
// OMG really?? random shuffle??
double floor_to_double_EK(const EK::FT& x)
{
	double a = std::floor(CGAL::to_double(x));
	while (a > x) a -= 1;
	while (a + 1 <= x) a += 1;
	return a;
}
void FirstHit(int n) {

	lint x, y, a, b, r, s, t, u;
	std::cin >> x >> y >> a >> b;
	IK::Point_2 p1(x, y), p2(a, b);
	IK::Ray_2 ray(p1, p2);
	EK::Point_2 ep1((double)x, (double)y), ep2((double)a, (double)b);
	EK::Ray_2 eray(ep1, ep2);
	EK::Segment_2 eseg;

	bool ishit = false;
	EK::FT minsqd = 5e100;
	EK::Point_2 minits(0, 0), curits(0, 0);

	std::vector<EK::Segment_2> esegs(n);
	for (int i = 0; i < n; i++) {
		_i >> r >> s >> t >> u;
		EK::Point_2 eq1((double)r, (double)s), eq2((double)t, (double)u);
		EK::Segment_2 esegq(eq1, eq2);
		esegs[i] = esegq;
	}
	std::random_shuffle(esegs.begin(), esegs.end());

	for (int i = 0; i < n; i++) {
		EK::Segment_2 esegq = esegs[i];
		EK::Point_2 eq1 = esegq.start(), eq2 = esegq.end();
		if (ishit) {
			if (CGAL::do_intersect(eseg, esegq)) {
				auto result = CGAL::intersection(eseg, esegq);

				if (const EK::Segment_2* sp = boost::get<EK::Segment_2>(&*result)) {
					_e << "segment" << _l;
					EK::FT sqd1 = CGAL::squared_distance(ep1, eq1);
					EK::FT sqd2 = CGAL::squared_distance(ep1, eq2);
					if (sqd1 < sqd2) {
						minits = eq1;
					}
					else {
						minits = eq2;
					}
				}
				else if (const EK::Point_2* pp = boost::get<EK::Point_2>(&*result)) {
					_e << "point" << _l;
					minits = *pp;
				}
				eseg = EK::Segment_2(ep1, minits);
			}
		}
		else {
			if (CGAL::do_intersect(eray, esegq)) {
				auto result = CGAL::intersection(eray, esegq);
				if (const EK::Segment_2* sp = boost::get<EK::Segment_2>(&*result)) {
					_e << "segment0" << _l;
					EK::FT sqd1 = CGAL::squared_distance(ep1, eq1);
					EK::FT sqd2 = CGAL::squared_distance(ep1, eq2);
					if (sqd1 < sqd2) {
						minits = eq1;
					}
					else if (sqd2 < sqd1) {
						minits = eq2;
					}
				}
				else if (const EK::Point_2* pp = boost::get<EK::Point_2>(&*result)) {
					minits = *pp;
				}
				eseg = EK::Segment_2(ep1, minits);
				ishit = true;
			}
		}
	}

	if (!ishit) {
		std::cout << "no" << std::endl;
		return;
	}
	std::cout << (lint)floor_to_double_EK(minits.x()) << " " << (lint)floor_to_double_EK(minits.y()) << std::endl;

	return;
}

// Week 13, PotW: World Cup / 100 pts
// LP + Triangulation
typedef CGAL::Delaunay_triangulation_2<IK>		Delaunay;
typedef Delaunay::Finite_faces_iterator		Face_iterator;
typedef Delaunay::Finite_edges_iterator		Edge_iterator;
typedef Delaunay::Vertex_handle		Vertex_handle;
typedef Delaunay::Point		DPt;
struct Warehouse {
	Pt p;
	int s;
	int a;
	Warehouse(int x = 0, int y = 0, int s = -1, int ac = -1) :s(s), a(ac) {
		p = Pt(x, y);
	}
};
struct Stadium {
	Pt p;
	int d;
	int u;
	Stadium(int x = 0, int y = 0, int d = -1, int u = -1) :d(d), u(u) {
		p = Pt(x, y);
	}
};
struct Contour {
	Pt p;
	int r;
	Contour(int x = 0, int y = 0, int r = 0) : r(r) {
		p = Pt(x, y);
	}
};
void WorldCup() {
	int n, m, c; _i >> n >> m >> c;
	std::vector<Warehouse> whs(n);
	std::vector<Stadium> sts(m);
	std::vector<Contour> cts; // not initialized -- need to choose!

	std::vector<Pt> whst;
	for (int i = 0; i < n; i++) {
		int x, y, s, a; _i >> x >> y >> s >> a;
		whs[i] = Warehouse(x, y, s, a);
		whst.push_back(Pt(x, y));
	}
	for (int j = 0; j < m; j++) {
		int x, y, d, u; _i >> x >> y >> d >> u;
		sts[j] = Stadium(x, y, d, u);
		whst.push_back(Pt(x, y));
	}
	int r[200][20] = {}, t[200][20] = {};
	for (int i = 0; i < n; i++) {
		for (int j = 0; j < m; j++) {
			_i >> r[i][j];
		}
	}

	Delaunay D;
	D.insert(whst.begin(), whst.end());

	for (int k = 0; k < c; k++) {
		lint x, y, rad;
		_i >> x >> y >> rad;
		Pt p(x, y);
		Vertex_handle vh = D.nearest_vertex(p);
		double d2 = CGAL::squared_distance(p, vh->point());
		if (d2 < rad*rad) {
			cts.push_back(Contour(x, y, rad));
		}
	}

	int uc = cts.size(); // used c
	for (int k = 0; k < uc; k++) {
		lint r2 = cts[k].r * cts[k].r;
		for (int i = 0; i < n; i++) {
			if (CGAL::squared_distance(whs[i].p, cts[k].p) < r2)
				for (int j = 0; j < m; j++)
					t[i][j] += (CGAL::squared_distance(sts[j].p, cts[k].p) > r2);
			else
				for (int j = 0; j < m; j++)
					t[i][j] += (CGAL::squared_distance(sts[j].p, cts[k].p) < r2);
		}
	}

	// form LP, n*m variables, n+m*2 constraints
	Program lp(CGAL::SMALLER, true, 0, false, 0);
	for (int i = 0; i < n; i++) {
		lp.set_b(i, whs[i].s);
		for (int j = 0; j < m; j++) {
			lp.set_a(i*m + j, i, 1);
		}
	}
	for (int j = 0; j < m; j++) {
		lp.set_b(n + j, -sts[j].d);
		lp.set_b(n + 2 * m + j, sts[j].d);
		lp.set_b(n + m + j, sts[j].u * 100);
		for (int i = 0; i < n; i++) {
			lp.set_a(i*m + j, n + j, -1);
			lp.set_a(i*m + j, n + 2 * m + j, 1);
			lp.set_a(i*m + j, n + m + j, whs[i].a);
		}
	}
	for (int i = 0; i < n; i++) {
		for (int j = 0; j < m; j++) {
			int idx = i*m + j;
			lp.set_c(idx, -100 * r[i][j] + t[i][j]);
		}
	}

	Solution s = CGAL::solve_linear_program(lp, ET());

	if (s.is_optimal()) {
		ET result = s.objective_value().numerator() / (100 * s.objective_value().denominator());
		lint llr = floor_to_double(-result);
		_o << llr << _l;
	}
	else {
		_o << "RIOT!" << _l;
	}
	return;
}

// Week 10: Light the Stage
// Delaunay Triangulation + binary search
typedef CGAL::Delaunay_triangulation_2<IK>		Delaunay;
typedef Delaunay::Finite_faces_iterator		Face_iterator;
typedef Delaunay::Finite_edges_iterator		Edge_iterator;
typedef Delaunay::Vertex_handle		Vertex_handle;
typedef Delaunay::Point		DPt;
struct Participant {
	Pt p;
	lint r;
	Participant(int x = 0, int y = 0, lint r = 0) :p(Pt(x, y)), r(r) {}
	Participant(Pt p, lint r = 0) :p(p), r(r) {}
};
void binarySearchLeftover(std::vector<Participant> &ps, std::vector<Pt> &lights, int h, std::vector<int> &leftover) {
	int m = ps.size(), n = lights.size();
	Delaunay D;
	D.clear();
	int start = 1, end = n - 1, mid = (start + end) / 2;
	std::vector<bool> aliveAfterMid(m, true);
	std::vector<int> tmpLeftOver;
	while (true) {
		if (end - start <= 1) break;
		D.clear();
		tmpLeftOver.clear();
		bool alive = false;
		D.insert(lights.begin(), lights.begin() + mid);
		for (int i = 0; i < m; i++) {
			Vertex_handle vh = D.nearest_vertex(ps[i].p);
			lint d2 = CGAL::squared_distance(vh->point(), ps[i].p);
			if (d2 >= (ps[i].r + h)*(ps[i].r + h)) {
				// existing one living participant: update the interval
				start = mid;
				mid = (start + end) / 2;
				alive = true;
				break;
			}
		}
		if (!alive) {
			end = mid - 1;
			mid = (start + end) / 2;
		}
	}
	_e << "mid: " << mid << _l;
	D.clear();
	D.insert(lights.begin(), lights.begin() + mid);

	if (mid < end) {
		std::vector<int> endleftover;
		D.clear();
		D.insert(lights.begin(), lights.begin() + end);
		for (int i = 0; i < m; i++) {
			Vertex_handle vh = D.nearest_vertex(ps[i].p);
			lint d2 = CGAL::squared_distance(vh->point(), ps[i].p);
			if (d2 >= (ps[i].r + h)*(ps[i].r + h)) {
				// existing one living participant: update the interval
				endleftover.push_back(i);
			}
		}
		if (!endleftover.empty()) {
			leftover.clear();
			leftover.insert(leftover.begin(), endleftover.begin(), endleftover.end());
		}
	}
	return;
}
void LightTheStage() {
	int m, n, h; _i >> m >> n;
	_e << m << " " << n << _l;
	std::vector<Participant> ps(m);
	for (int i = 0; i < m; i++) {
		int x, y, r;
		_i >> x >> y >> r;
		ps[i] = Participant(x, y, r);
	}
	_i >> h;
	std::vector<Pt> lights(n);
	for (int j = 0; j < n; j++) {
		int x, y; _i >> x >> y;
		lights[j] = Pt(x, y);
	}

	Delaunay D;
	D.insert(lights.begin(), lights.end());

	std::vector<int> leftover;
	for (int i = 0; i < m; i++) {
		Vertex_handle vh = D.nearest_vertex(ps[i].p);
		lint d2 = CGAL::squared_distance(vh->point(), ps[i].p);
		if (d2 >= (ps[i].r + h)*(ps[i].r + h)) {
			leftover.push_back(i);
		}
	}
	if (!leftover.empty()) {
		int l = leftover.size();
		for (int i = 0; i < l; i++) {
			_o << leftover[i] << " ";
		}
	}
	else {
		binarySearchLeftover(ps, lights, h, leftover);
		int l = leftover.size();
		for (int i = 0; i < l; i++) {
			_o << leftover[i] << " ";
		}
	}
	_o << _l;
	return;
}

// week 8: Bistro (nearest_vertex)
void Bistro(int n) {
	typedef CGAL::Delaunay_triangulation_2<IK>		Delaunay;
	typedef Delaunay::Finite_faces_iterator		Face_iterator;
	typedef Delaunay::Finite_edges_iterator		Edge_iterator;
	typedef Delaunay::Vertex_handle		Vertex_handle;
	typedef Delaunay::Point		DPt;

	//int n, m; _i >> n;
	std::vector<Pt> existing(n);
	for (int i = 0; i < n; i++) {
		int x, y;
		_i >> x >> y;
		existing[i] = Pt(x, y);
	}
	int m; _i >> m;
	std::vector<Pt> possible(m);
	for (int j = 0; j < m; j++) {
		int x, y;
		_i >> x >> y;
		possible[j] = Pt(x, y);
	}

	Delaunay D;
	D.insert(existing.begin(), existing.end());

	for (int j = 0; j < m; j++) {
		Vertex_handle vh = D.nearest_vertex(possible[j]);
		lint d2 = CGAL::squared_distance(vh->point(), possible[j]);
		_o << d2 << _l;
	}

	return;
}

// Week 11: Strikes Back
// Delaunay triangulation for r
// LP for e
void StrikesBack() {
	typedef CGAL::Delaunay_triangulation_2<IK>		Delaunay;
	typedef Delaunay::Finite_faces_iterator		Face_iterator;
	typedef Delaunay::Finite_edges_iterator		Edge_iterator;
	typedef Delaunay::Vertex_handle		Vertex_handle;
	typedef Delaunay::Point		DPt;

	struct Asteroid {
		Pt pos;
		int d;
		Asteroid(Pt p = Pt(), int d = 0) : pos(p), d(d) {}
	};

	int a, s, b;
	_i >> a >> s >> b;
	int emax;
	_i >> emax;

	std::vector<Asteroid> asters(a);
	std::vector<Pt> shotpos(s); // model r and e outside.
	std::vector<Pt> bounties(b);

	for (int i = 0; i < a; i++) {
		int x, y, d;
		_i >> x >> y >> d;
		asters[i] = Asteroid(Pt(x, y), d);
	}
	for (int i = 0; i < s; i++) {
		int x, y;
		_i >> x >> y;
		shotpos[i] = Pt(x, y);
	}
	for (int i = 0; i < b; i++) {
		int x, y;
		_i >> x >> y;
		bounties[i] = DPt(x, y);
	}

	// triangulate bounties
	Delaunay D;
	D.insert(bounties.begin(), bounties.end());

	_e << "delaunay" << _l;

	std::vector<lint> shotradsq(s);
	if (b > 0) {
		for (int i = 0; i < s; i++) {
			DPt dpt(shotpos[i]);
			Vertex_handle vh = D.nearest_vertex(shotpos[i]);
			shotradsq[i] = CGAL::squared_distance(shotpos[i], vh->point());
			if (shotradsq[i] < 1) {
				shotradsq[i] = 1;
			}
		}
		_e << "shotradsq" << _l;
	}

	// set up LP to solve for min\Sum_i e_i
	Program lp(CGAL::LARGER, true, 0, true, emax); // Ax >= b

	for (int i = 0; i < a; i++) {
		lp.set_b(i, asters[i].d);
		for (int j = 0; j < s; j++) {
			lint sqdistij = CGAL::squared_distance(asters[i].pos, shotpos[j]);
			if (b == 0 || sqdistij <= shotradsq[j]) {
				// in shot range
				lp.set_a(j, i, ET(1) / sqdistij); // will it work?
			}
		}
	}
	for (int j = 0; j < s; j++) {
		lp.set_c(j, 1);
	}

	_e << "lpsetup" << _l;

	// solve the program, using ET as the exact type
	Solution solution = CGAL::solve_linear_program(lp, ET());
	assert(solution.solves_linear_program(lp));
	if (solution.is_infeasible()) {
		_o << "n" << _l;
		return;
	}
	else {
		ET eoptimal = solution.objective_value().numerator() / solution.objective_value().denominator();
		if (eoptimal <= emax) {
			_o << "y" << _l;
		}
		else {
			_o << "n" << _l;
		}
	}
	return;

}

// Week 12 PotW: Golden Eye / 100pts
// Euclidean (Kruskal) MST + Union Find for connectivity
// Cautions(I am not good today...):
// 1. check connectivity before adding any edges
// 2. it could be that the distance from queries to its nearest_vertex dominates the distance
// 3. beware of the order of setting a variable and using its value......
// 4. set everything to _lint_....
lint minLevelSet(Graph& G, std::vector<Edge>& mst, std::vector<int>& s, std::vector<int> &t, std::vector<lint> &maxd2, lint maxp) {
	int n = boost::num_vertices(G);
	int m = s.size();
	WeightMap wmap = boost::get(boost::edge_weight, G);

	UnionFind uf(n);
	lint start = 0;
	std::vector<bool> isconnected(m, false);
	bool allconn = true;
	for (int j = 0; j < m; j++) {
		start = std::max(start, maxd2[j]);
		isconnected[j] = uf.inSameSet(s[j], t[j]);
		if (!isconnected[j]) allconn = false;
	}
	if (allconn || start > maxp) {
		return start;
	}

	std::vector<Edge>::iterator eit = mst.begin();

	for (; eit != mst.end(); eit++) {
		if (wmap[*eit] > maxp) return -1;
		int u = boost::source(*eit, G);
		int v = boost::target(*eit, G);
		uf.unionSets(u, v);
		if (eit == mst.end() - 1 || wmap[*(eit + 1)] >= start) {
			bool allconnected = true;
			for (int j = 0; j < m; j++) {
				if (isconnected[j]) continue;
				if (uf.inSameSet(s[j], t[j]))
					isconnected[j] = true;
				if (!isconnected[j])
					allconnected = false; // isconnected[j] could have been altered!
			}
			if (allconnected)
				return std::max(start, (lint)wmap[*eit]);
		}
	}
	return -1; // never happen
}
void GoldenEye2() {
	typedef CGAL::Triangulation_vertex_base_with_info_2<ind_t, IK>	Vb;
	typedef CGAL::Triangulation_data_structure_2<Vb>	Tds;
	typedef CGAL::Delaunay_triangulation_2<IK, Tds>		Delaunay;
	typedef Delaunay::Finite_faces_iterator		Face_iterator;
	typedef Delaunay::Finite_edges_iterator		Edge_iterator;
	typedef Delaunay::Vertex_handle		Vertex_handle;
	typedef Delaunay::Point		DPt;

	lint n, m, p;
	_i >> n >> m >> p;
	std::vector< std::pair<DPt, int> > jams(n);
	std::vector<Pt> ss(m);
	std::vector<Pt> ts(m);

	Graph G(n); // undirected graph
	WeightMap weight = boost::get(boost::edge_weight, G);

	int x, y;
	for (int i = 0; i < n; i++) {
		_i >> x >> y;
		jams[i] = std::make_pair(DPt(x, y), i);
	}
	for (int j = 0; j < m; j++) {
		int x0, y0, x1, y1;
		_i >> x0 >> y0 >> x1 >> y1;
		ss[j] = Pt(x0, y0);
		ts[j] = Pt(x1, y1);
	}

	Delaunay D;
	D.insert(jams.begin(), jams.end());

	// Euclid MST
	for (Edge_iterator ei = D.finite_edges_begin(); ei != D.finite_edges_end(); ++ei) {
		Vertex_handle v1 = ei->first->vertex((ei->second + 1) % 3);
		Vertex_handle v2 = ei->first->vertex((ei->second + 2) % 3);
		int i1 = v1->info();
		int i2 = v2->info();
		lint dist = CGAL::squared_distance(v1->point(), v2->point());
		//_e << i1 << " " << i2 << ":" << dist << _l;
		bool success; Edge e;
		boost::tie(e, success) = boost::add_edge(i1, i2, G);
		if (success) weight[e] = dist;
	}

	std::vector<Edge> spanning_tree;
	boost::kruskal_minimum_spanning_tree(G, std::back_inserter(spanning_tree));

	std::vector<lint> ds(m), dt(m), maxpow(m);
	std::vector<Vertex_handle> vs(m), vt(m);
	std::vector<int> visited(m);
	std::vector<bool> pset(m, true);
	for (int j = 0; j < m; j++) {
		Pt s = ss[j], t = ts[j];
		vs[j] = D.nearest_vertex(s), vt[j] = D.nearest_vertex(t);
		ds[j] = CGAL::squared_distance(vs[j]->point(), s);
		dt[j] = CGAL::squared_distance(vt[j]->point(), t);
		maxpow[j] = 4 * std::max(ds[j], dt[j]);
		visited[j] = 0;
		if (maxpow[j] > p) pset[j] = false;
	}

	// check power level p
	Graph MSTP(n);
	std::vector<Edge>::iterator eit;
	lint maxwinmst = weight[spanning_tree[spanning_tree.size() - 1]]; // max weight in the MST
	for (eit = spanning_tree.begin(); eit != spanning_tree.end(); ++eit) {
		if (weight[*eit] > p) break;
		Vertex u = boost::source(*eit, G);
		Vertex v = boost::target(*eit, G);
		boost::add_edge(u, v, MSTP);
	}

	std::vector<int> components(n);
	boost::connected_components(MSTP, &components[0]);
	std::vector<int> p_s, p_t, all_s, all_t;
	std::vector<lint> p_maxd2, all_maxd2;
	for (int j = 0; j < m; j++) {
		if (!pset[j]) continue;
		int sidx = vs[j]->info(), tidx = vt[j]->info();
		if (components[sidx] != components[tidx]) {
			pset[j] = false;
		}
		if (pset[j]) {
			p_s.push_back(sidx);
			p_t.push_back(tidx);
			p_maxd2.push_back(maxpow[j]);
		}
	}
	for (int j = 0; j < m; j++) {
		all_s.push_back(vs[j]->info());
		all_t.push_back(vt[j]->info());
		all_maxd2.push_back(maxpow[j]);
	}

	lint b = minLevelSet(G, spanning_tree, p_s, p_t, p_maxd2, p);
	lint a = minLevelSet(G, spanning_tree, all_s, all_t, all_maxd2, maxwinmst);

	for (int j = 0; j < m; j++) {
		if (pset[j]) _o << "y";
		else _o << "n";
	}
	_o << _l;
	_o << a << _l;
	_o << b << _l;
}
// Week 12 PotW: Golden Eye / 75pts
// called uf.inSameSet() too many times
void GoldenEye() {

	typedef CGAL::Triangulation_vertex_base_with_info_2<ind_t, IK>	Vb;
	typedef CGAL::Triangulation_data_structure_2<Vb>	Tds;
	typedef CGAL::Delaunay_triangulation_2<IK, Tds>		Delaunay;
	typedef Delaunay::Finite_faces_iterator		Face_iterator;
	typedef Delaunay::Finite_edges_iterator		Edge_iterator;
	typedef Delaunay::Vertex_handle		Vertex_handle;
	typedef Delaunay::Point		DPt;

	lint n, m, p;
	_i >> n >> m >> p;
	std::vector< std::pair<DPt, int> > jams(n);
	std::vector<Pt> ss(m);
	std::vector<Pt> ts(m);

	Graph G(n);
	WeightMap weight = boost::get(boost::edge_weight, G);

	int x, y;
	for (int i = 0; i < n; i++) {
		_i >> x >> y;
		jams[i] = std::make_pair(DPt(x, y), i);
	}
	for (int j = 0; j < m; j++) {
		int x0, y0, x1, y1;
		_i >> x0 >> y0 >> x1 >> y1;
		ss[j] = Pt(x0, y0);
		ts[j] = Pt(x1, y1);
	}

	Delaunay D;
	D.insert(jams.begin(), jams.end());

	// Euclid MST
	for (Edge_iterator ei = D.finite_edges_begin(); ei != D.finite_edges_end(); ++ei) {
		Vertex_handle v1 = ei->first->vertex((ei->second + 1) % 3);
		Vertex_handle v2 = ei->first->vertex((ei->second + 2) % 3);
		int i1 = v1->info();
		int i2 = v2->info();
		double dist = CGAL::squared_distance(v1->point(), v2->point());
		//_e << i1 << " " << i2 << ":" << dist << _l;
		bool success; Edge e;
		boost::tie(e, success) = boost::add_edge(i1, i2, G);
		if (success) weight[e] = dist;
	}

	std::vector<Edge> spanning_tree;
	boost::kruskal_minimum_spanning_tree(G, std::back_inserter(spanning_tree));

	double a = 0, b = -1;
	std::vector<double> ds(m), dt(m), maxpow(m);
	std::vector<Vertex_handle> vs(m), vt(m);
	std::vector<int> visited(m);
	for (int j = 0; j < m; j++) {
		Pt s = ss[j], t = ts[j];
		vs[j] = D.nearest_vertex(s), vt[j] = D.nearest_vertex(t);
		ds[j] = CGAL::squared_distance(vs[j]->point(), s);
		dt[j] = CGAL::squared_distance(vt[j]->point(), t);
		maxpow[j] = 4 * std::max(ds[j], dt[j]);
		visited[j] = 0;
	}

	UnionFind uf(n);
	std::vector<Edge>::iterator ei;
	double w = 0, wlastchanged = 0;
	bool changed = false;
	for (int j = 0; j < m; j++) {
		if (uf.inSameSet(vs[j]->info(), vt[j]->info())) {
			// effectively vs[j]->info() == vt[j]->info()
			visited[j] = 1;
			changed = true;
		}
	}

	// Gradually add edges of the MST
	for (ei = spanning_tree.begin(); ei != spanning_tree.end(); ++ei) {
		Vertex v1 = boost::source(*ei, G), v2 = boost::target(*ei, G);
		uf.unionSets(v1, v2);
		w = weight[*ei];
		// if (w > p && b < 0) b = wlastchanged;
		changed = false;
		bool allvisited = true;
		for (int j = 0; j < m; j++) {
			if (!visited[j]) allvisited = false;
			if (!visited[j] && uf.inSameSet(vs[j]->info(), vt[j]->info())) {
				visited[j] = 1;
				changed = true;
				maxpow[j] = std::max(w, maxpow[j]);
			}
		}
		if (changed) wlastchanged = w;
		if (allvisited) break;
	}
	//if (w > p && b < 0) b = wlastchanged;
	if (b < 0) b = 0;

	for (int j = 0; j < m; j++) {
		a = std::max(a, maxpow[j]);
		//if (!visited[j]) _e << vs[j]->info() << "?" << vt[j]->info() << _l;
		//_e << maxpow[j] << " ";
		if (maxpow[j] > p) visited[j] = 2;
		else {
			if (b < maxpow[j]) b = maxpow[j];
		}
	}
	_e << _l;

	for (int j = 0; j < m; j++) {
		_o << (visited[j] == 1 ? "y" : "n");
	}
	_o << _l;
	_o << (lint)std::ceil(a) << _l;
	_o << (lint)std::ceil(b) << _l;
	return;

}

// typedef CGAL:Gmpz ET;
void Inball(int n, int d) {
	Program lp(CGAL::SMALLER, false, 0, false, 0);
	int c[10] = {};
	for (int i = 0; i < d; i++) c[i] = i;
	const int r = d;

	int a = 0, b = 0;
	for (int i = 0; i < n; i++) {
		double norm = 0;
		for (int j = 0; j < d; j++) {
			_i >> a;
			lp.set_a(c[j], i, a);
			norm += a*a;
		}
		lp.set_a(r, i, std::round(std::sqrt(norm)));
		_i >> b;
		lp.set_b(i, b);
	}
	lp.set_l(r, true, 0);
	lp.set_c(r, -1);

	// solve the program, using ET as the exact type
	Solution s = CGAL::solve_linear_program(lp, ET());
	assert(s.solves_linear_program(lp));

	if (s.is_optimal()) {
		CGAL::Quadratic_program_solution<ET>::Variable_value_iterator
			opt = s.variable_values_begin();
		CGAL::Quotient<ET> rad = *(opt + d);
		_o << (int)floor_to_double(rad) << _l;
	}
	else if (s.is_unbounded()) {
		_o << "inf" << _l;
	}
	else if (s.is_infeasible()) {
		_o << "none" << _l;
	}
	return;
}

void Lestrade() {

	typedef CGAL::Triangulation_vertex_base_with_info_2<ind_t, IK>	Vb;
	typedef CGAL::Triangulation_data_structure_2<Vb>	Tds;
	typedef CGAL::Delaunay_triangulation_2<IK, Tds>		Delaunay;
	typedef Delaunay::Finite_faces_iterator		Face_iterator;
	typedef Delaunay::Finite_edges_iterator		Edge_iterator;
	typedef Delaunay::Vertex_handle		Vertex_handle;
	typedef Delaunay::Point		DPt;

	struct Gangster {
		int u;
		int v;
		int w;
		int ag;
		Gangster(int uu = 0, int vv = 0, int ww = 0, int a = -1) : u(uu), v(vv), w(ww), ag(a) {}
	};

	int Z, U, V, W;
	int A, G;
	_i >> Z >> U >> V >> W >> A >> G;

	std::vector<std::pair<DPt, ind_t> > pts(G);
	std::vector<Gangster> gang(G);
	for (ind_t i = 0; i < G; i++) {
		int x, y, u, v, w;
		_i >> x >> y >> u >> v >> w;
		DPt pg(x, y);
		pts[i] = std::make_pair(pg, i);
		gang[i] = Gangster(u, v, w);
	}
	Delaunay T;
	T.insert(pts.begin(), pts.end());
	//_e << gang.size() << _l;

	std::vector<int> ha(A);
	std::vector<int> spect(A);
	for (ind_t i = 0; i < A; i++) {
		int x, y, h;
		_i >> x >> y >> h;
		ha[i] = h;
		DPt pa(x, y);
		Vertex_handle vh = T.nearest_vertex(pa);
		ind_t gind = vh->info();
		ind_t aind = gang[gind].ag;
		//_e << "gind" << gind << "; aind" << aind << _l;

		if (aind < 0 || (aind >= 0 && ha[aind] > h)) {
			// the gangster has not been inspected by others
			// or he has been inspected by another more expensive agent
			// --> I will do that instead!
			spect[i] = gind;
			gang[gind].ag = i;
			if (aind >= 0) {
				spect[aind] = -1;
			}
		}
		else {
			spect[i] = -1;
		}
	}
	//_e << ha.size() << _l;

	std::vector<int> hawo;
	std::vector<Gangster> gangspected;
	for (int i = 0; i < A; i++) {
		if (spect[i] >= 0) {
			hawo.push_back(ha[i]);
			gangspected.push_back(gang[spect[i]]);
		}
	}
	int nworking = hawo.size();

	/// set up the LP problem
	Program lp(CGAL::LARGER, true, 0, true, 24.f);
	for (ind_t j = 0; j < nworking; j++) {
		int z = hawo[j];
		Gangster gs = gangspected[j];
		//_e << z << " " << gs.u << " " << gs.v << " " << gs.w << _l;
		lp.set_c(j, -gs.u);
		lp.set_a(j, 0, -z);
		lp.set_a(j, 1, gs.v);
		lp.set_a(j, 2, gs.w);
	}
	lp.set_b(0, -Z);
	lp.set_b(1, V);
	lp.set_b(2, W);

	// solve the program, using ET as the exact type
	Solution s = CGAL::solve_linear_program(lp, ET());
	assert(s.solves_linear_program(lp));

	if (s.is_optimal()) {
		ET optim = s.objective_value().numerator() / s.objective_value().denominator();
		//_e << "optim:" << optim.to_double() << _l;
		if (optim > -U)
			_o << "H" << _l;
		else
			_o << "L" << _l;
	}
	else if (s.is_unbounded()) {
		_o << "L" << _l;
	}
	else if (s.is_infeasible()) {
		_o << "H" << _l;
	}
}


#include "integration.hpp"
#include <iostream>
#include <Eigen/Sparse>
#include <Eigen/IterativeLinearSolvers>
using namespace std;

void explicitEulerStep(PhysicalSystem *system, double dt) {
    forwardEulerStep(system, dt);
}

void forwardEulerStep(PhysicalSystem *system, double dt) {
    int n = system->getDOFs();
    VectorXd x0(n), v0(n);
    system->getState(x0, v0);
    MatrixXd M(n,n);
    system->getInertia(M);
    VectorXd f0(n);
    system->getForces(f0);
    VectorXd a0(n); // acceleration
    for (int i = 0; i < n; i++)
        a0(i) = f0(i)/M(i,i);
    VectorXd x1 = x0 + v0*dt;
    VectorXd v1 = v0 + a0*dt;
    system->setState(x1, v1);
}


/*
 * Copyright 2016 Erik Crevel
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef VARDISTRIBUTION_HPP
#define VARDISTRIBUTION_HPP

#include <vector>
#include <tuple>
#include <utility>
#include <ostream>
#include <boost/multi_array.hpp>

class ArithmeticEncoder;
class ArithmeticDecoder;

class VarDistribution
{
	std::vector<std::pair<int, double>> curvePoints;

	void addPoint(int _i, double _sigma);

public:
	VarDistribution() {}
	template <class TContainer>
	VarDistribution(const TContainer& _counts);

	std::pair<double, double> getDist(unsigned char _val) const;

	void encodeDist(std::ostream& _out) const;
	void decodeDist(std::istream& _in);

	class Calculator
	{
		VarDistribution& dist;

		boost::multi_array<int,2> counts;

	public:
		Calculator(VarDistribution& _dist) :
			dist(_dist),
			counts(boost::extents[256][256])
		{}

		void add(int _val, unsigned char _v);

		void calculate();
	};
};

namespace
{

template <class TContainer>
double variance(const TContainer& _counts)
{
	int n = 0;
	double sumX = 0;
	double sumXX = 0;

	for(int x = 0; x < _counts.size(); ++x)
	{
		n += _counts[x];
		sumX += x*_counts[x];
		sumXX += x*x*_counts[x];
	}

	return (sumXX - sumX*sumX/n)/n;
}

}

template <class TContainer>
VarDistribution::VarDistribution(const TContainer& _counts)
{
	const int CURVE_SEGMENT_SIZE = 1024;

	std::vector<std::pair<int, double>> distVars(256);
	for(int varI = 0; varI < _counts.size(); ++varI)
	{
		const auto& subCounts = _counts[varI];

		int subTotal = std::accumulate(subCounts.begin(), subCounts.end(), 0);

		distVars[varI] = {subTotal, std::sqrt(variance(subCounts))};
	}

	int total = 0;
	int weightedI = 0;
	double weightedSigma = 0;
	for(int varI = 0; varI < distVars.size(); ++varI)
	{
		if(distVars[varI].first > 0)
		{
			total += distVars[varI].first;
			weightedI += distVars[varI].first * varI;
			weightedSigma += distVars[varI].first * distVars[varI].second;

			if(total >= CURVE_SEGMENT_SIZE)
			{
				this->addPoint((weightedI+total/2)/total, weightedSigma/total);

				total = 0;
				weightedI = 0;
				weightedSigma = 0;
			}
		}
	}

	if(total > 0)
		this->addPoint((weightedI+total/2)/total, weightedSigma/total);
}

#endif // VARDISTRIBUTION_HPP


#include <boost/graph/bellman_ford_shortest_paths.hpp>


#include "PCE.h"
#include "GeomCommonFunctions.h"
#include <Gui/Application.h>
#include <BRepBuilderAPI_GTransform.hxx>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <tchar.h>
#include <iostream>

using namespace PartDesignGui;

PCEArrange::
PCEArrange(
	int index_,
	int wlc_index_,
	int wl_index_,
	const std::vector<PCEPosition> & positions_,
	const std::vector<PCEEdge> & cutting_lines_,
	const double &volume_rate_) :
	index(index_),
	cutting_number(cutting_lines_.size()),
	wlc_index(wlc_index_),
	wl_index(wl_index_),
	positions(positions_),
	cutting_lines(cutting_lines_),
	volume_rate(volume_rate_),
	valid(false),
	eclass_index(-1),
	perfect_stock(false)
{
	Encode();
}

void PCEArrange::Encode()
{
	shape_order.clear();

	//encode
	Vector2d1 pos_centers;
	for (auto position : positions)
	{
		pos_centers.emplace_back(position.center);
		shape_order.emplace_back(position.transform->pce_shape->index);
	}

	Vector2d pos_center = Math::Functs::GetCenter(pos_centers);
	for (auto& center : pos_centers)center = center - pos_center;

	//Math::Functions::Vector2d3d();
	auto M = Math::Functs::RotationMatrix(Vector3d(0.0, 0.0, 1.0), Math::Math_PI);
	auto r_pos_centers = Math::Functs::Vector3d2d(Math::Functs::PosApplyM(Math::Functs::Vector2d3d(pos_centers), M));
	auto r_shape_order = shape_order;
	std::reverse(r_shape_order.begin(), r_shape_order.end());

	encode_0 = Vector2dString(pos_centers, shape_order, wl_index);
	encode_1 = Vector2dString(r_pos_centers, r_shape_order, wl_index);
	encode_packing_0 = Vector2dString(pos_centers, shape_order, -1);
	encode_packing_1 = Vector2dString(r_pos_centers, r_shape_order, -1);
}

PCEArrange::PCEArrange():
index(-1), eclass_index(-1), cutting_number(-1), wl_index(-1), wlc_index(-1), valid(false),volume_rate(0), perfect_stock(false)
{
}

void PCEArrange::Clear()
{
	for (auto& position : positions)position.Clear();
	std::vector<PCEPosition>().swap(positions);
	std::vector<PCEEdge>().swap(cutting_lines);
	std::vector<int>().swap(shape_order);
	Vector1i2().swap(cutting_orders);
	std::vector<PCEConstraint>().swap(sharing_constraints);
	std::vector<PCEEdge>().swap(edges);

	std::vector<std::vector<std::tuple<std::list<std::string>,int>>>().swap(prog_strs);
}

Vector3d3 PCEArrange::GetMesh(Vector3d2& surfs) const
{
	Vector3d3 all_points;
	for (auto position : positions)
		all_points.emplace_back(Math::Functs::PosApplyM(position.transform->surfs, position.M));
	return all_points;
}

std::string PCEArrange::Vector2dString(Vector2d1 ps, std::vector<int> ints, int wl_index_)
{
	const auto Comp = [](const Vector2d& a, const Vector2d& b)
	{
		return a[0] < b[0];
	};

	std::sort(ps.begin(), ps.end(), Comp);
	std::string str;
	for (auto p : ps)
	{
		double x = floor(p[0] * 10.0f + 0.5) / 10.0f;
		double y = floor(p[1] * 10.0f + 0.5) / 10.0f;
		str += Math::Functs::DoubleString(x);
		str += Math::Functs::DoubleString(y);
	}
	//sort(ints.begin(), ints.end());
	for (auto i : ints)
		str += Math::Functs::IntString(i);
	if (wl_index_ >= 0)
		str = str + Math::Functs::IntString(wl_index_);
	return str;
}


#ifndef AIKIDO_COMMON_STEPSEQUENCE_HPP_
#define AIKIDO_COMMON_STEPSEQUENCE_HPP_

#include <cassert>
#include <limits>
#include <tuple>
#include <boost/iterator/iterator_facade.hpp>

namespace aikido {
namespace common {

/// An iterator that returns a sequence of numbers between start point and end
/// point stepping at a fixed stepsize.
class StepSequence
{
public:
  class const_iterator;

  /// Constructor.
  ///
  /// \param stepSize Step size increments from the start point to the end
  /// point.
  /// \param includeStartpoint If includeStartpoint is true then the start point
  /// in the sequence will be the start point; else the start point in the
  /// sequence will be the start point plus the stepSize (if it is larger than
  /// the end point, it will be the end point.
  /// \param includeEndpoint If includeEndpoint is true then the final point in
  /// the sequence will be the end point, even if it is at less than stepSize
  /// from the second to last point.
  /// \param startPoint The start point that defines the sequence.
  /// \param endPoint The end point that defines the sequence.
  StepSequence(
      double stepSize,
      bool includeStartpoint = true,
      bool includeEndpoint = true,
      double startPoint = 0.0,
      double endPoint = 1.0);

  /// Constructs StepSequence in Matlab's linspace() style.
  StepSequence(
      double startPoint,
      double endPoint,
      std::size_t numSteps,
      bool includeEndpoint = true);

  /// Returns an iterator to the first element of the sequence.
  ///
  /// \return Iterator to the first element of the sequence.
  const_iterator begin() const;

  /// Returns an iterator to the element following the last element of the
  /// sequence.
  ///
  /// \return Iterator followin the last element of the sequence.
  const_iterator end() const;

  /// Returns the \c n-th element of the sequence.
  ///
  /// \return Element in the sequence.
  double operator[](std::size_t n) const;

  /// Returns the total length of sequence.
  ///
  /// \return Non-negative number of the tatal length of sequence.
  std::size_t getLength() const;

private:
  /// Computes the total length of sequence given step size. This is only called
  /// in the contructor.
  void updateNumSteps();

  /// Computes the step size of sequence given number of steps. This is only
  /// called in the contructor.
  void updateStepSize();

  /// Step size increments from the start point to the end point.
  double mStepSize;

  /// Whether the start point in the sequence will be the start point.
  const bool mIncludeStartPoint;

  /// Whether the end point in the sequence will be the end point.
  const bool mIncludeEndPoint;

  /// The start point that defines the sequence.
  const double mStartPoint;

  /// The end point that defines the sequence.
  const double mEndPoint;

  /// The total length of sequence.
  std::size_t mNumSteps;
};

class StepSequence::const_iterator
    : public boost::iterator_facade<StepSequence::const_iterator,
                                    double,
                                    boost::forward_traversal_tag,
                                    double>
{
public:
  /// Dereference implementation for boost::iterator_facade.
  double dereference() const;

  /// Increment implementation for boost::iterator_facade.
  void increment();

  /// Equal implementation for boost::iterator_facade.
  ///
  /// \return True if two iterators are at the same point in the sequence.
  bool equal(const StepSequence::const_iterator& other) const;

private:
  friend class StepSequence;

  /// Private constructor that should always be constructed from
  /// StepSequence::begin().
  const_iterator(const StepSequence& seq, std::size_t step);

  /// StepSequence associated with this iterator.
  const StepSequence& mSeq;

  /// Current step number.
  std::size_t mStep;

  /// Value of the current step.
  double mValue;
};

} // namespace common
} // namespace aikido

#endif // AIKIDO_COMMON_STEPSEQUENCE_HPP_


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/ref.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/is_kuratowski_subgraph.hpp>
#include <boost/graph/connected_components.hpp>
#include <vector>
#include <iostream>
using namespace std;
using namespace boost;

typedef adjacency_list<vecS, vecS, undirectedS> Graph;
typedef graph_traits<Graph>::vertex_descriptor VertDesc;
typedef graph_traits<Graph>::edge_iterator     EdgeIter;

int main(int argc, char** argv)
{
        if( argc < 3 ){
                cerr << "Usage: planargen n e\n";
                cerr << "   n - number of vertices\n";
                cerr << "   e - number of edges (<= 3n-6)\n";
                return 0;
        }
        int n = atoi(argv[1]);
        int e = atof(argv[2]);
        if( e > 3*n - 6 ){
                cerr << "Planr graph must have <= 3*n - 6 edges!\n";
                return 0;
        }

        Graph g(n);
        uint v1, v2;
        for( uint i = 0; i < e; ++i ){

                if( i < n ) v1 = i; // force all verts to be used at least once
                else v1 = rand() % n;

                do { v2 = rand() % n; } while( v1 == v2 );

                if( edge(v1, v2, g).second ) continue;

                auto g_tmp = g;
                add_edge(v1, v2, g_tmp);

                if( boyer_myrvold_planarity_test(g_tmp) ){
                        g = g_tmp;
                        cout << v1 << ", " << v2 << '\n';
                        cerr << (i*100.0/e) << "%\n";
                } else --i;
        }

        vector<int> component(num_vertices(g));
        int num = connected_components(g, &component[0]);

        vector<int>::size_type i;
        cerr << "Total number of components: " << num << endl;
}


#include <ecto/ecto.hpp>
#include <boost/thread.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

using ecto::tendrils;
using ecto::spore;

namespace ecto_test
{

  struct Accumulator
  {
    static void declare_io(const tendrils& p, tendrils& i, tendrils& o)
    {
      i.declare<double>(&Accumulator::left_,"left", "Left hand operand.");
      i.declare<double>(&Accumulator::right_,"right","Right hand operand.");
      o.declare<double>(&Accumulator::out_,"out","The current accumulation.", 0.0);
    }
    int process(const tendrils& inputs, const tendrils& /*outputs*/)
    {
      boost::mutex::scoped_lock lock(mutex);
      if ( inputs.find("left") != inputs.end() ) {
        std::cout << "  Left: " << *out_ << "+" << *left_ << "=" << *out_ + *left_ << std::endl;
        *out_ += *left_;
      }
      if ( inputs.find("right") != inputs.end() ) {
        std::cout << "  Right: " << *out_ << "+" << *right_ << "=" << *out_ + *right_ << std::endl;
        *out_ += *right_;
      }
      // sleep a bit to effectively test the sharing
      boost::this_thread::sleep(boost::posix_time::milliseconds(100));
      return ecto::OK;
    }
    spore<double> out_, left_, right_;
    mutable boost::mutex mutex;
  };
}

ECTO_CELL(ecto_test, ecto_test::Accumulator, "Accumulator", "Add inputs (potentially from different threads) to an incrementally accumulating sum.");


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

#include <Eigen/Dense>

#include <iostream>

using namespace Eigen;

void matrix_multiply(float* m1, int m1_rows, int m1_cols, float* m2, int m2_rows, int m2_cols, float* m_out) {
  Map<MatrixXf> A(m1, m1_rows, m1_cols);
  Map<MatrixXf> B(m2, m2_rows, m2_cols);
  Map<MatrixXf> out(m_out, m1_rows, m2_cols);

  out.noalias() = A * B;
}


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

#define MTL_VERBOSE_TEST

#include <boost/numeric/mtl/mtl.hpp>

// Commands in Matlab
// S = sparse(i,j,s,m,n,nzmax)
// S = sparse(i,j,s,m,n)
// S = sparse(i,j,s)
// S = sparse(m,n)

using mtl::io::tout;

typedef mtl::compressed2D<double, mtl::mat::unsigned_parameters>         matrix_type;

template <typename Matrix>
inline void check(const Matrix& A, unsigned m, unsigned n)
{
    MTL_THROW_IF(num_rows(A) != m, mtl::unexpected_result("Not exact number of rows."));
    MTL_THROW_IF(num_cols(A) != n, mtl::unexpected_result("Not exact number of columns."));
    MTL_THROW_IF(A.nnz() != 5, mtl::unexpected_result("Not exact number of non-zeros."));

    MTL_THROW_IF(A[4][3] != 3.0, mtl::unexpected_result("A[4][3] should be 3."));
    MTL_THROW_IF(A[1][6] != 4.0, mtl::unexpected_result("A[1][6] should be 4."));
    MTL_THROW_IF(A[1][5] != 0.0, mtl::unexpected_result("A[1][5] should be empty."));
}

template <typename SV, typename VV>
inline void test1(const SV& rows, const SV& cols, const VV& values, unsigned m, unsigned n)
{
    matrix_type A;
    A= make_sparse(rows, cols, values, m, n);
    tout << "test1: A is\n" << A;

    check(A, m, n);
}

template <typename SV, typename VV>
inline void test2(const SV& rows, const SV& cols, const VV& values)
{
    matrix_type A;
    A= make_sparse(rows, cols, values);
    tout << "test2: A is\n" << A;

    check(A, 5, 7);
}

inline void test3(unsigned m, unsigned n)
{
    matrix_type A;
    A= mtl::make_sparse(m, n);
    tout << "test3: A is\n" << A;

    MTL_THROW_IF(num_rows(A) != m, mtl::unexpected_result("Not exact number of rows."));
    MTL_THROW_IF(num_cols(A) != n, mtl::unexpected_result("Not exact number of columns."));
    MTL_THROW_IF(A.nnz() != 0, mtl::unexpected_result("Not exact number of non-zeros."));

    MTL_THROW_IF(A[1][5] != 0.0, mtl::unexpected_result("A[1][5] should be empty."));
}

int main(int, char**)
{
    mtl::dense_vector<unsigned>     rows(7), cols(7);
    rows= 2, 0, 4, 1, 3, 1, 0;
    cols= 0, 3, 3, 6, 5, 6, 2;

    mtl::dense_vector<double>       values(7);
    values= 1, 2, 3, 3, 5, 1, 0;

    test1(rows, cols, values, 6, 8);
    test2(rows, cols, values);
    test3(6, 8);

    return 0;
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2015 by Rokko Developers https://github.com/t-sakashita/rokko
*
* 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)
*
*****************************************************************************/

#define ROKKO_ENABLE_TIMER

#include <mpi.h>
#include <iostream>
#include <ctime>

#include <rokko/solver.hpp>
#include <rokko/grid.hpp>
#include <rokko/distributed_matrix.hpp>
#include <rokko/localized_vector.hpp>

#include <rokko/utility/frank_matrix.hpp>
#include <rokko/config.h>
#include <rokko/utility/timer.hpp>

#include <boost/lexical_cast.hpp>
#include <boost/asio.hpp>

int main(int argc, char *argv[]) {
  MPI_Init(&argc, &argv);
  typedef rokko::matrix_col_major matrix_major;
  //typedef rokko::matrix_row_major matrix_major;

  if (argc <= 2) {
    std::cerr << "error: " << argv[0] << " solver_name matrix_size" << std::endl;
    MPI_Abort(MPI_COMM_WORLD, 34);
  }

  rokko::timer timer;
  timer.registrate( 1, "diagonalize");
  //mkl_set_num_threads(4);

  std::string solver_name(argv[1]);
  unsigned int dim = boost::lexical_cast<unsigned int>(argv[2]);

  rokko::parallel_dense_solver solver(solver_name);
  solver.initialize(argc, argv);

  MPI_Comm comm = MPI_COMM_WORLD;
  rokko::grid g(comm);
  int myrank = g.get_myrank();
  int nprocs = g.get_nprocs();

  const int root = 0;

  rokko::distributed_matrix<matrix_major> mat(dim, dim, g, solver);

  rokko::localized_vector w(dim);
  rokko::distributed_matrix<matrix_major> Z(dim, dim, g, solver);

  for (int count=0; count<3; ++count) {
    if (myrank == root) {
      std::cout << "get_count:" << timer.get_count(1) << std::endl;
    }
    rokko::frank_matrix::generate(mat);

    try {
      MPI_Barrier(MPI_COMM_WORLD);
      //timer.start(1);
      solver.diagonalize(mat, w, Z, timer);
      MPI_Barrier(MPI_COMM_WORLD);
      //timer.stop(1);
    }

    catch (const char *e) {
      std::cout << "Exception : " << e << std::endl;
      MPI_Abort(MPI_COMM_WORLD, 22);
    }
  }

#ifndef NDEBUG
  if (myrank == root) {
    std::cout.precision(20);
    std::cout << "Computed Eigenvalues= " << w.transpose() << std::endl;
  }
#endif

  if (myrank == 0) {
    #ifdef _OPENMP
    std::cout << "num_procs = " << nprocs << std::endl;
    std::cout << "num_threads = " << omp_get_max_threads() << std::endl;
    //std::cout << "num_threads = " << mkl_get_num_threads() << std::endl;
    #endif
    std::cout << "solver_name = " << solver_name << std::endl;
    std::cout << "matrix = frank" << std::endl;
    std::cout << "dim = " << dim << std::endl;
    std::cout << "time = " << timer.get_average(1) << std::endl;
    std::cout << "rokko_version = " << ROKKO_VERSION << std::endl;
    std::cout << "hostname = " << boost::asio::ip::host_name() << std::endl;
    std::time_t now = std::time(0);
    std::cout << "date = " << ctime(&now)<< std::endl;
  }

  //timer.summarize();

  solver.finalize();
  MPI_Finalize();
  return 0;
}


/* Copyright (C) 2020 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
#include <NTL/ZZ.h>
#include <algorithm>
#include <complex>

#include <helib/norms.h>
#include <helib/helib.h>
#include <helib/debugging.h>
#include <helib/ArgMap.h>
#include <helib/EncryptedArray.h>
#include <helib/log.h>
#include <helib/fhe_stats.h>

NTL_CLIENT
using namespace helib;

bool verbose = true;

bool reset = false;

void resetPtxtMag(Ctxt& c, const PtxtArray& p)
{
  double maxAbs = NextPow2(Norm(p));
  c.setPtxtMag(NTL::xdouble(maxAbs));
}

/************** Each round consists of the following:
tmp1 = rotate(c0)
tmp1 += const1
c0 += const2
c0 *= tmp1  // c0 = (rotate(c0) + const1)*(c0 + const2) ...squared every round

tmp2 = c1 * const1
c1 = rotate(c1)
c1 += tmp2  // c1 = rotate(c1) + c1*const1 ...doubled every round

tmp3 = c2 * const2
c2 *= c3
c2 += tmp3 // c2 = = c2*c3 + c2*const2 = c2*(c3 + const2)

c3 = c3*const1
**************/

void debugCompare(const SecKey& sk,
                  const PtxtArray& p,
                  const Ctxt& c)
{
  PtxtArray pp(p.getView());
  pp.rawDecryptComplex(c, sk);

  double err = Distance(pp, p);
  double err_bound = c.errorBound();
  double rel_err = err/Norm(p);
  //double rel_err = abs_err / Norm(p);
  std::cout << "   "
            << " err=" << err
            << " err_bound=" << err_bound
            << " err_bound/err=" << (err_bound/err)
            << " rel_err=" << rel_err
            //<< "   "
            //<< " mag=" << Norm(p)
            //<< " mag_bound=" << c.getPtxtMag()
            //<< " scale=" << c.getRatFactor()
            << "\n";
  if (err > err_bound) std::cout << "**** BAD BOUND\n";
}

#define DEBUG_COMPARE(C, P, M)                                                 \
  do {                                                                         \
    if (verbose) {                                                             \
      CheckCtxt(C, M);                                                         \
      debugCompare(secretKey, P, C);                                           \
    }                                                                          \
  } while (0)

void testGeneralOps(const PubKey& publicKey,
                    const SecKey& secretKey,
                    long nRounds)
{
  const Context& context = publicKey.getContext();

  PtxtArray p0(context), p1(context), p2(context), p3(context);
  p0.random();
  p1.random();
  p2.random();
  p3.random();

  Ctxt c0(publicKey), c1(publicKey), c2(publicKey), c3(publicKey);
  p0.encrypt(c0);
  p1.encrypt(c1);
  p2.encrypt(c2);
  p3.encrypt(c3);

  HELIB_NTIMER_START(Circuit);

  for (long i = 0; i < nRounds; i++) {

    if (verbose)
      std::cout << "*** round " << i << "..." << std::endl;

    if (reset) {
      resetPtxtMag(c0, p0);
      resetPtxtMag(c1, p1);
      resetPtxtMag(c2, p2);
      resetPtxtMag(c3, p3);
    }

    DEBUG_COMPARE(c0, p0, "c0");
    DEBUG_COMPARE(c1, p1, "c1");
    DEBUG_COMPARE(c2, p2, "c2");
    DEBUG_COMPARE(c3, p3, "c3");

    long nslots = context.zMStar.getNSlots();

    long rotamt = RandomBnd(2 * nslots - 1) - (nslots - 1);
    // random number in [-(nslots-1)..nslots-1]

    // two random constants
    PtxtArray const1(context), const2(context);
    const1.random();
    const2.random();

    PtxtArray tmp1_p(p0);
    rotate(tmp1_p, rotamt);
    Ctxt tmp1(c0);
    rotate(tmp1, rotamt);
    DEBUG_COMPARE(tmp1, tmp1_p, "tmp1 = rotate(c0)");

    tmp1_p += const1;
    tmp1 += const1;
    DEBUG_COMPARE(tmp1, tmp1_p, "tmp1 += const1");

    p0 += const2;
    c0 += const2;
    DEBUG_COMPARE(c0, p0, "c0 += const2");

    p0 *= tmp1_p;
    c0.multiplyBy(tmp1);
    DEBUG_COMPARE(c0, p0, "c0 *= tmp1");

#if 0
    runningSums(p0);
    runningSums(c0);
    DEBUG_COMPARE(c0, p0, "totalSums");
#endif

#if 0
    p0 -= 17.5;
    c0 -= 17.5;
    DEBUG_COMPARE(c0, p0, "c0 -= 17.5");

    p0 *= 1.25;
    c0 *= 1.25;
    DEBUG_COMPARE(c0, p0, "c0 *= 1.25");
#endif

    PtxtArray tmp2_p(p1);
    tmp2_p *= const1;
    Ctxt tmp2(c1);
    tmp2 *= const1;
    DEBUG_COMPARE(tmp2, tmp2_p, "tmp2 = c1 * const1");

    rotate(p1, rotamt);
    rotate(c1, rotamt);
    DEBUG_COMPARE(c1, p1, "c1 = rotate(c1)");

#if 0
    //std::cerr << "*********** shamt=" << rotamt << "\n";
    //std:: cerr << p1 << "\n";
    shift(p1, rotamt);
    //std:: cerr << p1 << "\n";
    shift(c1, rotamt);
    DEBUG_COMPARE(c1, p1, "c1 = shift(c1)");
#endif

    p1 += tmp2_p;
    c1 += tmp2;
    DEBUG_COMPARE(c1, p1, "c1 += tmp2");

    PtxtArray tmp3_p(p2);
    tmp3_p *= const2;
    Ctxt tmp3(c2);
    tmp3 *= const2;
    DEBUG_COMPARE(tmp3, tmp3_p, "tmp3 = c2 * const2");

    p2 *= p3;
    c2 *= c3;
    DEBUG_COMPARE(c2, p2, "c2 *= c3");

    p2 += tmp3_p;
    c2 += tmp3;
    DEBUG_COMPARE(c2, p2, "c2 += tmp3");

    p3 *= const1;
    c3 *= const1;
    DEBUG_COMPARE(c3, p3, "c3 *= const1");

    if (verbose) {
      // Check correctness after each round
      PtxtArray pp0(context), pp1(context), pp2(context), pp3(context);

      pp0.rawDecryptComplex(c0, secretKey);
      pp1.rawDecryptComplex(c1, secretKey);
      pp2.rawDecryptComplex(c2, secretKey);
      pp3.rawDecryptComplex(c3, secretKey);

      if (!(pp0 == Approx(p0) && pp1 == Approx(p1) && pp2 == Approx(p2) &&
          pp3 == Approx(p3))) {
        std::cout << "FAIL AT ROUND " << i << "\n";
        break;
      }
    }
  }

  HELIB_NTIMER_STOP(Circuit);

  if (verbose) {
    std::cout << "===============\n";

    DEBUG_COMPARE(c0, p0, "c0");
    DEBUG_COMPARE(c1, p1, "c1");
    DEBUG_COMPARE(c2, p2, "c2");
    DEBUG_COMPARE(c3, p3, "c3");
  }

  PtxtArray pp0(context), pp1(context), pp2(context), pp3(context);
  PtxtArray ppp0(context), ppp1(context), ppp2(context), ppp3(context);

  pp0.decryptReal(c0, secretKey);
  pp1.decryptReal(c1, secretKey);
  pp2.decryptReal(c2, secretKey);
  pp3.decryptReal(c3, secretKey);

  ppp0.rawDecryptReal(c0, secretKey);
  ppp1.rawDecryptReal(c1, secretKey);
  ppp2.rawDecryptReal(c2, secretKey);
  ppp3.rawDecryptReal(c3, secretKey);

  if (verbose) {
    std::cout << "======== rounded/raw differences\n";
    std::cout << Distance(pp0, ppp0) << "\n";
    std::cout << Distance(pp1, ppp1) << "\n";
    std::cout << Distance(pp2, ppp2) << "\n";
    std::cout << Distance(pp3, ppp3) << "\n";
  }

  if (verbose) {
    std::cout << "======== actual/raw differences\n";
    std::cout << Distance(p0, ppp0) << "\n";
    std::cout << Distance(p1, ppp1) << "\n";
    std::cout << Distance(p2, ppp2) << "\n";
    std::cout << Distance(p3, ppp3) << "\n";
  }

  if (pp0 == Approx(p0) && pp1 == Approx(p1) && pp2 == Approx(p2) &&
      pp3 == Approx(p3))
    std::cout << "SUCCESS\n";
  else
    std::cout << "FAIL\n";

  if (verbose) {
    std::cout << std::endl;
    // printAllTimers();
    std::cout << std::endl;
  }
  resetAllTimers();
}

int main(int argc, char* argv[])
{
  helog.setLogToStderr();

  // Commandline setup

  ArgMap amap;

  long m = 16;
  long r = 8;
  long L = 0;
  double epsilon = 0.01; // Accepted accuracy
  long R = 1;
  long seed = 0;
  bool debug = false;

  amap.arg("m", m, "Cyclotomic index");
  amap.note("e.g., m=1024, m=2047");
  amap.arg("r", r, "Bits of precision");
  amap.arg("R", R, "number of rounds");
  amap.arg("L", L, "Number of bits in modulus", "heuristic");
  amap.arg("ep", epsilon, "Accepted accuracy");
  amap.arg("seed", seed, "PRG seed");
  amap.arg("verbose", verbose, "more printouts");
  amap.arg("debug", debug, "for debugging");
  amap.arg("reset", reset, "forces calls to setPtxtMag each round");

  amap.parse(argc, argv);

  if (seed)
    NTL::SetSeed(ZZ(seed));

  if (R <= 0)
    R = 1;
  if (L == 0) {
    if (R <= 2)
      L = 100 * R;
    else
      L = 220 * (R - 1);
  }

  if (verbose) {
    std::cout << "** m=" << m << ", #rounds=" << R << ", |q|=" << L
              << ", epsilon=" << epsilon << std::endl;
  }

  if (verbose) fhe_stats = true;

  // FHE setup keys, context, SKMs, etc

  Context context(m, /*p=*/-1, r);
  //context.scale = 4; // why is this 4?
  buildModChain(context, L);

  SecKey secretKey(context);
  secretKey.GenSecKey();        // A +-1/0 secret key
  addSome1DMatrices(secretKey); // compute key-switching matrices

  const PubKey& publicKey = secretKey;
  //const PubKey publicKey = secretKey;

  if (verbose) {
    std::cout << "security=" << context.securityLevel() << std::endl;
    context.zMStar.printout();
    std::cout << "r = " << context.alMod.getR() << std::endl;
    std::cout << "ctxtPrimes=" << context.ctxtPrimes
	 << ", specialPrimes=" << context.specialPrimes << std::endl
	 << std::endl;
  }
  if (debug) {
    dbgKey = &secretKey;
    dbgEa = context.ea;
  }
#ifdef HELIB_DEBUG
  dbgKey = &secretKey;
  dbgEa = context.ea;
#endif // HELIB_DEBUG

  // Run the tests.
  testGeneralOps(publicKey, secretKey, R);

  if (verbose) print_stats(cout);

  return 0;
}


//==================================================================================================
/*!

  Copyright 2016 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/sincos.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/function/sin.hpp>
#include <boost/simd/function/cos.hpp>

STF_CASE_TPL (" sincos",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::sincos;
  T a[] = {bs::Zero<T>(), bs::One<T>(), bs::Pio2_3<T>(), bs::Pi<T>(),
           bs::Pio_2<T>(), bs::Inf<T>(), bs::Minf<T>(), bs::Nan<T>()};
  size_t N =  sizeof(a)/sizeof(T);

  STF_EXPR_IS( (sincos(T()))
             , (std::pair<T,T>)
             );

   {
    for(size_t i=0; i < N; ++i)
    {
      std::pair<T,T> p = sincos(a[i]);
      STF_IEEE_EQUAL(p.first,  bs::sin(a[i]));
      STF_IEEE_EQUAL(p.second, bs::cos(a[i]));
      std::pair<T,T> q = bs::restricted_(bs::sincos)(a[i]);
      STF_IEEE_EQUAL(q.first,  bs::restricted_(bs::sin)(a[i]));
      STF_IEEE_EQUAL(q.second, bs::restricted_(bs::cos)(a[i]));
    }
   }

}


#ifndef ENCRYPTION_HPP
#define ENCRYPTION_HPP

#include <boost/multiprecision/cpp_int.hpp>
#include <cstring>
#include <fstream>
#include <iostream>
#include <string>

#include "big_int_type.hpp"
#include "key_generator.hpp"
#include "mod_exponentiation.hpp"

unsigned log_b(big_int n);

big_int text_to_numeric(const std::string &str);

std::string numeric_to_text(big_int num);

std::string hash_numeric(big_int num);

big_int unhash_text(const std::string &str);

/*
 * Data manipulation algorithm.
 */
void data_manipulation(std::string src_path, std::string dst_path, Key key);

/*
 * Encrypts a source file to a destination file.
 */
void encrypt_file(std::string src_path, std::string dst_path, Key pub_key);

/*
 * Decrypts a source file to a destination file.
 */
void decrypt_file(std::string src_path, std::string dst_path, Key priv_key);

#endif


#pragma once
#include <dynamic_msgs/State.h>
#include <Eigen/Dense>
#include <sp_const.hpp>

namespace DynamicPlanning {
    class LinearKalmanFilter{
    public:
        void initialize(double _sigma_y_sq, double _sigma_v_sq, double _sigma_a_sq){
            n = 6; // the number of states
            m = 3; // the number of observed states
            phi = 2; // position, velocity
            sigma_y_sq = _sigma_y_sq;
            sigma_v_sq = _sigma_v_sq;
            sigma_a_sq = _sigma_a_sq;

            F = Eigen::MatrixXd::Identity(n, n);
            B = Eigen::MatrixXd::Zero(n, n/phi);

            Q = Eigen::MatrixXd::Zero(n, n);
//            Q.block(0, 0, n/phi, n/phi) = Eigen::MatrixXd::Identity(n/phi, n/phi) * sigma_y_sq;
//            Q.block(n/phi, n/phi, n/phi, n/phi) = Eigen::MatrixXd::Identity(n/phi, n/phi) * sigma_v_sq;

            H = Eigen::MatrixXd::Zero(m, n);
            H.block(0, 0, m, m) = Eigen::MatrixXd::Identity(m, m);
            R = Eigen::MatrixXd::Identity(m, m) * sigma_y_sq;

            P = Eigen::MatrixXd::Zero(n, n);
            P.block(0, 0, n/phi, n/phi) = Eigen::MatrixXd::Identity(n/phi, n/phi) * 10;
            P.block(n/phi, n/phi, n/phi, n/phi) = Eigen::MatrixXd::Identity(n/phi, n/phi) * 10;

            Y = Eigen::VectorXd(m);
            X_hat = Eigen::VectorXd(n);

            isFirstInput = true;
        }

        dynamic_msgs::Obstacle filter(const dynamic_msgs::Obstacle& obstacle, ros::Time current_update_time){
            // Observe
            Y << obstacle.pose.position.x, obstacle.pose.position.y, obstacle.pose.position.z;

            if(isFirstInput){
                X_hat << Y, 0, 0, 0;
                isFirstInput = false;
            }
            else{
                // Update F
                dt = (current_update_time - prev_update_time).toSec();
                F.block(0, n/phi, n/phi, n/phi) = Eigen::MatrixXd::Identity(n/phi, n/phi) * dt;
                B.block(n/phi, 0, n/phi, n/phi) = Eigen::MatrixXd::Identity(n/phi, n/phi) * dt;
                Q = sigma_a_sq * B * B.transpose();
//                Q.block(0, 0, n/phi, n/phi) = Eigen::MatrixXd::Identity(n/phi, n/phi) * sigma_y_sq;

                // Predict
                X_hat_update = F * X_hat;
                Eigen::MatrixXd P_update = F * P * F.transpose() + Q;

                // Kalman Gain
                Eigen::MatrixXd S = H * P_update * H.transpose() + R;
                Eigen::MatrixXd K = P_update * H.transpose() * S.inverse();

                // Update
                X_hat = X_hat_update + K * (Y - H * X_hat_update);
                P = P_update - K * H * P_update;
            }

            dynamic_msgs::Obstacle result = obstacle;
            result.pose.position.x = X_hat(0, 0);
            result.pose.position.y = X_hat(1, 0);
            result.pose.position.z = X_hat(2, 0);
            result.velocity.linear.x = X_hat(3, 0);
            result.velocity.linear.y = X_hat(4, 0);
            result.velocity.linear.z = X_hat(5, 0);

//            double v = sqrt(pow(result.velocity.linear.x, 2) + pow(result.velocity.linear.y, 2) + pow(result.velocity.linear.z, 2));
//            std::cout << "dt: " << dt << ", v: " << v << std::endl;

            prev_update_time = current_update_time;
            return result;
        }

        Eigen::MatrixXd getPositionCovariance(){
            return P.block(0, 0, n/phi, n/phi);
        }

        double getUncertaintyRadius(double t_delta){
            F.block(0, n/phi, n/phi, n/phi) = Eigen::MatrixXd::Identity(n/phi, n/phi) * t_delta;
            B.block(n/phi, 0, n/phi, n/phi) = Eigen::MatrixXd::Identity(n/phi, n/phi) * t_delta;
            Q = sigma_a_sq * B * B.transpose();
            Eigen::MatrixXd P_update = F * P * F.transpose() + Q;

            Eigen::MatrixXd SIGMA = P_update.block(0, 0, n/phi, n/phi);
            double uncertainty_radius = 1.33 * sqrt(2.0 * (double)SIGMA.trace()); //TODO: consider vector direction
            return uncertainty_radius;
        }

    private:
        int n, m, phi;
        double sigma_y_sq, sigma_v_sq, sigma_a_sq, dt;
        Eigen::MatrixXd F, B, Q, H, R, P;
        Eigen::VectorXd Y, X_hat, X_hat_update;

        bool isFirstInput;
        ros::Time prev_update_time;
    };
}


/* Boost interval/detail/division.hpp file
 *
 * Copyright Guillaume Melquiond, Sylvain Pion 2003
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * None of the above authors make any representation about the
 * suitability of this software for any purpose. It is provided "as
 * is" without express or implied warranty.
 *
 * $Id: division.hpp,v 1.3 2003/02/05 17:34:32 gmelquio Exp $
 */

#ifndef BOOST_NUMERIC_INTERVAL_DETAIL_DIVISION_HPP
#define BOOST_NUMERIC_INTERVAL_DETAIL_DIVISION_HPP

#include <boost/numeric/interval/detail/interval_prototype.hpp>
#include <boost/numeric/interval/detail/bugs.hpp>
#include <boost/numeric/interval/rounded_arith.hpp>
#include <algorithm>

namespace boost {
namespace numeric {
namespace interval_lib {
namespace detail {

template<class T, class Policies> inline
interval<T, Policies> div_non_zero(const interval<T, Policies>& x,
                                   const interval<T, Policies>& y)
{
  // assert(!in_zero(y));
  typename Policies::rounding rnd;
  typedef interval<T, Policies> I;
  const T& xl = x.lower();
  const T& xu = x.upper();
  const T& yl = y.lower();
  const T& yu = y.upper();
  if (is_neg(xu))
    if (is_neg(yu))
      return I(rnd.div_down(xu, yl), rnd.div_up(xl, yu), true);
    else
      return I(rnd.div_down(xl, yl), rnd.div_up(xu, yu), true);
  else if (is_neg(xl))
    if (is_neg(yu))
      return I(rnd.div_down(xu, yu), rnd.div_up(xl, yu), true);
    else
      return I(rnd.div_down(xl, yl), rnd.div_up(xu, yl), true);
  else
    if (is_neg(yu))
      return I(rnd.div_down(xu, yu), rnd.div_up(xl, yl), true);
    else
      return I(rnd.div_down(xl, yu), rnd.div_up(xu, yl), true);
}

template<class T, class Policies> inline
interval<T, Policies> div_non_zero(const T& x, const interval<T, Policies>& y)
{
  // assert(!in_zero(y));
  typename Policies::rounding rnd;
  typedef interval<T, Policies> I;
  const T& yl = y.lower();
  const T& yu = y.upper();
  if (is_neg(x))
    return I(rnd.div_down(x, yl), rnd.div_up(x, yu), true);
  else
    return I(rnd.div_down(x, yu), rnd.div_up(x, yl), true);
}

template<class T, class Policies> inline
interval<T, Policies> div_positive(const interval<T, Policies>& x, const T& yu)
{
  // assert(yu > T(0));
  if (is_zero(x)) return x;
  typename Policies::rounding rnd;
  typedef interval<T, Policies> I;
  const T& xl = x.lower();
  const T& xu = x.upper();
  typedef typename Policies::checking checking;
  const T& inf = checking::inf();
  if (is_neg(xu))
    return I(-inf, rnd.div_up(xu, yu), true);
  else if (is_neg(xl))
    return I(-inf, inf, true);
  else
    return I(rnd.div_down(xl, yu), inf, true);
}

template<class T, class Policies> inline
interval<T, Policies> div_positive(const T& x, const T& yu)
{
  // assert(yu > T(0));
  typedef interval<T, Policies> I;
  if (is_zero(x)) return I(0, 0, true);
  typename Policies::rounding rnd;
  typedef typename Policies::checking checking;
  const T& inf = checking::inf();
  if (is_neg(x))
    return I(-inf, rnd.div_up(x, yu), true);
  else
    return I(rnd.div_down(x, yu), inf, true);
}

template<class T, class Policies> inline
interval<T, Policies> div_negative(const interval<T, Policies>& x, const T& yl)
{
  // assert(yl < T(0));
  if (is_zero(x.lower()) && is_zero(x.upper()))
    return x;
  typename Policies::rounding rnd;
  typedef interval<T, Policies> I;
  const T& xl = x.lower();
  const T& xu = x.upper();
  typedef typename Policies::checking checking;
  const T& inf = checking::inf();
  if (is_neg(xu))
    return I(rnd.div_down(xu, yl), inf, true);
  else if (is_neg(xl))
    return I(-inf, inf, true);
  else
    return I(-inf, rnd.div_up(xl, yl), true);
}

template<class T, class Policies> inline
interval<T, Policies> div_negative(const T& x, const T& yl)
{
  // assert(yl < T(0));
  typedef interval<T, Policies> I;
  if (is_zero(x)) return I(0, 0, true);
  typename Policies::rounding rnd;
  typedef typename Policies::checking checking;
  const T& inf = checking::inf();
  if (is_neg(x))
    return I(rnd.div_down(x, yl), inf, true);
  else
    return I(-inf, rnd.div_up(x, yl), true);
}

template<class T, class Policies> inline
interval<T, Policies> div_zero(const interval<T, Policies>& x)
{
  if (is_zero(x.lower()) && is_zero(x.upper()))
    return x;
  else return interval<T, Policies>::whole();
}

template<class T, class Policies> inline
interval<T, Policies> div_zero(const T& x)
{
  if (is_zero(x)) return interval<T, Policies>(0, 0, true);
  else return interval<T, Policies>::whole();
}

template<class T, class Policies> inline
interval<T, Policies> div_zero_part1(const interval<T, Policies>& x,
                                     const interval<T, Policies>& y, bool& b)
{
  // assert(y.lower() < 0 && y.upper() > 0);
  if (is_zero(x.lower()) && is_zero(x.upper()))
    { b = false; return x; }
  typename Policies::rounding rnd;
  typedef interval<T, Policies> I;
  const T& xl = x.lower();
  const T& xu = x.upper();
  const T& yl = y.lower();
  const T& yu = y.upper();
  typedef typename I::checking checking;
  const T& inf = checking::inf();
  if (is_neg(xu))
    { b = true;  return I(-inf, rnd.div_up(xu, yu), true); }
  else if (is_neg(xl))
    { b = false; return I(-inf, inf, true); }
  else
    { b = true;  return I(-inf, rnd.div_up(xl, yl), true); }
}

template<class T, class Policies> inline
interval<T, Policies> div_zero_part2(const interval<T, Policies>& x,
                                     const interval<T, Policies>& y)
{
  // assert(y.lower() < 0 && y.upper() > 0 && (div_zero_part1(x, y, b), b));
  typename Policies::rounding rnd;
  typedef interval<T, Policies> I;
  typedef typename I::checking checking;
  const T& inf = checking::inf();
  if (is_neg(x.upper()))
    return I(rnd.div_down(x.upper(), y.lower()), inf, true);
  else
    return I(rnd.div_down(x.lower(), y.upper()), inf, true);
}

} // namespace detail
} // namespace interval_lib
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_INTERVAL_DETAIL_DIVISION_HPP