/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/math/interpolations/cubicinterpolation.hpp>
#include <ql/methods/finitedifferences/meshers/concentrating1dmesher.hpp>
#include <ql/methods/finitedifferences/meshers/fdmmeshercomposite.hpp>
#include <ql/methods/finitedifferences/utilities/fdmdirichletboundary.hpp>
#include <ql/methods/finitedifferences/utilities/fdmboundaryconditionset.hpp>
#include <ql/methods/finitedifferences/boundarycondition.hpp>
#include <ql/methods/finitedifferences/solvers/fdmbackwardsolver.hpp>
#include <ql/experimental/models/quadraticlfm.hpp>
#include <ql/experimental/finitedifferences/fdmdupire1dop.hpp>

#include <boost/function.hpp>

#include <algorithm>

namespace QuantLib {

QuadraticLfm::QuadraticLfm(
	const std::vector<Real> &rateTimes,
	const std::vector<Real> &initialForwards,
	const std::vector<std::vector<std::vector<Real> > > &sigma,
	const std::vector<std::vector<Real> > &b,
	const std::vector<std::vector<Real> > &c)
	: rateTimes_(rateTimes), initialForwards_(initialForwards), sigma_(sigma),
	  b_(b), c_(c) {
	N_ = rateTimes.size();
	QL_REQUIRE(N_ - 1 == initialForwards_.size(),
			   "rateTimes size ("
				   << N_ << ") minus 1 must be equal to number of forwards ("
				   << initialForwards_.size() << ")");
	K_ = sigma_.size();
	QL_REQUIRE(K_ >= 1, "number of factors ("
							<< K_ << ") must be greater or equal to one");
	for (Size k = 0; k < K_; ++k) {
		QL_REQUIRE(N_ - 1 == sigma_[k].size(),
				   "for factor k ("
					   << k << ") the number of sigma functions ("
					   << sigma_[k].size()
					   << ") must be equal to the number of forwards N-1 ("
					   << (N_ - 1) << ")");
		for (Size i = 0; i < N_ - 1; ++i) {
			QL_REQUIRE(N_ - 1 == sigma_[k][i].size(),
					   "for factor k (" << k << ") and Libor i (" << i
										<< ") the piecewise sigma function "
										   "must consist of N-1 (" << (N_ - 1)
										<< ") values, but is ("
										<< sigma_[k][i].size() << ")");
		}
	}
}

const void QuadraticLfm::checkSwapParameters(const Size n, const Size m,
											 const Size step) {
	QL_REQUIRE(N_ - 1 >= m && m > n && n >= 0,
			   "for a swap rate 0 <= n (" << n << ") < m (" << m << ") <= N-1 ("
										  << (N_ - 1) << ") must hold");
	QL_REQUIRE((m - n) % step == 0,
			   "m (" << m << ") minus n (" << n << ") = " << (m - n)
					 << " must be divisible by step (" << step << ")");
	return;
}

int QuadraticLfm::q(const Real t) {
	QL_REQUIRE(t >= 0.0 && t < rateTimes_[N_ - 2],
			   "at time " << t << " all forwards are dead");
	return static_cast<int>(
		std::upper_bound(rateTimes_.begin(), rateTimes_.end(), t) -
		rateTimes_.begin());
}

Real QuadraticLfm::P(const Size n, const Size m) {
	QL_REQUIRE(N_ - 1 >= m && m > n && n >= 0, "for a discount factor 0 <= n ("
												   << n << ") < m (" << m
												   << ") <= N-1 (" << (N_ - 1)
												   << ") must hold");
	Real tmp = 1.0;
	for (Size i = n; i < m; ++i)
		tmp *=
			1.0 /
			(1.0 + initialForwards_[i] * (rateTimes_[i + 1] - rateTimes_[i]));
	return tmp;
}

Real QuadraticLfm::S(const Size n, const Size m, const Size step) {
	checkSwapParameters(n, m, step);
	Real annuity = 0.0;
	for (Size i = n + step; i <= m; i += step)
		annuity += P(n, i) * (rateTimes_[i] - rateTimes_[i - step]);
	return (1.0 - P(n, m)) / annuity;
}

Real QuadraticLfm::dSdL(const Size n, const Size m, const Size step,
						const Size i, const Real h) {
	checkSwapParameters(n, m, step);
	QL_REQUIRE(N_ - 2 >= i && i >= 0, "for dSdL, 0 <= i (" << i << ") <= N-2 ("
														   << (N_ - 2)
														   << ") must hold");
	QL_REQUIRE(h > 0.0, "for dSdL h (" << h << ") must be positive");
	Real f = S(n, m, step);
	Real tmp = initialForwards_[i];
	initialForwards_[i] += h;
	Real fh = S(n, m, step);
	initialForwards_[i] = tmp;
	return (fh - f) / h;
}

Real QuadraticLfm::eta(const Size n, const Size m, const Size step,
					   const Real t, const Real s) {
	Array sVec(1, s);
	return eta(n, m, step, t, sVec)[0];
}

Disposable<Array> QuadraticLfm::eta(const Size n, const Size m, const Size step,
									const Real t, const Array &s) {

	checkSwapParameters(n, m, step);

	// t = 0 can not be calculated
	Real t0 = std::max(0.0001, t);

	// index for vectors where piecewise values are stored (sigma, b, c)
	Size ind = static_cast<Size>(q(t0));
	// time between last index and t
	Real timeToLastIndex = ind == 0 ? t0 : t0 - rateTimes_[ind - 1];
	// forward swap rate S(0)
	Real s0 = S(n, m, step);

	// set up vectors

	std::vector<Real> qi(m - n, 0.0);    // dS/dL_i * L_i(0) / S(0)
	std::vector<Real> Qi(m - n, 0.0);    // sum_j q_j s_i,j(t)
	std::vector<Real> intQi(m - n, 0.0); // int_0^t Qi(t) dt
	std::vector<std::vector<Real> > sij; // sigma_i,k (t) * sigma_j,k (t)
	std::vector<std::vector<Real> >
		intsisj; // int_0^t \sum_k sigma_i,k (t) sigma_j,k (t) dt

	for (Size j = n; j < m; ++j) {
		std::vector<Real> sijtmp(m - n, 0.0);
		std::vector<Real> intsisjtmp(m - n, 0.0);
		sij.push_back(sijtmp);
		intsisj.push_back(intsisjtmp);
	}

	// precompute results

	for (Size i = n; i < m; ++i) {
		qi[i - n] = dSdL(n, m, step, i) * initialForwards_[i] / s0;
		for (Size k = 0; k < K_; ++k) {
			for (Size j = n; j < m; ++j) {
				sij[i - n][j - n] +=
					sigma_[k][i - n][ind] * sigma_[k][j - n][ind];
			}
		}
		for (Size j = n; j < m; ++j) {
			for (Size k = 0; k < K_; ++k) {
				intsisj[i - n][j - n] += sigma_[k][i - n][ind] *
										 sigma_[k][j - n][ind] *
										 timeToLastIndex;
				for (Size ii = 0; ii < ind; ++ii) {
					intsisj[i - n][j - n] +=
						sigma_[k][i - n][ii] * sigma_[k][j - n][ii] *
						(rateTimes_[ii + 1] - rateTimes_[ii]);
				}
			}
		}
	}

	for (Size i = n; i < m; ++i) {
		for (Size j = n; j < m; ++j) {
			Qi[i - n] += qi[j - n] * sij[i - n][j - n];
			for (Size k = 0; k < K_; ++k) {
				intQi[i - n] += qi[j - n] * sigma_[k][i - n][ind] *
								sigma_[k][j - n][ind] * timeToLastIndex;
				for (Size ii = 0; ii < ind; ++ii) {
					intQi[i - n] += qi[j - n] * sigma_[k][i - n][ii] *
									sigma_[k][j - n][ii] *
									(rateTimes_[ii + 1] - rateTimes_[ii]);
				}
			}
		}
	}

	// =================================================================================
	// my own derivation (needs work ...)
	// =================================================================================

	// // E_i

	// std::vector<Real> Ei(m - n, 0.0);

	// Real denom = 0.0;
	// for (Size i = n; i < m; ++i) {
	//     for (Size j = n; j < m; ++j) {
	//         Real tmp = 0.0;
	//         for (Size k = 0; k < K_; ++k) {
	//             tmp += intsisj[k][i - n][j - n];
	//         }
	//         denom += initialForwards_[i] * initialForwards_[j] * qi[i -
	//         n] *
	//                  qi[j - n] * tmp;
	//     }
	// }

	// for (Size i = n; i < m; ++i) {
	//     Real tmp1 = 0.0;
	//     for (Size k = 0; k < K_; ++k) {
	//         for (Size j = n; j < m; ++j) {
	//             tmp1 +=
	//                 initialForwards_[j] * qi[j - n] * intsisj[k][i - n][j
	//                 -
	//                 n];
	//         }
	//     }
	//     tmp1 *= initialForwards_[i];
	//     Ei[i - n] = tmp1 / denom;
	// }

	// // eta squared

	Array eta2(s.size(), 0.0);
	// for (Size i = n; i < m; ++i) {
	//     for (Size j = n; j < m; ++j) {
	//         for (Size k = 0; k < s.size(); ++k) {
	//             if (i != j) {
	//                 eta2[k] += qi[i - n] * qi[j - n] * sij[i - n][j - n]
	//                 *
	//                            (initialForwards_[i] * initialForwards_[j]
	//                            +
	//                             b_[i][ind] * Ei[i - n] *
	//                             initialForwards_[j]
	//                             *
	//                                 (s[k] - s0) +
	//                             b_[j][ind] * Ei[j - n] *
	//                             initialForwards_[i]
	//                             *
	//                                 (s[k] - s0) +
	//                             (c_[i][ind] * Ei[i - n] * Ei[i - n] *
	//                                  initialForwards_[j] +
	//                              c_[j][ind] * Ei[j - n] * Ei[j - n] *
	//                                  initialForwards_[i]) *
	//                                 (s[k] - s0) * (s[k] - s0));
	//             } else {
	//                 eta2[k] += qi[i - n] * qi[i - n] * sij[i - n][i - n]
	//                 *
	//                            (initialForwards_[i] * initialForwards_[i]
	//                            +
	//                             2.0 * b_[i][ind] * initialForwards_[i] *
	//                                 Ei[i - n] * (s[k] - s0) +
	//                             (b_[i][ind] * b_[i][ind] +
	//                              2.0 * c_[i][ind] * Ei[i - n] * Ei[i - n]
	//                              *
	//                                  (s[k] - s0) * (s[k] - s0)));
	//             }
	//         }
	//     }
	// }

	// =================================================================================
	// Jonathan's derivation
	// =================================================================================

	Real sigma2 = 0.0, intSigma2 = 0.0;
	for (Size i = n; i < m; ++i) {
		for (Size j = n; j < m; ++j) {
			sigma2 += qi[i - n] * qi[j - n] * sij[i - n][j - n];
			intSigma2 += qi[i - n] * qi[j - n] * intsisj[i - n][j - n];
		}
	}

	Real b = 0.0, c = 0.0;
	for (Size i = n; i < m; ++i) {
		b += b_[i][ind] * qi[i - n] * Qi[i - n] * intQi[i - n] /
			 (sigma2 * intSigma2);
		c += c_[i][ind] * qi[i - n] * Qi[i - n] * intQi[i - n] * intQi[i - n] /
			 (sigma2 * intSigma2 * intSigma2);
	}

	for (Size k = 0; k < s.size(); ++k) {
		Real x = (s[k] - s0) / s0;
		eta2[k] = s0 * (1.0 + b * x + c * x * x) * std::sqrt(sigma2);
	}

	for (Size k = 0; k < s.size(); ++k) {
		// through the approximation for eta2 it may get negative (?)
		eta2[k] = std::max(eta2[k], 0.0);
	}

	return eta2;

} // eta

Disposable<std::vector<Real> >
QuadraticLfm::callPrices(const Size n, const Size m, const Size step,
						 const std::vector<Real> &strikes) {

	checkSwapParameters(n, m, step);

	// expiry time
	Real expiryTime = rateTimes_[n];

	// forward swap rate
	Real forward = S(n, m, step);

	// grid parameters (hardcoded here ... !)
	const Real start = std::min(0.00001, strikes.front() * 0.5);
	const Real end = std::max(0.10, strikes.back() * 1.5);
	const Size size = 500;
	const Real density = 0.1;
	const Size steps = static_cast<Size>(std::ceil(expiryTime * 24));
	const Size dampingSteps = 5;

	// Layout
	std::vector<Size> dim(1, size);
	const boost::shared_ptr<FdmLinearOpLayout> layout(
		new FdmLinearOpLayout(dim));

	// Mesher
	const boost::shared_ptr<Fdm1dMesher> m1(new Concentrating1dMesher(
		start, end, size, std::pair<Real, Real>(forward, density), true));
	const std::vector<boost::shared_ptr<Fdm1dMesher> > meshers(1, m1);
	const boost::shared_ptr<FdmMesher> mesher(
		new FdmMesherComposite(layout, meshers));

	// Boundary conditions
	FdmBoundaryConditionSet boundaries;

	// initial values
	Array rhs(mesher->layout()->size());
	for (FdmLinearOpIterator iter = layout->begin(); iter != layout->end();
		 ++iter) {
		Real k = mesher->location(iter, 0);
		rhs[iter.index()] = std::max(forward - k, 0.0);
	}

	// strike grid
	const Array strikeGrid = mesher->locations(0);

	// local vol function
	LocalVolHelper localVol(this, n, m, step, strikeGrid);

	// solver
	boost::shared_ptr<FdmDupire1dOp> map(new FdmDupire1dOp(mesher, localVol));
	FdmBackwardSolver solver(map, boundaries,
							 boost::shared_ptr<FdmStepConditionComposite>(),
							 FdmSchemeDesc::Douglas());
	solver.rollback(rhs, expiryTime, 0.0, steps, dampingSteps);

	// interpolate solution
	boost::shared_ptr<Interpolation> solution(new CubicInterpolation(
		strikeGrid.begin(), strikeGrid.end(), rhs.begin(),
		CubicInterpolation::Spline, true, CubicInterpolation::SecondDerivative,
		0.0, CubicInterpolation::SecondDerivative, 0.0));
	// boost::shared_ptr<Interpolation> solution(new
	// LinearInterpolation(k.begin(),k.end(),rhs.begin()));
	solution->disableExtrapolation();
	std::vector<Real> result(strikes.size());
	std::transform(strikes.begin(), strikes.end(), result.begin(), *solution);
	return result;

} // callPrices

} // namespace QuantLib


#include <armadillo>
#include <iostream>

using namespace arma;

int main(int argc, char *argv[]) {
    if (argc < 2) {
        std::cerr << "# error: no file specified" << std::endl;
        return 1;
    }
    mat A;
    A.load(argv[1], raw_ascii);
    if (A.n_rows != A.n_cols) {
        std::cerr << "# error: matrix should be square" << std::endl;
        return 2;
    }
    mat U, V;
    vec s;
    svd(U, s, V, A);
    U.print("U:");
    s.print("s:");
    V.print("V:");
    mat B = diagmat(s);
    mat Delta = abs((U*B)*V.t() - A);
    Delta.print("delta:");
    return 0;
}


#define GLM_FORCE_XYZW_ONLY

#include <iostream>
#include <sstream>
#include <time.h>

#include <spob/spob2glm.h>
#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::VectorXd;

#include "interpolation.h"
#include "draw.h"
#include "spline.h"
#include "interpolation.h"
//#include "mba.hpp"

typedef function<void(const std::vector<vec2>&, int)> DrawFunction;
typedef function<bool(const vec2&, int)> TerminateFunction;

int n = 4, m = 2;
int angle = 45;
bool isFirst = true;

int maxDepth = 100;
int insideCount = 1;
double min_len = 5.3;
bool isExperiment = false;

double posAnimation = 0;

void empty_draw(const std::vector<vec2>& poly, int a) {}

//-----------------------------------------------------------------------------
std::pair<space2, space2> getFractalSpaces(space2 space, DrawFunction draw_poly = empty_draw, DrawFunction draw_triangle = empty_draw, int depth = 0) {
	// Задаем координаты квадрата
	vector<vec2> p = placePolyOnEdge(calcRegularPolygon(n, vec2(0), 1, 0), 0);

	// Высчитываем координаты прямоугольного треугольника, который лежит своей гипотенузой на оси X, с углом alpha при основании
	double alpha = spob::deg2rad(angle);
	vec2 tr_a(0, 0), tr_b(1, 0), tr_c(cos(alpha), 0);
	tr_c = rotate(tr_c, vec2(0), alpha);

	// Рисуем квадрат
	draw_poly(fromMas(space, p), depth);

	// Строим пространство, которое находится на верхней стороне квадрата
	space2 tr_line = makeLine2(p[m+1], p[m]);

	// Переводим координаты треугольника к этому пространству
	tr_a = tr_line.from(tr_a);
	tr_b = tr_line.from(tr_b);
	tr_c = tr_line.from(tr_c);

	draw_triangle(fromMas(space, std::vector<vec2>{tr_a, tr_b, tr_c}), depth);

	// Строим пространства, которые находятся на обоих катетах этого треугольника
	space2 l1 = makeLine2(tr_a, tr_c);
	space2 l2 = makeLine2(tr_c, tr_b);

	/*l1.j *= 0.8;
	l2.j *= 0.8;*/
	/*l1 = rotate(l1, l1.pos, spob::deg2rad(-50));
	l2 = rotate(l2, l2.pos + l2.i, spob::deg2rad(50));*/

	return {l1, l2};
}

//-----------------------------------------------------------------------------
void draw_pythagoras_tree(space2 space, DrawFunction draw_poly, DrawFunction draw_triangle, TerminateFunction isTerminate, int depth = 0) {
	// Выходим из рекурсии, если одна из осей (аналогично и сторона квадрата) имеет длину меньше, чем 2
	if (isTerminate(space.i, depth))
		return;

	auto sp = getFractalSpaces(space, draw_poly, draw_triangle, depth);

	// Рекурсивно строим дерево в этих пространствах
	draw_pythagoras_tree(space.from(sp.first), draw_poly, draw_triangle, isTerminate, depth+1);
	draw_pythagoras_tree(space.from(sp.second), draw_poly, draw_triangle, isTerminate, depth+1);
}

//-----------------------------------------------------------------------------
std::pair<vec2, vec2> calcBoundingBox(void) {
	bool isInitialized = false;
	vec2 min, max;
	draw_pythagoras_tree(getStandardCrd2(), [&] (const vector<vec2>& poly, int depth) {
		if (!isInitialized) {
			isInitialized = true;
			min = poly[0];
			max = poly[1];
		}
		for (auto& i : poly) {
			if (i.x < min.x) min.x = i.x;
			if (i.y < min.y) min.y = i.y;
			if (i.x > max.x) max.x = i.x;
			if (i.y > max.y) max.y = i.y;
		}
	}, [&] (const vector<vec2>& poly, int depth) {}, [&] (vec2 i, int depth) -> bool {
		return depth > 500 || i.length() < 0.01;
	});
	return {min, max};
}

//-----------------------------------------------------------------------------
void draw_animation(void) {
	crd2 standard = getStandardCrd2();
	vector<vec2> square = placePolyOnEdge(calcRegularPolygon(4, vec2(0), 1, 0), 0);

	//-------------------------------------------------------------------------
	// Считаем ограничивающий прямоугольник у фрактала и систему координат, которрая будет идеально смотреть на фрактал вместе с границами
	auto bbox = calcBoundingBox();
	auto viewport = calcViewPort(bbox.first, bbox.second);
	viewport = increaseViewportBorderByMinAxis(viewport, 0.1);
	double coef = viewport.i.length() / viewport.j.length();

	vec2 size;
	double sz = 500;
	if (coef > 1)
		size = vec2(sz, sz / coef);
	else
		size = vec2(sz * coef, sz);

	//-------------------------------------------------------------------------
	// Инициализируем все изображения
	ImageGif gif;
	ImageGif gif2;
	Image img(size, viewport, maxDepth+5);
	Image img2(size, viewport, maxDepth+5);

	img.setViewPort(viewport);
	img2.setViewPort(viewport);

	string t = std::to_string(time(0));
	stringstream sout;
	sout << "p3_" << n << "." << m << "_" << angle;
	gif.start(img.imgs[0]->size(), sout.str() + ".gif");
	gif2.start(img2.imgs[0]->size(), sout.str() + "_explanation.gif");

	//-------------------------------------------------------------------------
	// Инициализируем системы координат и интерполяцию
	auto sp = getFractalSpaces(standard);
	//auto space = sp.second, another = sp.first;
	space2 space, another;
	if (isFirst) {
		space = sp.first;
		another = sp.second;
	} else {
		space = sp.second;
		another = sp.first;
	}

	auto start = standard;
	for (int i = 0; i < insideCount; i++)
		start = space.from(start);
	auto end = space.from(start);
	MatrixPowerInterpolator interpolator(start, end);
	//SplineInterpolator2 interpolator(5, start, end);

	//-------------------------------------------------------------------------
	// Основной цикл рисования
	double count = 60;
	for (int i = 0; i <= count; i += (isExperiment) ? 60 : 1) {
	//int i = 0; {
		cout << i << endl;
		posAnimation = i/count;

		// Сетка и координаты на изображении-объяснении
		img.clear(White);
		//img.clear(Black);
		img.set_pen(0.5/80.0, Black, 0);

		//img2.clear(White);
		img2.clear(White);
		img2.set_pen(1.5/80.0, setAlpha(Gray, 192), 0);
		img2.draw_grid(standard, 0);
		img2.set_pen(1.5/80.0, setAlpha(Gray, 192), maxDepth+2);
		img2.draw_crd(standard, maxDepth+2);
		img2.set_pen(1/80.0, Black, 0);

		// Интерполированная система координат
		space2 c = interpolator.interpolate(posAnimation);
		//space2 c = interpolate(start, end, posAnimation);
		//space2 c = interpolateCircular(start, end, posAnimation, 1.12, 2, true);
		space2 d = c.from(viewport);
		img.setViewPort(d);

		// Сам процесс рисования фрактала
		static double max_len = img.screen_tr.fromDir(vec2(0, 1)).length();
		double start_min_len = std::min(min_len + 1, min_len * 2);

		auto draw = [&] (const vector<vec2>& poly, int depth) {
			double len = distance(img.screen_tr.from(poly[1]), img.screen_tr.from(poly[0]));
			double pos = (len-min_len)/(max_len-min_len);

			double alphapos = 1;
			if (len < start_min_len) alphapos = std::min((len - min_len)/(start_min_len-min_len), 1.0);

			//Color start = rgb(0x15, 0x57, 0x99), end = rgb(0x15, 0x99, 0x57);
			//Color start = Miku, end = Red;
			Color start = getColorBetween(0.1, Miku, White), end = Blue;
			//Color start = getColorBetween(0.5, Red, Black), end = Blue;
			//Color start = White, end = Blue;
			//Color start = Black, end = Blue;
			//Color start = Gray, end = getColorBetween(0.3, Miku, Black);
			//Color start = Gray, end = Red;
			//Color start = getColorBetween(0.5, getColorBetween(0.5, Green, Black), getColorBetween(0.5, Yellow, Black)), end = Red;
			Color clr = setAlpha(getColorBetween(
				sqrt(sqrt(pos)),
				//pos,
				end, start),
				//sqrt(sqrt(alphapos))
				alphapos
				* 128);
			img.imgs[depth+1]->setBrush(clr);
			img2.imgs[depth+1]->setBrush(clr);

			{
				auto p1 = fromMas(img.screen_tr, poly);
				Polygon_d p2;
				for (auto& i : p1) p2.array.push_back(i);
				img.imgs[depth+1]->drawPolygon(p2);
			}
			{
				auto p1 = fromMas(img2.screen_tr, poly);
				Polygon_d p2;
				for (auto& i : p1) p2.array.push_back(i);
				img2.imgs[depth+1]->drawPolygon(p2);
			}
		};

		//#ifndef _DEBUG
		draw_pythagoras_tree(standard, draw, draw, [&] (vec2 i, int depth) -> bool {
			return depth > maxDepth || img.screen_tr.fromDir(i).length() < min_len;
		});
		//#endif

		// Рисуем все текущие системы координат
		img2.draw_crd(start, maxDepth+2);
		img2.draw_crd(end, maxDepth+2);
		img2.draw_crd(c, maxDepth+2);

		// Рисуем viewport
		img2.set_pen(2.5/80.0, Black, maxDepth+2);
		img2.draw_polygon(fromMas(d, square), maxDepth+2);

		img2.set_pen(0.5/80.0, Gray, maxDepth+2);

		if (!isExperiment) {
			space2 startcopy = standard;
			double startStandardLength = startcopy.fromDir(standard.i).length();
			double countSplines = 50;
			for (int i = 0; i < countSplines; i++) {
				double countLines = 10;
				for (int j = 0; j < countLines; j++) {
					auto p1 = interpolator.interpolate(j/countLines);
					auto p2 = interpolator.interpolate((j+1)/countLines);

					//auto p1 = interpolate(start, end, j/countLines);
					//auto p2 = interpolate(start, end, (j+1)/countLines);

					//auto p1 = interpolateCircular(start, end, j/countLines, 1.12, 2, true);
					//auto p2 = interpolateCircular(start, end, (j+1)/countLines, 1.12, 2, true);

					img2.set_pen(4/80.0 * startcopy.fromDir(p1.i).length() / startStandardLength, Black, maxDepth+2);
					img2.draw_line(startcopy.from(p1.pos), startcopy.from(p2.pos), maxDepth+2);
				}
				startcopy = space2(end).from(space2(start).to(startcopy));
			}
		}

		img.combine_layers();
		img2.combine_layers();

		gif.process(*img.imgs[0], 2);
		gif2.process(*img2.imgs[0], 2);
	}

	gif.end();
	gif2.end();
}

//-----------------------------------------------------------------------------
double gauss_kernel(double x) {
	// x in [-1, 1],
	x *= 3;
	return std::exp(-x*x/2.0);
}

//-----------------------------------------------------------------------------
void draw_interpolation() {
	crd2 standard = getStandardCrd2();

	// Начальные расчеты
	space2 a = standard;
	space2 b = standard; b.move(vec2(3, 3)); b = rotate(b, b.pos, spob::deg2rad(70)); b.i *= 1.1; b.j *= 1.1;
	space2 c = standard; c.move(vec2(5, 4)); c = rotate(c, c.pos, spob::deg2rad(-15)); c.i *= 0.3; c.j *= 0.3;

	glm::mat3 A = getFromMatrix(a);
	glm::mat3 B = getFromMatrix(b);
	glm::mat3 C = getFromMatrix(c);

	std::vector<double> points = {0, 1, 2};
	SplineInterpolator3 splinea(points, 0, 5);
	SplineInterpolator3 splineb(points, 1, 5);
	SplineInterpolator3 splinec(points, 2, 5);

	//MatrixPowerInterpolator interpolator(a, b);

	//-------------------------------------------------------------------------
	// Цикл расчетов
	double count = 60;
	std::vector<space2> counts;
	for (int i = 0; i <= 2*count; i++) {
		double t = i/count;

		// Подход на основе полиномов Лагранжа и возведении матрицы в степень
		/*double ta =  (t*t-3*t+2)/2.0; // 0,1 ; 1,0 ; 2,0
		double tb = 2*t-t*t; // 0,0 ; 1,1 ; 2,0
		double tc = (t*t-t)/2.0; // 0,0 ; 1,0 ; 2,1
		glm::mat3 P = pow(C, tc) * pow(B, tb) * pow(A, ta);*/

		// Подход на основе ядерных функций и возведении матрицы в степень
		/*double ta =  gauss_kernel(t); // 0,1 ; 1,0 ; 2,0
		double tb = gauss_kernel(t-1); // 0,0 ; 1,1 ; 2,0
		double tc = gauss_kernel(t-2); // 0,0 ; 1,0 ; 2,1
		glm::mat3 P = pow(C, tc) * pow(B, tb) * pow(A, ta);*/

		double ta = splinea.interpolate(t);
		double tb = splineb.interpolate(t);
		double tc = splinec.interpolate(t);
		glm::mat3 P = pow(C, tc) * pow(B, tb) * pow(A, ta);
		counts.push_back(getToCrd(P));

		//counts.push_back(interpolate(a, b, t));
		//counts.push_back(interpolateCircular(a, b, t, 5, 2));
		//counts.push_back(interpolator.interpolate(t));
	}

	//-------------------------------------------------------------------------
	// Расчет окна видимости и размера изображения
	space2 viewport = standard;
	viewport.pos -= viewport.fromDir(vec2(1, 1)/2.0);
	viewport.j *= 7;
	viewport.i *= 10;

	double coef = viewport.i.length() / viewport.j.length();
	double sz = 1000;
	vec2 size;
	if (coef > 1)
		size = vec2(sz, sz / coef);
	else
		size = vec2(sz * coef, sz);

	//-------------------------------------------------------------------------
	// Инициализируем все изображения
	ImageGif gif;
	Image img(size, viewport);
	img.setViewPort(viewport);

	gif.start(img.imgs[0]->size(), "interpolation.gif");

	//-------------------------------------------------------------------------
	// Цикл рисования
	for (int i = 0; i < counts.size(); i++) {
		//img.setViewPort(counts[i]);

		// Рисуем сетку
		img.clear(White);
		img.set_pen(1.5/80.0, setAlpha(Gray, 192));
		img.draw_grid(standard);
		img.set_pen(1.5/80.0, setAlpha(Gray, 192));
		img.draw_crd(standard);

		img.set_pen(1/80.0, Black);
		vec2 lastPos = a.pos;
		for (auto& j : counts) {
			img.set_alpha(16);
			img.draw_crd(j);
			img.set_alpha(255);
			img.draw_line(lastPos, j.pos);
			lastPos = j.pos;
		}
		for (auto j : counts) {
			j = c.from(j);

			img.set_alpha(16);
			img.draw_crd(j);
			img.set_alpha(255);
			img.draw_line(lastPos, j.pos);
			lastPos = j.pos;
		}
		for (auto j : counts) {
			j = c.from(c.from(j));

			img.set_alpha(16);
			img.draw_crd(j);
			img.set_alpha(255);
			img.draw_line(lastPos, j.pos);
			lastPos = j.pos;
		}
		img.set_alpha(255);

		// Рисуем все текущие системы координат
		img.set_pen(3/80.0, Black);
		img.draw_crd(a);
		img.draw_crd(b);
		img.draw_crd(c);
		img.draw_crd(counts[i]);

		gif.process(*img.imgs[0], 2);
	}

	gif.end();
}

//-----------------------------------------------------------------------------
int main() {
	//draw_animation();
	draw_interpolation();
}


#include "arap_material.h"
#include "main.h"
#include "utils.h"

#include <Eigen/Dense>
#include <Eigen/SVD>

using namespace materials;

namespace {
template <int dim, typename T>
void svd_w(const Eigen::Matrix<T, dim, dim>& mat_inp,
           Eigen::Matrix<T, dim, dim>& U, Eigen::Matrix<T, dim, 1>& S,
           Eigen::Matrix<T, dim, dim>& V) {
    using Mat = Eigen::Matrix<T, dim, dim>;
    Eigen::JacobiSVD<Mat> svd{mat_inp,
                              Eigen::ComputeFullU | Eigen::ComputeFullV};
    S = svd.singularValues();
    U = svd.matrixU();
    V = svd.matrixV();
    if ((U.determinant() < 0) != (V.determinant() < 0)) {
#if 0
        // code copied from libsan SVDW to maintain consistency
        constexpr double EPS = 1e-3;
        int best_idx = -1, best_idx_nr = dim + 1;
        for (size_t i = 0; i < dim; ++i) {
            size_t j = i + 1;
            // ms already sorted
            while (j < dim && std::fabs(S(i) - S(j)) < EPS) {
                ++j;
            }
            int nr = j - i;
            // best case is to negate an odd number of smallest singular
            // values (so si+sj != 0 in the hessian);
            // otherwise negate one value whose has the least
            // repetitionss
            if (nr <= best_idx_nr || (nr == best_idx_nr + 1 && nr % 2 == 1)) {
                best_idx = i;
                best_idx_nr = nr;
                if (nr == 1) {
                    break;
                }
            }
            i = j;
        }
        if (best_idx_nr == 1 || best_idx_nr % 2 == 0) {
            U.col(best_idx) = -U.col(best_idx);
            S(best_idx) = -S(best_idx);
        } else {
            for (int i = best_idx; i < best_idx + best_idx_nr; ++i) {
                U.col(i) = -U.col(i);
                S(i) = -S(i);
            }
        }
#else
        U.col(dim - 1) = -U.col(dim - 1);
        S(dim - 1) = -S(dim - 1);
#endif
    }
}
}  // namespace

template <int dim, typename T>
T ARAPElasticityMaterial<dim, T>::EnergyDensity(const MatrixDimT& F) const {
    MatrixDimT U, V, R;
    Eigen::Matrix<T, dim, 1> S;
    svd_w(F, U, S, V);
    R.noalias() = U * V.transpose();
    return (F - R).squaredNorm() * (this->mu() * 0.5);
}

template <int dim, typename T>
typename ARAPElasticityMaterial<dim, T>::MatrixDimT
ARAPElasticityMaterial<dim, T>::StressTensor(const MatrixDimT& F) const {
    MatrixDimT U, V, R;
    Eigen::Matrix<T, dim, 1> S;
    svd_w(F, U, S, V);
    R.noalias() = U * V.transpose();
    return (F - R) * this->mu();
}

template <int dim, typename T>
typename ARAPElasticityMaterial<dim, T>::MatrixDimT
ARAPElasticityMaterial<dim, T>::StressDifferential(const MatrixDimT& F,
                                                   const MatrixDimT& dF) const {
    throw std::runtime_error{"unimplemented"};
}

template <int dim, typename T>
typename ARAPElasticityMaterial<dim, T>::MatrixDim2T
ARAPElasticityMaterial<dim, T>::StressDifferential(const MatrixDimT& F) const {
    cf_assert(dim == 3);
    MatrixDimT U, V;
    Eigen::Matrix<T, dim, 1> S;
    svd_w(F, U, S, V);
    Eigen::Matrix<T, 3, 3> T0, T1, T2;
    T0 << 0, -1, 0, 1, 0, 0, 0, 0, 0;
    T0 = std::sqrt(0.5) * U * T0 * V.transpose();
    T1 << 0, 0, 0, 0, 0, 1, 0, -1, 0;
    T1 = std::sqrt(0.5) * U * T1 * V.transpose();
    T2 << 0, 0, 1, 0, 0, 0, -1, 0, 0;
    T2 = std::sqrt(0.5) * U * T2 * V.transpose();

    Eigen::Map<Eigen::Matrix<T, 9, 1>> t0{T0.data()}, t1{T1.data()},
            t2{T2.data()};
    T s0 = S(0), s1 = S(1), s2 = S(2);
    Eigen::Matrix<T, 9, 9> H;
    H.setIdentity();
    T (*clip)(T);
    if (baseline::g_hessian_proj) {
        clip = [](T x) { return std::max<T>(x, 2); };
    } else {
        clip = [](T x) { return x; };
    }
    H.noalias() -= 2 / (clip(s0 + s1)) * t0 * t0.transpose();
    H.noalias() -= 2 / (clip(s1 + s2)) * t1 * t1.transpose();
    H.noalias() -= 2 / (clip(s0 + s2)) * t2 * t2.transpose();
    return H * this->mu();
}

template class materials::ARAPElasticityMaterial<3, double>;


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_PIO_180_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_PIO_180_HPP_INCLUDED
/*!
 * \file
**/
#include <boost/simd/sdk/constant/constant.hpp>
#include <boost/simd/sdk/constant/register.hpp>

/*!
 * \ingroup trigo_constant
 * \defgroup trigo_constant_pio_180 pio_180 constant
 *
 * \par Description
 * Constant pio_180 : \f$\frac\pi{180}\f$.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/pio_180.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T,class A0>
 *     meta::call<tag::_pio_180_(A0)>::type
 *     pio_180();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Pio_180
 *
 * \return type T value
 *
 **/

namespace nt2
{
  namespace tag
  {
    BOOST_SIMD_CONSTANT_REGISTER( Pio_180, double
                                , 0, 0x3c8efa35
                                , 0x3f91df46a2529d3all
                                );
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Pio_180, Pio_180);
}

#endif


#pragma once
#include <boost/iterator/iterator_facade.hpp>
#include <cmath>
#include <cstdio>
#include <cassert>
#include <stdexcept>
#include <limits>

namespace occgrid {
template <typename T>
inline int signum(T val) {
    return (T(0) < val) - (val < T(0));
}

template <typename real_t, typename int_t>
class ray_trace_iterator
  : public
    boost::iterator_facade<
    ray_trace_iterator<real_t, int_t>
    , std::pair<int_t, int_t>
    , boost::forward_traversal_tag
    , std::pair<int_t, int_t>
    >
{
    private:
      typedef typename boost::iterator_facade<
        ray_trace_iterator<real_t, int_t>
        , std::pair<int_t, int_t>
        , boost::forward_traversal_tag
        , std::pair<int_t, int_t>
        > super_t;
      // Input arguments
      real_t
        px_,
        py_,
        dx_,
        dy_,
        origin_x_,
        origin_y_,
        cell_size_x_,
        cell_size_y_;

      // intermediate variables for faster computation
      int_t dirx_, diry_; /// (-1, 0, 1) integral steps (direction)
      real_t ex_, ey_; /// distance to the nearest grid line
      real_t Tx_, Ty_; /// Maximum time to collision (from one grid line to next)

      // State of iterator
      int_t i_, j_; /// Grid index
      real_t tx_, ty_; /// time to collision to next grid line
    public:
      ray_trace_iterator(
          real_t px,
          real_t py,
          real_t dx,
          real_t dy,
          real_t origin_x,
          real_t origin_y,
          real_t cell_size_x,
          real_t cell_size_y
          ) :
        px_(px),
        py_(py),
        dx_(dx),
        dy_(dy),
        origin_x_(origin_x),
        origin_y_(origin_y),
        cell_size_x_(cell_size_x),
        cell_size_y_(cell_size_y)
      {
        using std::floor;
        using std::fabs;
        // shift coordinates
        px = px - origin_x;
        py = py - origin_y;

        // current grid cell
        i_ = static_cast<int_t>(floor(px / cell_size_x));
        j_ = static_cast<int_t>(floor(py / cell_size_y));

        dirx_ = signum(dx);
        diry_ = signum(dy);

        // whether the grid line we are going to hit is floor() or ceil()
        // depends on the direction ray is moving
        // using the fact that ceil() = floor() + 1
        int_t floor_or_ceilx = (dirx_ > 0) ? 1 : 0;
        int_t floor_or_ceily = (diry_ > 0) ? 1 : 0;
#ifdef DEBUG
        printf("cell: (%i, %i), dxdy:(%f, %f)\n", i, j, dx, dy);
        //std::cout << "cell size:" << cell_size_ << "pos:" << position << std::endl;
#endif
        // distance to nearest grid line
        ex_ = fabs((i_ + floor_or_ceilx) * cell_size_x - px);
        ey_ = fabs((j_ + floor_or_ceily) * cell_size_y - py);

        // (max) time to collision from one grid line to another
        Tx_ = (dx == 0) ? std::numeric_limits<real_t>::infinity() : cell_size_x / fabs(dx);
        Ty_ = (dy == 0) ? std::numeric_limits<real_t>::infinity() : cell_size_y / fabs(dy);

        // time to collision from this position
        tx_ = (dx == 0) ? std::numeric_limits<real_t>::infinity() : ex_ / fabs(dx);
        ty_ = (dy == 0) ? std::numeric_limits<real_t>::infinity() : ey_ / fabs(dy);

        if ( ! ((tx_ >= 0) && (ty_ >= 0))) {
          printf("t:(%f, %f), direction:(%f, %f), position:(%f, %f), cell:(%d, %d), cellsize:(%f, %f)\n",
              tx_, ty_, dx, dy, px, py, i_, j_, cell_size_x, cell_size_y);
          throw std::logic_error("tx < 0 or ty < 0");
        }

        // time is always positive
        assert(tx_ >= 0);
        assert(ty_ >= 0);
      }

      typename super_t::reference dereference() const {
        return std::make_pair(i_, j_);
      }

      bool equal(ray_trace_iterator it) const {
        return ((it.i_ == i_) && (it.j_ == j_) &&
          (it.tx_ == tx_) && (it.ty_ == ty_) &&
          (it.Tx_ == Tx_) && (it.Ty_ == Ty_) &&
          (it.dirx_ == dirx_) && (it.diry_ == diry_));
      }

      void increment() {
          if (tx_ < ty_) {
            i_ += dirx_;
            ty_ = ty_ - tx_;
            tx_ = Tx_;
          } else {
            j_ += diry_;
            tx_ = tx_ - ty_;
            ty_ = Ty_;
          }
      }

      std::pair<real_t, real_t>
        real_position() const {

          // whether the grid line we are going to hit is floor() or ceil()
          // depends on the direction ray is moving
          int_t floor_or_ceilx = (dirx_ > 0) ? 1 : 0;
          int_t floor_or_ceily = (diry_ > 0) ? 1 : 0;

          real_t ex = (dx_ == 0) ? ex_ // error is same as starting point
            : tx_ * fabs(dx_);
          real_t ey = (dy_ == 0) ? ey_
            : ty_ * fabs(dy_);

          real_t px = (i_ + floor_or_ceilx) * cell_size_x_ - ex * dirx_;
          real_t py = (j_ + floor_or_ceily) * cell_size_y_ - ey * diry_;

          // shift coordinates
          px = px + origin_x_;
          py = py + origin_y_;

          return std::make_pair(px, py);
      }
};
} // namespace occgrid


// Run like this
//./generate --data competition/S1a/S1b_short_dataset1_training.csv
// This code will forecast 4 unknown parameters of the Matern covariance matrix
// Developed by Alexander Litvinenko (RWTH Aachen) and Ronald Kriemann (MIS MPG Leipzig)
// Based on the HLIBPro library (v. 2.9) www.hlibpro.com
// No warranties.

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

#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/bessel.hpp>

#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_gamma.h>

#include <gsl/gsl_multimin.h>

#include "hlib.hh"

using namespace std;
using boost::format;
using namespace HLIB;
using namespace boost::program_options;
using  real_t    = HLIB::real;

enum {
    IDX_SIGMA  = 0,
    IDX_LENGTH = 1,
    IDX_NU     = 2,
    IDX_TAU    = 3
};

//Use a method described by Abramowitz and Stegun:
double gaussrand_Stegun()
{
    static double U, V;
    static int phase = 0;
    double Z;

    if(phase == 0) {
        U = (rand() + 1.) / (RAND_MAX + 2.);
        V = rand() / (RAND_MAX + 1.);
        Z = sqrt(-2 * log(U)) * sin(2 * M_PI * V);
    } else
        Z = sqrt(-2 * log(U)) * cos(2 * M_PI * V);

    phase = 1 - phase;

    return Z;
}

//Use a method discussed in Knuth and due originally to Marsaglia:

double gaussrand_Knuth()
{
    static double V1, V2, S;
    static int phase = 0;
    double X;

    if(phase == 0) {
        do {
            double U1 = (double)rand() / RAND_MAX;
            double U2 = (double)rand() / RAND_MAX;

            V1 = 2 * U1 - 1;
            V2 = 2 * U2 - 1;
            S = V1 * V1 + V2 * V2;
        } while(S >= 1 || S == 0);

        X = V1 * sqrt(-2 * log(S) / S);
    } else
        X = V2 * sqrt(-2 * log(S) / S);

    phase = 1 - phase;

    return X;
}

// global options
int        nmin      = CFG::Cluster::nmin;
double     eps       = 1e-6;
double     fac_eps   = 1e-6;
double     shift     = 1e-7;
bool       use_ldl   = false;

//
// read dataset from file
//
void
read_data ( const std::string &       datafile,
            std::vector< T2Point > &  vertices,
            BLAS::Vector< double > &  Z_data )
{
    std::ifstream  in( datafile );

    if ( ! in ) // error
        exit( 1 );

    size_t  N_vtx = 0;

    #if 1

    std::string  line;

    std::getline( in, line );

    if ( line == "x,y" )
    {
        std::list< T2Point >  pos;
        std::list< double >   vals;

        while ( std::getline( in, line ) )
        {
            auto    parts = split( line, "," );
            double  x = atof( parts[0].c_str() );
            double  y = atof( parts[1].c_str() );

            pos.push_back( T2Point( x, y ) );
        }// while

        N_vtx = pos.size();

        std::cout << "learning dataset" << std::endl;
        std::cout << N_vtx << std::endl;

        vertices.resize( N_vtx );
        Z_data = BLAS::Vector< double >( N_vtx );

        int  i = 0;

        for ( auto  p : pos )
            vertices[ i++ ] = p;

        i = 0;

        for ( idx_t  i = 0; i < idx_t(N_vtx); ++i )
           Z_data( i++ ) =  gaussrand_Knuth(); //gaussrand_Stegun() ;
    }// if
    else
    {
        std::cout << "you should not be here, something is wrong with the input file" << std::endl;
        HERROR( ERR_NOT_IMPL, "", "" );
    }

    #else

    in >> N_vtx;

    std::cout << "reading " << N_vtx << " datapoints" << std::endl;

    vertices.resize( N_vtx );
    Z_data = BLAS::Vector< double >( N_vtx );

    for ( idx_t  i = 0; i < idx_t(N_vtx); ++i )
    {
        int     index = i;
        double  x, y, z;
        // double  v     = 0.0;

        in >> index >> x >> y >> z;
        // in >> index >> x >> y >> z >> v;

        vertices[ index ] = T2Point( x, y );
        //Z_data( index )   = v;
    }// for

    #endif

    //
    // for visualization of data, export 2D points with v value in csv file
    //

    // std::ofstream  out( "data.csv" );

    // out << "x,y,z,v" << std::endl;
    // out << "x,y,z" << std::endl;

    //for ( uint  i = 0; i < N_vtx; ++i )
    //   out << vertices[i].x() << "," << vertices[i].y() << ",0" << std::endl;
//      out << vertices[i].x() << "," << vertices[i].y() << ",0," << Z_data( i ) << std::endl;
}

//
// define PredictionProblem to forecast unknown values in new locations
//
struct GeneratingProblem
{
    std::vector< T2Point >                vertices;
    std::unique_ptr< TCoordinate >        coord;
    std::unique_ptr< TClusterTree >       ct;
    std::unique_ptr< TBlockClusterTree >  bct;
    std::unique_ptr< TVector >            Z;

    GeneratingProblem ( const std::string &  datafile )
    {
        init( datafile );
    }

    void
    init ( const std::string &  datafile )
    {
        BLAS::Vector< double >  Z_data;

        read_data( datafile, vertices, Z_data );
        std::cout << "the grid is successfully read" << std::endl;
        coord = std::make_unique< TCoordinate >( vertices );

        TAutoBSPPartStrat  part_strat;
        TBSPCTBuilder      ct_builder( & part_strat, nmin );

        ct = ct_builder.build( coord.get() );
        //print_vtk( & coord, "ct_coord" );
        //print_vtk( & coord_predict, "ct_coord_predict" );

        TStdGeomAdmCond    adm_cond( 2.0, use_min_diam );
        TBCBuilder         bct_builder;

        bct = bct_builder.build( ct.get(), ct.get(), & adm_cond );
        Z   = std::make_unique< TScalarVector >( *ct->root(), std::move( Z_data ) );
        ct->perm_e2i()->permute( Z.get() );
  }

    //BLAS::Vector< double >
    std::unique_ptr< TVector >
    eval ( const double  sigma,
           const double  length,
           const double  nu,
           const double tau )
    {

        TMaternCovCoeffFn< T2Point >  matern_coefffn( sigma, length, nu,  vertices );
        TPermCoeffFn< double >        coefffn( & matern_coefffn, ct->perm_i2e(), ct->perm_i2e() );

        TACAPlus< double >            aca( & coefffn );
        auto                          acc = fixed_prec( eps );
        TDenseMatBuilder< double >    h_builder( & coefffn, & aca );

        auto                          C        = h_builder.build( bct.get(), acc );
        TPSMatrixVis  mvis;

         //mvis.svd(true).print( C.get(), "myC" );

//        print_ps(bct->root(), "bct.eps");
//        print_ps(bct_predict->root(), "bct_predict.eps");

        //       mvis.svd(true).print( C_predict.get(), "myC_predict" );

        //if ( shift != 0.0 )
        //    add_identity( C.get(), tau*tau );

        auto                          fac_acc  = fixed_prec( fac_eps );
        auto                          C_fac    = C->copy();
        auto                          fac_opts = fac_options_t{ point_wise, CFG::Arith::storage_type, false };

        chol( C_fac.get(), fac_acc );

        //std::cout << "    |С|_F             = " << norm_F( C.get() ) << std::endl;
        //std::cout << "    |L|_F             = " << norm_F( C_fac.get() ) << std::endl;
        //std::cout << "    |С|_2             = " << norm_2( C.get() ) << std::endl;
        //std::cout << "    |L|_2             = " << norm_2( C_fac.get() ) << std::endl;
        //mvis.svd(true).print( C_fac.get(), "myL" );

        //std::unique_ptr< TFacInvMatrix >   C_inv;

        const size_t                  N     = vertices.size();

        auto                          Z_generated = C->row_vector();

      //  std::cout << "  size of C = " << C->rows() << "x"<< C->cols() << std::endl;
      //  std::cout << "  size of C_predict = " << C_predict->rows() <<"x"<<  C_predict->cols() << std::endl;
      //  std::cout << "  sol size = " << sol->size() << std::endl;
      //  std::cout << "  ||sol|| = " << sol->norm2() << std::endl;
      //  std::cout << "  ||Z_predict|| = " << Z_predict->norm2() << std::endl;

        //auto                          ZdotCZ = std::real( Z->dot( sol.get() ) );
        //Z_predict = C_predict * sol.get();
        mul_vec( real_t(1), C_fac.get(), Z.get(), real_t(0), Z_generated.get(), apply_normal );
        //C_predict->mul_vec( 1.0, sol.get(), 0.0, Z_predict.get(), apply_normal );

        ct->perm_i2e()->permute( Z_generated.get() );
        std::cout << "  ||Z_generated|| = " << Z_generated->norm2() << std::endl;

        //TMatlabVectorIO  vio;

        //vio.write( Z_predict,  "x.mat", "x" );

        FILE* f1;
        f1 = fopen("111gen_d.txt", "w");

        for ( size_t  i = 0; i < Z_generated->size(); i++ )
          fprintf(f1," %6.6e, %6.6e, %6.6e\n",   vertices[i].x(),  vertices[i].y(), Z_generated->entry(i));
        fclose(f1);
        return std::move( Z_generated );
    }
};

//
// wrapper from GSL to LogLikeliHoodProblem
//
/*double
  eval_logli ( const gsl_vector *  param,
  void *              data )
  {
  double sigma  = gsl_vector_get( param, IDX_SIGMA );
  double length = gsl_vector_get( param, IDX_LENGTH );
  double nu     = gsl_vector_get( param, IDX_NU );
  double tau    = gsl_vector_get( param, IDX_TAU );

  LogLikeliHoodProblem *  problem = static_cast< LogLikeliHoodProblem * >( data );

  return - problem->eval( sigma, length, nu, tau );
  }
*/
//
// optimization function using GSL
//

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

    CFG::set_verbosity( 3 );
    INIT();

    //std::string  datafile = "datafile.txt";
    //std::string  datafile_predict = "datafile_predict.txt";
    std::string  datafile = "grid.txt";

    //
    // define command line options
    //

    options_description             all_opts;
    options_description             vis_opts( "usage: generatig [options] datafile\n  where options include" );
    options_description             hid_opts( "Hidden options" );
    positional_options_description  pos_opts;
    variables_map                   vm;

    // standard options
    vis_opts.add_options()
        ( "help,h",                       ": print this help text" )
        ( "threads,t",   value<int>(),    ": number of parallel threads" )
        ( "verbosity,v", value<int>(),    ": verbosity level" )
        ( "nmin",        value<int>(),    ": set minimal cluster size" )
        ( "eps,e",       value<double>(), ": set H accuracy" )
        ( "epslu",       value<double>(), ": set only H factorization accuracy" )
        ( "shift",       value<double>(), ": regularization parameter" )
        ( "ldl",                          ": use LDL factorization" )
        ;

    hid_opts.add_options()
        ( "data",        value<std::string>(), ": datafile " );

    // options for command line parsing
    all_opts.add( vis_opts ).add( hid_opts );

    // all "non-option" arguments should be "--data" arguments
    pos_opts.add( "data", -1 );

    //
    // parse command line options
    //

    try
    {
        store( command_line_parser( argc, argv ).options( all_opts ).positional( pos_opts ).run(), vm );
        notify( vm );
    }// try
    catch ( required_option &  e )
    {
        std::cout << e.get_option_name() << " requires an argument, try \"-h\"" << std::endl;
        exit( 1 );
    }// catch
    catch ( unknown_option &  e )
    {
        std::cout << e.what() << ", try \"-h\"" << std::endl;
        exit( 1 );
    }// catch

    //
    // eval command line options
    //

    if ( vm.count( "help") )
    {
        std::cout << vis_opts << std::endl;
        exit( 1 );
    }// if

    if ( vm.count( "nmin"      ) ) nmin     = vm["nmin"].as<int>();
    if ( vm.count( "eps"       ) ) eps      = vm["eps"].as<double>();
    if ( vm.count( "epslu"     ) ) fac_eps  = vm["epslu"].as<double>();
    if ( vm.count( "shift"     ) ) shift    = vm["shift"].as<double>();
    if ( vm.count( "threads"   ) ) CFG::set_nthreads( vm["threads"].as<int>() );
    if ( vm.count( "verbosity" ) ) CFG::set_verbosity( vm["verbosity"].as<int>() );
    if ( vm.count( "ldl"       ) ) use_ldl  = true;

    // default to general eps
    if ( fac_eps == -1 )
        fac_eps = eps;

    if ( vm.count( "data" ) )
        datafile = vm["data"].as<std::string>();
    else
    {
        std::cout << "usage: generating [options] datafile" << std::endl;
        exit( 1 );
    }// if

    double  sigma  = 2.0; //take these values from previous experiments (Part 1a)
    double  length = 0.1;
    double  nu     = 0.5;
    double  tau    = 0.0;

    GeneratingProblem  problem( datafile );
    problem.eval( sigma, length, nu, tau);

    DONE();
}


#include "Common.h"
#include "CPS4.h"
#include "PropertiesHolder/PropertiesHolder.h"
#include "Material.h"
#include <Eigen/Dense>

#include "GaussQuadrature.h"

Eigen::Matrix<float, 4, 4> CPS4::m_C;
Eigen::Matrix<float, 4, 4> CPS4::m_IC;

float XI[4] = { -1.0, 1.0, 1.0, -1.0 };
float ETA[4] = { -1.0, -1.0, 1.0, 1.0 };

void CPS4::Init()
{
	//x0: -1; x1: 1; x2: 1; x3: -1
	//y0: -1; y1: -1; y2: 1; y3: 1
	m_C <<	Eigen::Vector4f(1.0,				1.0,				1.0,				1.0),
			Eigen::Vector4f(XI[0],				XI[1],				XI[2],				XI[3]),			// x0, x1, x2, x3
			Eigen::Vector4f(ETA[0],				ETA[1],				ETA[2],				ETA[3]),		// y0, y1, y2, y3
			Eigen::Vector4f(XI[0] * ETA[0],		XI[1] * ETA[1],		XI[2] * ETA[2],		XI[3] * ETA[3]);		// x0y0, x1y1, x2y2, x3y3
	m_IC = m_C.inverse();
}

void CPS4::SetIndices(const std::vector<int>& indices)
{
	assert(indices.size() == 4);
	m_nodes[0] = indices[0];
	m_nodes[1] = indices[1];
	m_nodes[2] = indices[2];
	m_nodes[3] = indices[3];
}

std::vector<int> CPS4::GetIndices() const
{
	std::vector<int> indices(4);
	indices[0] = m_nodes[0];
	indices[1] = m_nodes[1];
	indices[2] = m_nodes[2];
	indices[3] = m_nodes[3];
	return indices;
}

std::vector<Eigen::Vector3f> CPS4::GetFunctionValuesAtNodes(const Eigen::VectorXf& deforms)const
{
	Eigen::Matrix<float, 8, 1> uv;
	std::vector<Eigen::Vector3f> output;
	for (int i = 0; i < 4; ++i)
	{
		uv[2 * i + 0] = deforms[2 * m_nodes[i] + 0];
		uv[2 * i + 1] = deforms[2 * m_nodes[i] + 1];
	}

	for (int i = 0; i < 4; i++)
	{
		Eigen::Matrix<float, 3, 8> B = GetB(XI[i], ETA[i] );
		Eigen::Vector3f strain = B * uv;
		output.push_back(strain);
	}
	return output;
}

void CPS4::CalcK(const StrideDataArray& nodes, const tfem::MaterialPtr mat, std::vector<Eigen::Triplet<float> >& tripletVector)
{
	m_mat = mat;
	Eigen::Vector4f X;
	Eigen::Vector4f Y;
	for (int i = 0; i < 4; ++i)
	{
		X[i] = nodes(m_nodes[i], 0);
		Y[i] = nodes(m_nodes[i], 1);
	}

	m_KX = m_IC * X;
	m_KY = m_IC * Y;

	float area = 0;
	Eigen::Matrix<float, 8, 8> K;
	K.setZero();

	float xi, eta, w1, w2;
	for (int i = 0; GaussQuadrature::GetWeights<2>(i, xi, w1); i++)
	{
		for (int j = 0; GaussQuadrature::GetWeights<2>(j, eta, w2); j++)
		{
			float w = w1 * w2;
			Eigen::Matrix<float, 2, 2> J = GetJ(xi, eta);

			Eigen::Matrix<float, 3, 8> B = GetB(xi, eta);

			K += B.transpose() * mat->GetElasticityMatrix(fem::PT_FlatStress) * B * J.determinant() * w;
			area += J.determinant() * w;
		}
	}

	GrabTriplets(K, tripletVector);
}

void CPS4::GrabTriplets(const Eigen::Matrix<float, 8, 8>& K, std::vector<Eigen::Triplet<float> >& tripletVector) const
{
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			Eigen::Triplet<float> trplt11(2 * m_nodes[i] + 0, 2 * m_nodes[j] + 0, K(2 * i + 0, 2 * j + 0));
			Eigen::Triplet<float> trplt12(2 * m_nodes[i] + 0, 2 * m_nodes[j] + 1, K(2 * i + 0, 2 * j + 1));
			Eigen::Triplet<float> trplt21(2 * m_nodes[i] + 1, 2 * m_nodes[j] + 0, K(2 * i + 1, 2 * j + 0));
			Eigen::Triplet<float> trplt22(2 * m_nodes[i] + 1, 2 * m_nodes[j] + 1, K(2 * i + 1, 2 * j + 1));

			tripletVector.push_back(trplt11);
			tripletVector.push_back(trplt12);
			tripletVector.push_back(trplt21);
			tripletVector.push_back(trplt22);
		}
	}
}

tfem::Material* CPS4::GetMaterial()
{
	return m_mat.get();
}

IElement* CPS4::Create()
{
	return new CPS4;
}

CPS4::CPS4()
{

}

Eigen::Matrix<float, 1, 4> CPS4::GetP(float xi, float eta) const
{
	return Eigen::Matrix<float, 1, 4>(1, xi, eta, xi * eta);
}

Eigen::Matrix<float, 1, 4> CPS4::GetdPdxi(float xi, float eta) const
{
	return Eigen::Matrix<float, 1, 4>(0, 1, 0, eta);
}

Eigen::Matrix<float, 1, 4> CPS4::GetdPdeta(float xi, float eta) const
{
	return Eigen::Matrix<float, 1, 4>(0, 0, 1, xi);
}

Eigen::Matrix<float, 2, 2> CPS4::GetJ(float xi, float eta) const
{
	float dxdxi = GetdPdxi(xi, eta) * m_KX;
	float dydxi = GetdPdxi(xi, eta) * m_KY;
	float dxdeta = GetdPdeta(xi, eta) * m_KX;
	float dydeta = GetdPdeta(xi, eta) * m_KY;
	Eigen::Matrix<float, 2, 2> result;
	result << dxdxi, dydxi, dxdeta, dydeta;
	return result;
}

Eigen::Matrix<float, 3, 8> CPS4::GetB(float xi, float eta) const
{
	Eigen::Matrix<float, 3, 8> B;
	Eigen::Matrix<float, 2, 2> J = GetJ(xi, eta);
	Eigen::Matrix<float, 2, 2> IJ = J.inverse();

	Eigen::Matrix<float, 1, 4> dNdxi = GetdPdxi(xi, eta) * m_IC;
	Eigen::Matrix<float, 1, 4> dNdeta = GetdPdeta(xi, eta) * m_IC;

	for (int k = 0; k < 4; ++k)
	{
		Eigen::Matrix<float, 2, 1> dNkdxieta(dNdxi[k], dNdeta[k]);
		Eigen::Matrix<float, 2, 1> dNkdxy = IJ * dNkdxieta;
		B(0, 2 * k + 0) = dNkdxy[0];
		B(0, 2 * k + 1) = 0;
		B(1, 2 * k + 0) = 0;
		B(1, 2 * k + 1) = dNkdxy[1];
		B(2, 2 * k + 0) = dNkdxy[1];
		B(2, 2 * k + 1) = dNkdxy[0];
	}
	return B;
}


/**
 * @file 	ShortestPathHeuristic.cpp
 * @author 	Fabian Wegscheider
 * @date 	Jul 10, 2017
 */

#include <boost/heap/fibonacci_heap.hpp>
#include "MyDijkstra.h"
#include "ShortestPathHeuristic.h"

using Pair = std::pair<int, double>;
using std::vector;

/**
 * Data that is stored in one node of a heap. Contains an integer and a double.
 * Comparisons are made by the double, smaller has higher priority
 */
struct heap_data
{
    heap::fibonacci_heap<heap_data>::handle_type handle;
    Pair pair;

    heap_data(Pair p):
        pair(p)
    {}

    bool operator<(heap_data const & rhs) const {
        return pair.second > rhs.pair.second;
    }
};

/*
 * Finds all primes in {2,...,n} using the fact that if a natural number
 * has a divisor it also has at least one prime divisor. the resulting vector
 * contains the numbers -1.
 */
vector<int> ShortestPathHeuristic::findPrimes(int n) {
	assert(n >= 2);

	vector<int> primes;
	primes.push_back(2);

	for (int i = 3; i < n; ++i) {
		bool isPrime = true;
		for (unsigned int j = 0; j < primes.size() && primes[j]*primes[j] <= i; j++) {
			if (i % primes[j] == 0) {
				isPrime = false;
				break;
			}
		}
		if (isPrime) {
			primes.push_back(i);
		}
	}

	for (unsigned int i = 0; i < primes.size(); ++i) {
		primes[i]--;
	}

	return primes;
}

double ShortestPathHeuristic::constructSteinerTree(Graph& g, int numVertices, int source) {
	assert(numVertices >= 2);

	using Heap = heap::fibonacci_heap<heap_data>;

	vector<int> primes = findPrimes(numVertices);
	int numTerminals = primes.size();

	double** distances = new double*[numTerminals];
	int** predecessors = new int*[numTerminals];

	/*shortest paths from all terminals to all other nodes are calculated*/
	for (int i = 0; i < numTerminals; ++i) {
		if (primes[i] != source) {
			distances[i] = new double[numVertices];
			predecessors[i] = new int[numVertices];
			MyDijkstra::computeShortestPaths(g, numVertices, primes[i], distances[i], predecessors[i]);
		} else {
			distances[i] = new double[0];
			predecessors[i] = new int[0];
		}
	}

	/*we use a priority queue to keep track of remaining and closest terminals*/
	Heap heap;
	Heap::handle_type *handles = new Heap::handle_type[numTerminals];

	//all terminals except for the source are added to heap
	for (int i = 0; i < numTerminals; ++i) {
		if (primes[i] != source) {
			handles[i] = heap.push(std::make_pair(i, distances[i][source]));
		}
	}

	//we also keep track of the closest node in the tree for each terminal
	double objectiveValue = 0;
	int nearestNodes[numTerminals];
	for (int i = 0; i < numTerminals; i++) {
		nearestNodes[i] = source;
	}

	while (!heap.empty()) {
		Pair nextTerminal = heap.top().pair;
		int connectionNode = nearestNodes[nextTerminal.first];
		heap.pop();			//top element in heap always is the closest to tree

		objectiveValue += (distances[nextTerminal.first])[connectionNode];

		//now we iterate over all remaining terminals to check whether
		//they have become closer to the new tree
		for (Heap::iterator it = heap.begin(); it != heap.end(); ++it) {
			Pair curr = (*it).pair;
			double min = curr.second;
			int pred = connectionNode;
			bool reachedEnd = false;

			//for each terminal we iterate over all vertices on newly added path
			while (!reachedEnd){
				reachedEnd = (pred == primes[nextTerminal.first]);
				double tmp = (distances[curr.first])[pred];
				if (tmp < min) {
					min = tmp;
					nearestNodes[curr.first] = pred;
				}
				pred = predecessors[nextTerminal.first][pred];
			}

			//update of heap if neccessary
			if (min < curr.second) {
				(*handles[curr.first]).pair.second = min;
				heap.increase(handles[curr.first]);
			}
		} //end of iteration through heap

	} //end of algorithm

	for (int i = 0; i < numTerminals; i++) {
		delete[] distances[i];
		delete[] predecessors[i];
	}
	delete[] distances;
	delete[] predecessors;
	delete[] handles;

	return objectiveValue;

}

double ShortestPathHeuristic::constructSteinerTree(Graph& g, int numVertices) {
	return constructSteinerTree(g, numVertices, 1);
}


#include <fstream>
#include <boost/config.hpp>
#include <boost/version.hpp>
// CGAL headers
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Stream_lines_2.h>
#include <CGAL/Runge_kutta_integrator_2.h>
#include <CGAL/Regular_grid_2.h>

// Qt headers
#include <QtGui>
#include <QString>
#include <QActionGroup>
#include <QFileDialog>
#include <QInputDialog>

// GraphicsView items and event filters (input classes)
#include <CGAL/Qt/StreamLinesGraphicsItem.h>
#include <CGAL/Qt/RegularGridVectorFieldGraphicsItem.h>

// for viewportsBbox
#include <CGAL/Qt/utility.h>
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>
#endif
// the two base classes
#include "ui_Stream_lines_2.h"
#include <CGAL/Qt/DemosMainWindow.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;

typedef CGAL::Regular_grid_2<K> Regular_grid;
typedef CGAL::Runge_kutta_integrator_2<Regular_grid> Runge_kutta_integrator;
typedef CGAL::Stream_lines_2<Regular_grid, Runge_kutta_integrator> Stream_lines;
typedef CGAL::Stream_lines_2<Regular_grid, Runge_kutta_integrator>::Stream_line_iterator_2 Stream_line_iterator;
typedef CGAL::Stream_lines_2<Regular_grid, Runge_kutta_integrator>::Point_iterator_2 Point_iterator;
typedef CGAL::Stream_lines_2<Regular_grid, Runge_kutta_integrator>::Point_2 Point_2;
typedef CGAL::Stream_lines_2<Regular_grid, Runge_kutta_integrator>::Vector_2 Vector;

typedef K::Iso_rectangle_2 Iso_rectangle_2;

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Stream_lines_2
{
  Q_OBJECT

private:
  Stream_lines * stream_lines;
  Runge_kutta_integrator * runge_kutta_integrator;
  Regular_grid * regular_grid;
  double density;
  double ratio;
  double integrating;
  int sampling;
  QGraphicsScene scene;

  CGAL::Qt::StreamLinesGraphicsItem<Stream_lines,K> * sli;
  CGAL::Qt::RegularGridVectorFieldGraphicsItem<Regular_grid,K> * rgi;

public:
  MainWindow();

public Q_SLOTS:

  void on_actionLoadPoints_triggered();

  void on_actionClear_triggered();

  void on_actionSavePoints_triggered();

  void on_actionRecenter_triggered();

  virtual void open(QString fileName);

private:
  void generate();

Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow(), density(12.0), ratio(1.6), integrating(1.0), sampling(1)
{
  setupUi(this);

  this->graphicsView->setAcceptDrops(false);

  // Manual handling of actions
  //

  QObject::connect(this->actionQuit, SIGNAL(triggered()),
		   this, SLOT(close()));

  //
  // Setup the scene and the view
  //
  scene.setItemIndexMethod(QGraphicsScene::NoIndex);
  scene.setSceneRect(-100, -100, 100, 100);
  this->graphicsView->setScene(&scene);

  // Turn the vertical axis upside down
  this->graphicsView->matrix().scale(1, -1);

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Stream_lines_2.html");
  this->addAboutCGAL();

  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)),
	  this, SLOT(open(QString)));
}

/*
 *  Qt Automatic Connections
 *  https://doc.qt.io/qt-5/designer-using-a-ui-file.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */

void
MainWindow::on_actionClear_triggered()
{
  Q_EMIT( changed());
}

void
MainWindow::generate()
{
  stream_lines = new Stream_lines(*regular_grid, *runge_kutta_integrator, density, ratio, sampling);

  sli = new CGAL::Qt::StreamLinesGraphicsItem<Stream_lines, K>(stream_lines);
  rgi = new CGAL::Qt::RegularGridVectorFieldGraphicsItem<Regular_grid, K>(regular_grid);

  QObject::connect(this, SIGNAL(changed()),
		   sli, SLOT(modelChanged()));

  rgi->setVerticesPen(QPen(Qt::red, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  rgi->setEdgesPen(QPen(Qt::gray, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  sli->setEdgesPen(QPen(Qt::blue, 0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  scene.addItem(sli);
  scene.addItem(rgi);

  on_actionRecenter_triggered();
  Q_EMIT( changed());
}

void
MainWindow::on_actionLoadPoints_triggered()
{
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#endif
  QString fileName = QFileDialog::getOpenFileName(this,
						  tr("Open grid file"),
						  "."
						#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
						,tr("WKT files (*.wkt *.WKT)")
						#endif
                                                  );
  if(! fileName.isEmpty()){
    open(fileName);
  }
}

void
MainWindow::open(QString fileName)
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);
  std::ifstream ifs(qPrintable(fileName));

  runge_kutta_integrator = new Runge_kutta_integrator(integrating);
  double iXSize, iYSize;
  iXSize = iYSize = 512;
  if(fileName.endsWith(".wkt", Qt::CaseInsensitive))
  {
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
    std::vector<std::vector<Point_2> > mp;
    int size= -1;
    do
    {
      std::vector<Point_2> ps;
      CGAL::read_multi_point_WKT(ifs, ps);
      if(size == -1)
        size = static_cast<int>(ps.size());
      else if(ps.size() > 0 && size != static_cast<int>(ps.size()))
        ps.resize(size);
      else if(ps.size() == 0)
        continue;
      mp.push_back(ps);
    }while(ifs.good() && !ifs.eof());
    regular_grid = new Regular_grid(size, static_cast<int>(mp.size()), iXSize, iYSize);
    /*fill the grid with the appropriate values*/
    for (unsigned int i=0;i<static_cast<unsigned int>(size);++i)
      for (unsigned int j=0;j<mp.size();++j)
      {
        regular_grid->set_field(i, j, Vector(mp[j][i].x(), mp[j][i].y()));
      }
#else
    QApplication::restoreOverrideCursor();
    return;
#endif
  }
  else{
    unsigned int x_samples, y_samples;
    ifs >> x_samples;
    ifs >> y_samples;
    regular_grid = new Regular_grid(x_samples, y_samples, iXSize, iYSize);
    /*fill the grid with the appropriate values*/
    for (unsigned int i=0;i<x_samples;i++)
      for (unsigned int j=0;j<y_samples;j++)
      {
        double xval, yval;
        ifs >> xval;
        ifs >> yval;
        regular_grid->set_field(i, j, Vector(xval, yval));
      }
  }
  ifs.close();
  // default cursor
  QApplication::restoreOverrideCursor();
  this->addToRecentFiles(fileName);
  generate();
  Q_EMIT( changed());

}

void
MainWindow::on_actionSavePoints_triggered()
{
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
  QString fileName = QFileDialog::getSaveFileName(this,
						  tr("Save points"),
						  ".",
                                                  tr("WKT files (*.wkt *.WKT)"));
  if(! fileName.isEmpty()){
    std::ofstream ofs(qPrintable(fileName));

    std::vector<std::vector<Point_2> >mp;
    mp.resize(regular_grid->get_dimension().second);
    for (int i=0;i<regular_grid->get_dimension().first;++i)
    {
      mp[i].reserve(regular_grid->get_dimension().second);
      for (int j=0;j<regular_grid->get_dimension().second;++j)
      {
        mp[i].push_back(Point_2(regular_grid->get_field(j,i).x(),
                                regular_grid->get_field(j,i).y()));
      }
      CGAL::write_multi_point_WKT(ofs, mp[i]);
    }
    ofs.close();
  }
#endif
}

void
MainWindow::on_actionRecenter_triggered()
{
  this->graphicsView->setSceneRect(rgi->boundingRect());
  this->graphicsView->fitInView(rgi->boundingRect(), Qt::KeepAspectRatio);
}

#include <CGAL/Qt/resources.h>

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Stream_lines_2 demo");

  // Import resources from libCGAL (Qt5).
  // See https://doc.qt.io/qt-5/qdir.html#Q_INIT_RESOURCE
  CGAL_QT_INIT_RESOURCES;
  Q_INIT_RESOURCE(Stream_lines_2);

  MainWindow mainWindow;
  mainWindow.show();
  return app.exec();
}

#include "Stream_lines_2.moc"


// Copyright (C) 2019 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

// This file was created by Steffen Urban (urbste@googlemail.com) or
// company address (steffen.urban@zeiss.com)
// January 2019

#include "theia/sfm/pose/six_point_radial_distortion_homography.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Jacobi>

namespace theia {

using Eigen::Matrix;
using Eigen::MatrixXd;
using Eigen::Vector3d;
using Eigen::Vector2d;
using Vector6d = Eigen::Matrix<double, 6, 1>;
using Array51d = Eigen::Array<double, 5, 1>;
using Matrix68d = Eigen::Matrix<double, 6, 8>;
using Matrix62d = Eigen::Matrix<double, 6, 2>;
using Matrix65d = Eigen::Matrix<double, 6, 5>;
using Eigen::Matrix3d;

bool IsNearZero(double val) {
  return (+val < (100.0 * std::numeric_limits<double>::epsilon())) &&
         (-val < (100.0 * std::numeric_limits<double>::epsilon()));
}

bool SixPointRadialDistortionHomography(
    const std::vector<Eigen::Vector2d>& normalized_feature_points_left,
    const std::vector<Eigen::Vector2d>& normalized_feature_points_right,
    std::vector<RadialHomographyResult>* results, const double lmin,
    const double lmax) {
  Matrix62d X;
  Matrix62d U;

  for (int i = 0; i < 6; ++i) {
    X.row(i) = normalized_feature_points_left[i];
    U.row(i) = normalized_feature_points_right[i];
  }

  Matrix68d M;
  Vector6d u2 = U.col(0).array().square() + U.col(1).array().square();

  M.col(0) = -X.col(1).array() * U.col(0).array();
  M.col(1) = -X.col(1).array() * U.col(1).array();
  M.col(2) = -X.col(1);
  M.col(3) = X.col(0).array() * U.col(0).array();
  M.col(4) = X.col(0).array() * U.col(1).array();
  M.col(5) = X.col(0);
  M.col(6) = -X.col(1).array() * u2.array();
  M.col(7) = X.col(0).array() * u2.array();

  Eigen::JacobiSVD<Matrix68d, Eigen::FullPivHouseholderQRPreconditioner> Svd1(
      M, Eigen::ComputeFullV);
  const Eigen::Matrix<double, 8, 8>& V1 = Svd1.matrixV();

  const double a = -V1(2, 6) * V1(7, 6) + V1(5, 6) * V1(6, 6);
  const double b = -V1(2, 6) * V1(7, 7) - V1(2, 7) * V1(7, 6) +
                   V1(5, 6) * V1(6, 7) + V1(5, 7) * V1(6, 6);
  const double c = -V1(2, 7) * V1(7, 7) + V1(5, 7) * V1(6, 7);
  const double d = b * b - 4.0 * a * c;

  int nsols = 0;
  Vector2d rs;

  if (IsNearZero(d)) {
    nsols = 1;
    rs(0) = (-b) / (2.0 * a);
  } else if (d > 0.0) {
    nsols = 2;
    double d2 = std::sqrt(d);
    rs(0) = (-b + d2) / (2.0 * a);
    rs(1) = (-b - d2) / (2.0 * a);
  } else {
    return false;
  }

  const Vector6d x2 = X.col(0).array().square() + X.col(1).array().square();
  Vector6d u3, r;
  Matrix<double, 8, 1> n;
  Matrix65d T;
  T.col(0) = -M.col(3);
  T.col(1) = -M.col(4);

  for (int i = 0; i < nsols; i++) {
    n = rs(i) * V1.col(6) + V1.col(7);
    const double l2 = n(6) / n(2);
    // skip this solution early if radial distortion is spurious
    if (l2 < lmin || l2 > lmax) {
      continue;
    }

    u3 = u3.Ones() + l2 * u2;
    r = n(0) * U.col(0) + n(1) * U.col(1) + n(2) * u3;

    T.col(2) = -X.col(0).array() * u3.array();
    T.col(3) = x2.array() * r.array();
    T.col(4) = r;

    Eigen::JacobiSVD<Matrix65d> Svd2(T, Eigen::ComputeFullV);
    Matrix<double, 5, 1> v2 = Svd2.matrixV().col(4);

    v2.head(4) /= v2(4);
    const double l1 = v2(3);
    // skip this solution early if radial distortion is spurious
    if (l1 < lmin || l1 > lmax) {
      continue;
    }

    RadialHomographyResult res;
    // fill homograhapy
    res.H << n(0), n(1), n(2), n(3), n(4), n(5), v2(0), v2(1), v2(2);
    // fill radial distortion values
    res.l1 = l1;
    res.l2 = l2;
    results->push_back(res);
  }

  return nsols > 0;
}
}


#include "discrete_exponential_map.h"

#include <set>
#include <queue>

#include <Eigen/Dense>

#include <igl/hessian_energy.h>
#include <igl/massmatrix.h>
#include <igl/cotmatrix.h>

#include <geometry/patch.h>
#include <shape_signatures/shape_signature.h>
#include <algorithms/shortest_path.h>
#include <matching/surface_stroke.h>

using namespace shortest_path;

DiscreteExponentialMap::DiscreteExponentialMap():
	_TBN(Eigen::Matrix3d::Identity()),
	_TBN_inv(Eigen::Matrix3d::Identity()),
	_geometry(nullptr) {

}

// Unintuitively, p_vid is actually the index of the vertex in relation to patch->origin_mesh()
DiscreteExponentialMap::DiscreteExponentialMap(std::shared_ptr<Patch> patch, Eigen::DenseIndex p_vid, const Eigen::MatrixXd* guide_points) {
	// Implementation of Discrete Exponential Map from,
	// "Part-Based Representation and Editing of 3D Surface Models", Ryan Schmidt, 2011

	auto mesh = patch->origin_mesh();

	const Eigen::MatrixXd& V = mesh->vertices();
	const Eigen::MatrixXd& N_orig = mesh->vertex_normals();
	Eigen::MatrixXd N(N_orig.rows(), N_orig.cols());

	if (V.size() <= 0 || N.size() <= 0) {
		return;
	}

	std::map<Eigen::DenseIndex, std::shared_ptr<DjikstraVertexNode>> nodes = djikstras_algorithm(patch, p_vid);

	if (nodes.size() == 0) {
		// djikstra's failed??
		return;
	}

	// Smooth the normals a bit before generating the map
	Eigen::SparseMatrix<double> M2;
	Eigen::SparseMatrix<double> QH;

	Eigen::MatrixXd V3 = V.leftCols<3>();
	Eigen::MatrixXi F3 = mesh->faces().leftCols<3>();

	igl::massmatrix(V3, F3, igl::MASSMATRIX_TYPE_BARYCENTRIC, M2);
	igl::hessian_energy(V3, F3, QH);

    // Smoothing -- 0.0 is no smoothing, 1.0 is full
	const double alpha = 0.25;

	Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> hessSolver(alpha * QH + (1.0 - alpha) * M2);
	N << hessSolver.solve((1.0 - alpha) * M2 * N_orig);

	Eigen::Vector3d p = V.row(p_vid).block<1, 3>(0, 0).transpose();
	Eigen::Vector3d Np = N.row(p_vid).block<1, 3>(0, 0).transpose().normalized();

	Eigen::Matrix3d Tp_TBN = basis_from_plane_normal(Np);

	// For each point in patch, create a chain of points also within the patch leading back to the center by the shortest path
	std::priority_queue<std::shared_ptr<DjikstraVertexNode>, std::vector<std::shared_ptr<DjikstraVertexNode>>, DjikstraDist> Q;

	for (auto it = nodes.begin(); it != nodes.end(); ++it) {
		Q.push(it->second);
	}

	// Create a set of planar undirected edges representing the discrete exponential map
	std::map<Eigen::DenseIndex, Eigen::Vector2d> DEM_points;
	DEM_points.insert(std::make_pair(Q.top()->_vid, Eigen::Vector2d(0.0, 0.0)));
	Q.pop();

	while (!Q.empty()) {
		auto node = Q.top();
		Q.pop();

		assert(node->_prev != nullptr);
		assert(node->_dist > 0.0);
		assert(node->_vid >= 0);

		// Run from each neighbor already present in DEM_points, as an "Upwind Average"
		// Find all of q's neighbors already in the DEM that share locality with node->_prev
		std::vector<Eigen::DenseIndex> neighbors = patch->origin_mesh()->one_ring(node->_vid);
		std::vector<std::pair<Eigen::Vector2d, double>> upwind;
		Eigen::Vector2d from_parent;
		double parent_dist = 0.0;

		for (Eigen::DenseIndex prev_vid : neighbors) {
			auto prev = DEM_points.find(prev_vid);

			if (prev == DEM_points.end()) {
				continue;
			}

			Eigen::Vector3d q = V.row(node->_vid).block<1, 3>(0, 0).transpose();

			if (prev_vid == p_vid) {
				Eigen::Vector2d Tpq = local_log_map(p, Tp_TBN, q).topRows<2>();

				double weight = (q - p).squaredNorm() + 1e-7;

				upwind.push_back(std::make_pair(Tpq, weight));

				if (prev_vid == node->_prev->_vid) {
					// Store away for special locality test when averaging upwind points
					parent_dist = Tpq.norm();
					from_parent = Tpq;
				}

				continue;
			}

			Eigen::Vector3d r = V.row(prev_vid).block<1, 3>(0, 0).transpose();
			Eigen::Vector2d Tpr = prev->second;

			// A vector parallel to two planes is the cross product of the two normals.
			Eigen::Vector3d Nr = N.row(prev_vid).block<1, 3>(0, 0).transpose().normalized();
			Eigen::Matrix3d Tr_TBN = basis_from_plane_normal(Nr);

			// Do not pass through the intermediate planes -- instead, just transform directly into Tp after log_r(q)
			Eigen::Vector2d Trq = local_log_map(r, Tr_TBN, q).topRows<2>();

			// 3D rotation Mn
			Eigen::Vector3d rot_axis = Nr.cross(Np);

			if (rot_axis.isZero(1e-7)) {
				rot_axis = Tp_TBN.col(0);
			}

			rot_axis.normalize();

			double rot_angle_3d = std::acos(std::min(1.0, std::max(-1.0, Nr.dot(Np))));
			Eigen::AngleAxis<double> R(rot_angle_3d, rot_axis);

			// 2D rotation for planar basis alignment
			Eigen::Vector3d er = (R * Tr_TBN.col(0)).normalized();
			Eigen::Vector3d ep = Tp_TBN.col(0);

			assert(er.dot(Np) < 1e-7);

			double rot_angle_2d = std::acos(std::min(1.0, std::max(-1.0, er.dot(ep))));

			if (!er.cross(ep).isZero(1e-7) && er.cross(ep).normalized().dot(Np) > 0.0) {
				rot_angle_2d *= -1.0;
			}

			Eigen::Rotation2D<double> E(rot_angle_2d);

			Eigen::Vector2d Tpq = Tpr + E * Trq;

			double weight = 1.0 / ((q - r).squaredNorm() + 1e-7);

			if (prev_vid == node->_prev->_vid) {
				// Store away for special locality test when averaging upwind points
				parent_dist = (Tpr - Tpq).norm();
				from_parent = Tpq;
			}

			upwind.push_back(std::make_pair(Tpq, weight));
		}

		assert(upwind.size() > 0);

		Eigen::Vector2d Tpq_avg(0.0, 0.0);

		double total_weight = 0.0;
		for (auto pt : upwind) {
			if ((pt.first - from_parent).norm() > parent_dist / 2.0) {
				continue;
			}

			total_weight += pt.second;
		}

		for (auto pt : upwind) {
			if ((pt.first - from_parent).norm() > parent_dist / 2.0) {
				continue;
			}

			Tpq_avg += pt.second * pt.first / total_weight;
		}

		DEM_points.insert(std::pair<Eigen::DenseIndex, Eigen::Vector2d>(node->_vid, Tpq_avg));
	}

	if (!init(p_vid, Tp_TBN, DEM_points, patch)) {
		throw std::invalid_argument("DEM arguments invalid!");
	}
}

DiscreteExponentialMap::DiscreteExponentialMap(const Eigen::DenseIndex center_vid, Eigen::Matrix3d& TBN, const std::map<Eigen::DenseIndex, Eigen::Vector2d>& vertices, std::shared_ptr<Patch> geometry) {
	if (!init(center_vid, TBN, vertices, geometry)) {
		throw std::invalid_argument("DEM arguments invalid!");
	}
}

DiscreteExponentialMap::~DiscreteExponentialMap() {

}

Eigen::MatrixXi DiscreteExponentialMap::get_reindexed_faces() const {
	Eigen::MatrixXi F = _faces;

	std::map<Eigen::DenseIndex, Eigen::DenseIndex> vid_remap;
	Eigen::DenseIndex i = 0;
	for (auto it = _vertices.cbegin(); it != _vertices.cend(); ++it, ++i) {
		vid_remap.insert(std::make_pair(it->first, i));
	}

	for (i = 0; i < F.size(); ++i) {
		F(i) = vid_remap.at(F(i));
	}

	return F;
}

bool DiscreteExponentialMap::init(const Eigen::DenseIndex center_vid, Eigen::Matrix3d& TBN, const std::map<Eigen::DenseIndex, Eigen::Vector2d>& vertices, std::shared_ptr<Patch> geometry) {
	_geometry = geometry;
	_TBN = TBN;
	_TBN_inv = TBN.inverse();
	_vertices = vertices;

	_center_vid = center_vid;

	// Only include faces made up of vertices in the map
	std::set<Eigen::DenseIndex> fids;
	std::shared_ptr<Mesh> mesh = _geometry->origin_mesh();
	const Eigen::MatrixXi& F = mesh->faces();
	const Eigen::MatrixXd& V = mesh->vertices();

	for (Eigen::DenseIndex i = 0; i < F.rows(); ++i) {
		bool included = true;

		for (Eigen::DenseIndex j = 0; j < F.cols(); ++j) {
			if (vertices.find(F(i, j)) == vertices.end()) {
				included = false;
				break;
			}
		}

		if (!included) {
			continue;
		}

		for (Eigen::DenseIndex j = 0; j < F.cols(); ++j) {
			// Check edge length (it should not be severely distorted
			Eigen::DenseIndex next = (j + 1) % F.cols();

			double dist = (V.row(F(i, j)).leftCols<3>() - V.row(F(i, next)).leftCols<3>()).norm();
			double dem_dist = (vertices.at(F(i, j)) - vertices.at(F(i, next))).norm();

			if (dem_dist > 2.0 * dist) {
				included = false;
				break;
			}
		}

		if (included) {
			fids.insert(i);
		}
	}

	_faces = Eigen::MatrixXi(fids.size(), F.cols());

	int fIndex = 0;
	for (auto fid : fids) {
		_fid_remap.insert(std::make_pair(fIndex, fid));
		_faces.row(fIndex++) << F.row(fid);
	}

	std::stringstream ss; ss << geometry->origin_mesh()->resource_dir() << "//matlab//dem_debug.m";
	to_matlab(ss.str());

	std::vector<std::pair<Eigen::DenseIndex, Eigen::Vector2d>> face_centers;

	_center_fid = -1;

	std::vector<Eigen::DenseIndex> center_fids;
	for (Eigen::DenseIndex i = 0; i < _faces.rows(); ++i) {
		for (Eigen::DenseIndex j = 0; j < _faces.cols(); ++j) {
			if (_faces(i, j) == _center_vid) {
				center_fids.push_back(i);
				break;
			}
		}
	}

	for (auto fid : center_fids) {
		Eigen::Vector2d fc = Eigen::Vector2d::Zero();

		for (Eigen::DenseIndex j = 0; j < _faces.cols(); ++j) {
			fc += vertices.at(_faces(fid, j)) / static_cast<double>(_faces.cols());
		}

		face_centers.push_back(std::pair<Eigen::DenseIndex, Eigen::Vector2d>(fid, fc));
	}

	Eigen::DenseIndex centroid_fid = -1;
	double dist = std::numeric_limits<double>::max();

	for (Eigen::DenseIndex i = 0; i < face_centers.size(); ++i) {
		double c_dist = (face_centers[i].second - vertices.at(_center_vid)).norm();

		if (c_dist < dist) {
			dist = c_dist;
			_center_fid = face_centers[i].first;
		}
	}

	if (_center_fid < 0 && _vertices.size() > 2) {
		throw std::domain_error("Invalid _center_fid!");
	}

	return true;
}

Eigen::DenseIndex DiscreteExponentialMap::get_center_vid() const {
	return _center_vid;
}

Eigen::DenseIndex DiscreteExponentialMap::get_center_fid() const {
	return _center_fid;
}

Eigen::MatrixXd DiscreteExponentialMap::get_3d_vertices() const {
	// get_reindexed_faces describes the faces for this vertex ordering
	Eigen::MatrixXd V_3d(_vertices.size(), 3);

	Eigen::DenseIndex index = 0;
	for (auto it = _vertices.cbegin(); it != _vertices.cend(); ++it) {
		Eigen::Vector3d tbn_point = (Eigen::Vector3d() << it->second, 0.0).finished();

		V_3d.row(index++) = _TBN * tbn_point;
	}

	return V_3d;
}

double DiscreteExponentialMap::get_radius() const {
	Eigen::MatrixXd V = get_3d_vertices();

	// the DEM is relative to the frame origin, so the radius is just the greatest magnitude norm
	double radius = 0.0;
	for (Eigen::DenseIndex i = 0; i < V.rows(); ++i) {
		radius = std::max(V.row(i).norm(), radius);
	}

	return radius;
}

Eigen::Vector3d DiscreteExponentialMap::get_normal() const {
	return _TBN.col(2);
}

Eigen::Vector3d DiscreteExponentialMap::get_tangent() const {
	return _TBN.col(0);
}

Eigen::Vector3d DiscreteExponentialMap::get_bitangent() const {
	return _TBN.col(1);
}

Eigen::Vector2d DiscreteExponentialMap::interpolated_polar(Eigen::Vector3d barycentric_coords, const std::vector<Eigen::DenseIndex>& vids) {
	Eigen::Vector2d polar;
	Eigen::MatrixXd points(2,3);

	for (Eigen::DenseIndex i = 0; i < 3; i++) {
		// Just gonna let it throw an exception if the vids are no in the map -- shame on the user!
		points.col(i) = _vertices[vids[i]];
	}

	Eigen::Vector2d xy = points * barycentric_coords;

	polar << std::sqrt(std::pow(xy(0), 2) + std::pow(xy(1), 2)), std::atan2(xy(1), xy(0));

	return polar;
}

Eigen::DenseIndex DiscreteExponentialMap::nearest_vertex_by_polar(const Eigen::Vector2d& polar_point) {
	Eigen::Vector2d xy_point;
	xy_point << polar_point(0) * std::cos(polar_point(1)),
				polar_point(0) * std::sin(polar_point(1));

	Eigen::DenseIndex vid = -1;
	double dist = std::numeric_limits<double>::max();
	for (auto v : _vertices) {
		double t_dist = (v.second - xy_point).norm();

		if (t_dist < dist) {
			vid = v.first;
			dist = t_dist;
		}
	}

	return vid;
}

Eigen::VectorXd DiscreteExponentialMap::query_map_value(const Eigen::Vector2d& xy_point, std::shared_ptr<ShapeSignature> sig) const {
	// Find triangle which contains (x, y)
	unsigned int i = 0;
	for (i = 0; i < _faces.rows(); ++i) {
		Eigen::DenseIndex r = _faces(i, 0);
		Eigen::DenseIndex s = _faces(i, 1);
		Eigen::DenseIndex t = _faces(i, 2);

		Eigen::Vector2d a = _vertices.at(_faces(i, 0));
		Eigen::Vector2d b = _vertices.at(_faces(i, 1));
		Eigen::Vector2d c = _vertices.at(_faces(i, 2));

		if (point_in_triangle(xy_point, a, b, c)) {
			break;
		}
	}

	// In case point isn't in any triangle, return some invalid result (-1.0?)
	if (i >= _faces.rows()) {
		// point is not within any triangle of the map, so return a vector packed with -1.0s
		return Eigen::VectorXd::Constant(sig->feature_dimension(), -1.0);
	}

	// Find barycentric coordinates of the point within the triangle
	// https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
	Eigen::Vector2d v0 = _vertices.at(_faces(i, 1)) - _vertices.at(_faces(i, 0));
	Eigen::Vector2d v1 = _vertices.at(_faces(i, 2)) - _vertices.at(_faces(i, 0));
	Eigen::Vector2d v2 = xy_point - _vertices.at(_faces(i, 0));
	double d00 = v0.dot(v0);
	double d01 = v0.dot(v1);
	double d11 = v1.dot(v1);
	double d20 = v2.dot(v0);
	double d21 = v2.dot(v1);
	double denom = d00 * d11 - d01 * d01;
	double v = (d11 * d20 - d01 * d21) / denom;
	double w = (d00 * d21 - d01 * d20) / denom;
	double u = 1.0f - v - w;

	if (u + v + w - 1.0 > std::numeric_limits<double>::epsilon()) {
		throw std::domain_error("query_map_value(): Invalid barycentric coordinates!");
	}

	// return linearly interpolated feature values from triangle vertices with respect to (x,y)
	//Eigen::VectorXd value = u * features.row(_faces(i, 0)) + v * features.row(_faces(i, 1)) + w * features.row(_faces(i, 2));
	Eigen::VectorXd u_coord = sig->lerpable_coord(_fid_remap.at(i), _faces(i, 0));
	Eigen::VectorXd v_coord = sig->lerpable_coord(_fid_remap.at(i), _faces(i, 1));
	Eigen::VectorXd w_coord = sig->lerpable_coord(_fid_remap.at(i), _faces(i, 2));

	Eigen::VectorXd value = u * u_coord
						  + v * v_coord
						  + w * w_coord;

	value = sig->lerpable_to_signature_value(value);

	return value;
}

Eigen::VectorXd DiscreteExponentialMap::query_map_value_polar(const Eigen::Vector2d& polar_point, std::shared_ptr<ShapeSignature> sig) const {
	Eigen::Vector2d xy_point;
	xy_point << polar_point(0) * std::cos(polar_point(1)),
				polar_point(0) * std::sin(polar_point(1));

	return query_map_value(xy_point, sig);
}

bool DiscreteExponentialMap::to_matlab(std::string script_out_path) {
	std::ofstream m(script_out_path, std::ofstream::out);

	if (m.is_open()) {
		m << "figure;" << std::endl;
		m << "hold on;" << std::endl;
		m << "axis equal;" << std::endl;
		m << "grid on;" << std::endl;

		auto vertices = get_raw_vertices();
		m << "v = [ ..." << std::endl;
		for (auto vert : vertices) {
			m << vert.second.transpose() << "; ..." << std::endl;
		}
		m << "];" << std::endl;

		const Eigen::MatrixXi& faces = get_reindexed_faces();
		m << "f = [ ..." << std::endl;
		for (Eigen::DenseIndex i = 0; i < faces.rows(); ++i) {
			m << faces.row(i) << "; ..." << std::endl;
		}
		m << "];" << std::endl;

		m << "for i=1:size(f,1)" << std::endl;
		m << "a = [v(f(i, 1) + 1, :), 0.0];" << std::endl;
		m << "b = [v(f(i, 2) + 1, :), 0.0];" << std::endl;
		m << "c = [v(f(i, 3) + 1, :), 0.0];" << std::endl;
		m << "cb = (c - b) / norm(c - b);" << std::endl;
		m << "ab = (a - b) / norm(a - b);" << std::endl;
		m << "n = cross(cb, ab);" << std::endl;
		m << "n = n / norm(n);" << std::endl;
		m << "C = 'g';" << std::endl;
		m << "if dot(n, [0, 0, 1]) < 1 - 1e-7" << std::endl;
		m << "	C = 'r';" << std::endl;
		m << "end" << std::endl;
		m << "h = fill([a(1), b(1), c(1)], [a(2), b(2), c(2)], C);" << std::endl;
		m << "set(h, 'facealpha', .5);" << std::endl;
		m << "end" << std::endl;

		m << "scatter(v(:,1), v(:,2), 'mo');" << std::endl;

		m.close();
	}
	else {
		return false;
	}

	return true;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*

 Copyright (C) 2016 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file stochasticcollocationinvcdf.hpp
    Stochastic collocation inverse cumulative distribution function
*/

#ifndef quantlib_stochastic_collation_inv_cdf_hpp
#define quantlib_stochastic_collation_inv_cdf_hpp

#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/interpolations/lagrangeinterpolation.hpp>

#include <boost/function.hpp>
#include <functional>

namespace QuantLib {
    //! Stochastic collocation inverse cumulative distribution function

    /*! References:
        L.A. Grzelak, J.A.S. Witteveen, M.Suárez-Taboada, C.W. Oosterlee,
        The Stochastic Collocation Monte Carlo Sampler: Highly efficient
        sampling from “expensive” distributions
        http://papers.ssrn.com/sol3/papers.cfm?abstract_id=2529691
     */

    class StochasticCollocationInvCDF {
      public:
        typedef Real argument_type;
        typedef Real result_type;

        StochasticCollocationInvCDF(
            const boost::function<Real(Real)>& invCDF,
            Size lagrangeOrder,
            Real pMax = Null<Real>(),
            Real pMin = Null<Real>());

        Real value(Real x) const;
        Real operator()(Real u) const;

      private:
        const Array x_;
        const Volatility sigma_;
        const Array y_;
        const LagrangeInterpolation interpl_;
    };
}

#endif