/*
 * Copyright (c) 2013-2014 Masahide Kashiwagi (kashi@waseda.jp)
 */

/*
 * test program for rounded math emulation
 *  compare add, sub, mul, div and sqrt under
 *  - changing rounding mode
 *  - emulating rounded math using twosum and twoproduct
 */

#include <fenv.h>
#include <boost/random.hpp>
#include <ctime>

#ifndef NT
#define NT 10000000
#endif

struct hwround {
	public:

	static void roundnear() {
		fesetround(FE_TONEAREST);
	}

	static void rounddown() {
		fesetround(FE_DOWNWARD);
	}

	static void roundup() {
		fesetround(FE_UPWARD);
	}

	static void roundchop() {
		fesetround(FE_TOWARDZERO);
	}

	static double add_up(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		roundup();
		r = x1 + y1;
		roundnear();
		return r;
	}

	static double add_down(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		rounddown();
		r = x1 + y1;
		roundnear();
		return r;
	}

	static double sub_up(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		roundup();
		r = x1 - y1;
		roundnear();
		return r;
	}

	static double sub_down(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		rounddown();
		r = x1 - y1;
		roundnear();
		return r;
	}

	static double mul_up(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		roundup();
		r = x1 * y1;
		roundnear();
		return r;
	}

	static double mul_down(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		rounddown();
		r = x1 * y1;
		roundnear();
		return r;
	}

	static double div_up(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		roundup();
		r = x1 / y1;
		roundnear();
		return r;
	}

	static double div_down(const double& x, const double& y) {
		volatile double r, x1 = x, y1 = y;
		rounddown();
		r = x1 / y1;
		roundnear();
		return r;
	}

	static double sqrt_up(const double& x) {
		volatile double r, x1 = x;
		roundup();
		r = sqrt(x1);
		roundnear();
		return r;
	}

	static double sqrt_down(const double& x) {
		volatile double r, x1 = x;
		rounddown();
		r = sqrt(x1);
		roundnear();
		return r;
	}
};

struct nohwround {

	static void fasttwosum(const double& a, const double& b, double& x, double& y) {
		double tmp;
		x = a + b;
		tmp = x - a;
		y = b - tmp;
	}

	#if 0
	static void twosum(const double& a, const double& b, double& x, double& y) {
		double tmp;
		x = a + b;
		tmp = x - a;
		if (std::fabs(tmp) == std::numeric_limits<double>::infinity()) {
			tmp = x * 0.5 - a * 0.5;
			y = ((a * 0.5 - (x * 0.5 - tmp)) + (b * 0.5 - tmp)) * 2.;
		} else {
			y = (a - (x - tmp)) + (b - tmp);
		}
	}
	#endif

	static void twosum(const double& a, const double& b, double& x, double& y) {
		double tmp;

		x = a + b;
		if (std::fabs(a) > std::fabs(b)) {
			tmp = x - a;
			y = b - tmp;
		} else {
			tmp = x - b;
			y = a - tmp;
		}
	}

	static void split(const double& a, double& x, double& y) {
		static const double sigma = ldexp(1., 27) + 1.;
		double tmp;

		tmp = a * sigma;
		x = tmp - (tmp - a);
		y = a - x;
	}

	static void twoproduct(const double& a, const double& b, double& x, double& y) {
		static const double th = ldexp(1., 996);
		static const double c1 = ldexp(1., -28);
		static const double c2 = ldexp(1., 28);
		static const double th2 = ldexp(1., 1023);

		double na, nb, a1, a2, b1, b2;

		x = a * b;
		#if 0
		if (std::fabs(x) == std::numeric_limits<double>::infinity()) {
			y = 0.;
			return;
		}
		#endif
		if (std::fabs(a) > th) {
			na = a * c1;
			nb = b * c2;
		} else if (std::fabs(b) > th) {
			na = a * c2;
			nb = b * c1;
		} else {
			na = a;
			nb = b;
		}
		split(na, a1, a2);
		split(nb, b1, b2);
		if (std::fabs(x) > th2) {
			y = a2 * b2 - ((((x * 0.5) - (a1 * 0.5)  * b1) * 2. - a2 * b1) - a1 * b2);
		} else {
			y = a2 * b2 - (((x - a1 * b1) - a2 * b1) - a1 * b2);
		}
	}

	// succ and pred by Rump

	static double succ(const double& x) {
		static const double th1 = ldexp(1., -969);
		static const double th2 = ldexp(1., -1021);
		static const double c1 = ldexp(1., -53) + ldexp(1., -105);
		static const double c2 = ldexp(1., -1074);
		static const double c3 = ldexp(1., 53);
		static const double c4 = ldexp(1., -53);

		double a, c, e;

		a = std::fabs(x);
		if (a >= th1) return x + a * c1;
		if (a < th2) return x + c2;
		c = c3 * x;
		e = c1 * std::fabs(c);
		return (c + e) * c4;
	}

	static double pred(const double& x) {
		static const double th1 = ldexp(1., -969);
		static const double th2 = ldexp(1., -1021);
		static const double c1 = ldexp(1., -53) + ldexp(1., -105);
		static const double c2 = ldexp(1., -1074);
		static const double c3 = ldexp(1., 53);
		static const double c4 = ldexp(1., -53);

		double a, c, e;

		a = std::fabs(x);
		if (a >= th1) return x - a * c1;
		if (a < th2) return x - c2;
		c = c3 * x;
		e = c1 * std::fabs(c);
		return (c - e) * c4;
	}

	static double add_up(const double& x, const double& y) {
		double r, r2;

		twosum(x, y, r, r2);
		if (r == std::numeric_limits<double>::infinity()) {
			return r;
		} else if (r == -std::numeric_limits<double>::infinity()) {
			if (x == -std::numeric_limits<double>::infinity() || y == -std::numeric_limits<double>::infinity()) {
				return r;
			} else {
				return -(std::numeric_limits<double>::max)();
			}
		}

		if (r2 > 0.) {
			return succ(r);
		}

		return r;
	}

	static double add_down(const double& x, const double& y) {
		double r, r2;

		twosum(x, y, r, r2);
		if (r == std::numeric_limits<double>::infinity()) {
			if (x == std::numeric_limits<double>::infinity() || y == std::numeric_limits<double>::infinity()) {
				return r;
			} else {
				return (std::numeric_limits<double>::max)();
			}
		} else if (r == -std::numeric_limits<double>::infinity()) {
			return r;
		}

		if (r2 < 0.) {
			return pred(r);
		}

		return r;
	}

	static double sub_up(const double& x, const double& y) {
		double r, r2;

		twosum(x, -y, r, r2);
		if (r == std::numeric_limits<double>::infinity()) {
			return r;
		} else if (r == -std::numeric_limits<double>::infinity()) {
			if (x == -std::numeric_limits<double>::infinity() || y == std::numeric_limits<double>::infinity()) {
				return r;
			} else {
				return -(std::numeric_limits<double>::max)();
			}
		}

		if (r2 > 0.) {
			return succ(r);
		}

		return r;
	}

	static double sub_down(const double& x, const double& y) {
		double r, r2;

		twosum(x, -y, r, r2);
		if (r == std::numeric_limits<double>::infinity()) {
			if (x == std::numeric_limits<double>::infinity() || y == -std::numeric_limits<double>::infinity()) {
				return r;
			} else {
				return (std::numeric_limits<double>::max)();
			}
		} else if (r == -std::numeric_limits<double>::infinity()) {
			return r;
		}

		if (r2 < 0.) {
			return pred(r);
		}

		return r;
	}

	static double mul_up(const double& x, const double& y) {
		double r, r2;
		double x1, y1;
		double s, s2, t;
		static const double th = ldexp(1., -969); // -1074 + 106 - 1
		static const double c = ldexp(1., 537); // 1074 / 2

		// if (x == 0. || y == 0.) return x * y;

		twoproduct(x, y, r, r2);
		if (r == std::numeric_limits<double>::infinity()) {
			return r;
		} else if (r == -std::numeric_limits<double>::infinity()) {
			if (std::fabs(x) == std::numeric_limits<double>::infinity() || std::fabs(y) == std::numeric_limits<double>::infinity()) {
				return r;
			} else {
				return -(std::numeric_limits<double>::max)();
			}
		}

		if (fabs(r) >= th) {
			if (r2 > 0.) return succ(r);
			return r;
		} else {
			twoproduct(x * c, y * c, s, s2);
			t = (r * c) * c;
			if ( t < s || (t == s && s2 > 0.)) {
				return succ(r);
			}
			return r;
		}
	}

	static double mul_down(const double& x, const double& y) {
		double r, r2;
		double x1, y1;
		double s, s2, t;
		static const double th = ldexp(1., -969); // -1074 + 106 - 1
		static const double c = ldexp(1., 537); // 1074 / 2

		// if (x == 0. || y == 0.) return x * y;

		twoproduct(x, y, r, r2);
		if (r == std::numeric_limits<double>::infinity()) {
			if (std::fabs(x) == std::numeric_limits<double>::infinity() || std::fabs(y) == std::numeric_limits<double>::infinity()) {
				return r;
			} else {
				return (std::numeric_limits<double>::max)();
			}
		} else if (r == -std::numeric_limits<double>::infinity()) {
			return r;
		}

		if (fabs(r) >= th) {
			if (r2 < 0.) return pred(r);
			return r;
		} else {
			twoproduct(x * c, y * c, s, s2);
			t = (r * c) * c;
			if ( t > s || (t == s && s2 < 0.)) {
				return pred(r);
			}
			return r;
		}
	}

	static double div_up(const double& x, const double& y) {
		double r, r2;
		double xn, yn, d;
		static const double th1 = ldexp(1., -969); // -1074 + 106 - 1
		static const double th2 = ldexp(1., 918); // 1023 - 105
		static const double c1 = ldexp(1., 105); // -969 - (-1074)
		static const double c2 = ldexp(1., -1074);

		if (x == 0. || y == 0. || std::fabs(x) == std::numeric_limits<double>::infinity() || std::fabs(y) == std::numeric_limits<double>::infinity() || x != x  || y != y) {
			return x / y;
		}

		if (y < 0.) {
			xn = -x;
			yn = -y;
		} else {
			xn = x;
			yn = y;
		}

		if (fabs(xn) < th1) {
			if (fabs(yn) < th2) {
				xn *= c1;
				yn *= c1;
			} else {
				if (xn < 0.) return 0.;
				else return c2;
			}
		}

		d = xn / yn;

		if (d == std::numeric_limits<double>::infinity()) {
			return d;
		} else if (d == -std::numeric_limits<double>::infinity()) {
			return -(std::numeric_limits<double>::max)();
		}

		twoproduct(d, yn, r, r2);
		if ( r < xn || ((r == xn) && r2 < 0.)) {
			return succ(d);
		}
		return d;
	}

	static double div_down(const double& x, const double& y) {
		double r, r2;
		double xn, yn, d;
		static const double th1 = ldexp(1., -969); // -1074 + 106 - 1
		static const double th2 = ldexp(1., 918); // 1023 - 105
		static const double c1 = ldexp(1., 105); // -969 - (-1074)
		static const double c2 = ldexp(1., -1074);

		if (x == 0. || y == 0. || std::fabs(x) == std::numeric_limits<double>::infinity() || std::fabs(y) == std::numeric_limits<double>::infinity() || x != x  || y != y) {
			return x / y;
		}

		if (y < 0.) {
			xn = -x;
			yn = -y;
		} else {
			xn = x;
			yn = y;
		}

		if (fabs(xn) < th1) {
			if (fabs(yn) < th2) {
				xn *= c1;
				yn *= c1;
			} else {
				if (xn < 0.) return -c2;
				else return 0.;
			}
		}

		d = xn / yn;

		if (d == std::numeric_limits<double>::infinity()) {
			return (std::numeric_limits<double>::max)();
		} else if (d == -std::numeric_limits<double>::infinity()) {
			return d;
		}

		twoproduct(d, yn, r, r2);
		if ( r > xn || ((r == xn) && r2 > 0.)) {
			return pred(d);
		}
		return d;
	}

	static double sqrt_up(const double& x) {
		double r, r2, d;
		static const double th1 = ldexp(1., -969); // -1074 + 106 - 1
		static const double c1 = ldexp(1., 106); // -969 - (-1074) + 1
		static const double c2 = ldexp(1., 53); // sqrt(c1)

		d = sqrt(x);

		if (x < th1) {
			double d2, x2;
			x2 = x * c1;
			d2 = d * c2;
			twoproduct(d2, d2, r, r2);
			if ( r < x2 || (r == x2 && r2 < 0.)) {
				return succ(d);
			}
			return d;
		}

		twoproduct(d, d, r, r2);
		if ( r < x || (r == x && r2 < 0.)) {
			return succ(d);
		}
		return d;
	}

	static double sqrt_down(const double& x) {
		double r, r2, d;
		static const double th1 = ldexp(1., -969); // -1074 + 106 - 1
		static const double c1 = ldexp(1., 106); // -969 - (-1074) + 1
		static const double c2 = ldexp(1., 53); // sqrt(c1)

		d = sqrt(x);

		if (x < th1) {
			double d2, x2;
			x2 = x * c1;
			d2 = d * c2;
			twoproduct(d2, d2, r, r2);
			if ( r > x2 || (r == x2 && r2 > 0.)) {
				return pred(d);
			}
			return d;
		}

		twoproduct(d, d, r, r2);
		if ( r > x || (r == x && r2 > 0.)) {
			return pred(d);
		}
		return d;
	}

};

bool samedouble(double x, double y)
{
	// return *((unsigned long long *)(&x)) == *((unsigned long long *)(&y));
	if (x != x && y != y) return true;
	return x == y;
}

void check(double x, double y)
{
	volatile double r1, r2;

	r1 = hwround::add_up(x, y);
	r2 = nohwround::add_up(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "add_up error\n"; std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::add_down(x, y);
	r2 = nohwround::add_down(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "add_down error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::sub_up(x, y);
	r2 = nohwround::sub_up(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "sub_up error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::sub_down(x, y);
	r2 = nohwround::sub_down(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "sub_down error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::mul_up(x, y);
	r2 = nohwround::mul_up(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "mul_up error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::mul_down(x, y);
	r2 = nohwround::mul_down(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "mul_down error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::div_up(x, y);
	r2 = nohwround::div_up(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "div_up error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::div_down(x, y);
	r2 = nohwround::div_down(x, y);
	if (!samedouble(r1, r2)) {
		std::cout << "div_down error\n";
		std::cout << x << "\n";
		std::cout << y << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::sqrt_up(x);
	r2 = nohwround::sqrt_up(x);
	if (!samedouble(r1, r2)) {
		std::cout << "sqrt_up error\n";
		std::cout << x << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}

	r1 = hwround::sqrt_down(x);
	r2 = nohwround::sqrt_down(x);
	if (!samedouble(r1, r2)) {
		std::cout << "sqrt_down error\n";
		std::cout << x << "\n";
		std::cout << r1 << "\n";
		std::cout << r2 << "\n";
	}
}

int main() {
	double x, y;
	int i, j;
	unsigned long long t;

	double specials[11] = {
		0.,
		-0.,
		std::numeric_limits<double>::infinity(),
		-std::numeric_limits<double>::infinity(),
		(std::numeric_limits<double>::max)(),
		-(std::numeric_limits<double>::max)(),
		(std::numeric_limits<double>::min)(),
		-(std::numeric_limits<double>::min)(),
		std::numeric_limits<double>::denorm_min(),
		-std::numeric_limits<double>::denorm_min()
	};
	specials[10] = specials[2] + specials[3]; // making NaN

	boost::variate_generator<boost::mt19937, boost::uniform_int<unsigned long long> > rand(boost::mt19937(time(0)), boost::uniform_int<unsigned long long>(0, -1));

	std::cout.precision(17);

	// cause overflow of intermediate variable in twoproduct
	// x = 6.929001713869936e+236;
	// y = 2.5944475251952003e+71;
	// check(x, y);

	// cause overflow of intermediate variable in twosum
	// x = 3.5630624444874539e+307;
	// y = -1.7976931348623157e+308;
	// check(x, y);

	// check general-general case

	for (i=0; i<NT; i++) {
		t = rand();
		x = *((double*)(&t));
		t = rand();
		y = *((double*)(&t));
		check(x, y);
	}

	// check general-special case

	for (i=0; i<NT; i++) {
		t = rand();
		x = *((double*)(&t));
		for (j=0; j<11; j++) {
			y = specials[j];
			check(x, y);
			check(y, x);
		}
	}

	// check special-special case

	for (i=0; i<11; i++) {
		x = specials[i];
		for (j=0; j<11; j++) {
			y = specials[j];
			check(x, y);
		}
	}
}


#include <iostream>
#include <unistd.h>
#include <vector>
#include "OrthonormalHermite.h"
#include <Eigen/Dense>

void printDetailsOfObject(unsigned int longSleep, unsigned int shortSleep, APPRSDK::OrthonormalHermite<double>& H1, bool showPartialDerivatives=false)
{
    std::cout<<"Base points"<<std::endl;
    usleep(shortSleep);
    std::cout<<H1.GetDomain()<<std::endl;
    std::cout<<"END OF BASE POINTS"<<std::endl<<std::endl;
    usleep(longSleep);

    std::cout<<"Retrieving calculated first degree Hermite function"<<std::endl;
    usleep(shortSleep);
    std::cout<<H1.GetFunctionSystem().col(0)<<std::endl;
    std::cout<<"END OF HERMITE FUNCTION OUTPUT"<<std::endl<<std::endl;
    usleep(longSleep);

    std::cout<<"Retrieving calculated third degree Hermite function"<<std::endl;
    usleep(shortSleep);
    std::cout<<H1.GetFunctionSystem().col(4)<<std::endl;
    std::cout<<"END OF HERMITE FUNCTION OUTPUT"<<std::endl<<std::endl;
    usleep(longSleep);

    std::cout<<"Retrieving calculated derivative first degree"<<std::endl;
    usleep(shortSleep);
    std::cout<<H1.GetDFunctionSystem().col(0)<<std::endl;
    std::cout<<"END OF DERIVATIVE OUTPUT"<<std::endl;
    usleep(longSleep);

    std::cout<<"Retrieving calculated derivative third degree"<<std::endl;
    usleep(shortSleep);
    std::cout<<H1.GetDFunctionSystem().col(4)<<std::endl;
    std::cout<<"END OF DERIVATIVE OUTPUT"<<std::endl;
    usleep(longSleep);

    if (showPartialDerivatives)
    {
        std::cout<<"Retrieving partial derivatives third and fourth columns"<<std::endl;
        usleep(shortSleep);
        std::cout<<H1.GetPartialDerivativesFunctionSystem().col(4)<<std::endl;
        std::cout<<"END OF THIRD COLUMN"<<std::endl;
        std::cout<<H1.GetPartialDerivativesFunctionSystem().col(5)<<std::endl;
        std::cout<<"END OF PARTIAL DERIVATIVE OUTPUT"<<std::endl;
        usleep(longSleep);

        std::cout<<"Retrieving index values"<<std::endl;
        usleep(shortSleep);
        std::cout<<H1.GetIndex()<<std::endl;
        std::cout<<"END OF INDEX OUTPUT"<<std::endl;
    }
}

int main()
{
    const unsigned int longSleep = 3000;
    const unsigned int shortSleep = 1000;
    Eigen::Matrix<double, 1, 2> params;

    std::cout<<"Function system test begun..."<<std::endl;
    APPRSDK::OrthonormalHermite<double> H1(100, 10);
    std::cout<<"OrthonormalHermite object succesfully created"<<std::endl;

    printDetailsOfObject(longSleep, shortSleep, H1);

    std::cout<<"Applying parameters: lambda = 0.5, t = 50"<<std::endl;
    params(0,0) = 0.5;
    params(0,1) = 50;
    H1.ApplyNonLinearParameters(params);
    std::cout<<"Paramtere application succesful. Printing results...";
    usleep(shortSleep);

    printDetailsOfObject(longSleep, shortSleep, H1, true);

    return 0;
}


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

/*
 Copyright (C) 2015 Andres Hernandez

 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 isotropicrandomwalk.hpp
    \brief Isotropic random walk
*/

#ifndef quantlib_isotropic_random_walk_hpp
#define quantlib_isotropic_random_walk_hpp

#include <ql/mathconstants.hpp>
#include <ql/math/randomnumbers/mt19937uniformrng.hpp>
#include <ql/math/array.hpp>
#include <boost/random/variate_generator.hpp>

namespace QuantLib {

    //! Isotropic random walk
    /*! A variate is used to draw from a random element of a
        probability distribution. The draw corresponds to the
        radius of a d-dimensional sphere. The position on the
        surface of the d-dimensional sphere is randomly chosen
        with all points on the surface having the same probability,
        i.e. all directions are isotropic and the step is randomly
        drawn from the given variate.
    */
    template <class Distribution, class Engine>
    class IsotropicRandomWalk {
      public:
        typedef boost::variate_generator<Engine, Distribution> VariateGenerator;
        IsotropicRandomWalk(const Engine& eng, Distribution dist, Size dim,
                            const Array& weights = Array(),
                            unsigned long seed = 0) :
            variate_(eng, dist), rng_(seed),
            weights_(weights), dim_(dim) {
            if (weights_.empty())
                weights_ = Array(dim, 1.0);
            else
                QL_REQUIRE(dim_ == weights_.size(), "Invalid weights");
        }
        template <class InputIterator>
        inline void nextReal(InputIterator first) const {
            Real radius = variate_();
            Array::const_iterator weight = weights_.begin();
            if (dim_ > 1) {
                //Isotropic random direction
                Real phi = M_PI*rng_.nextReal();
                for (Size i = 0; i < dim_ - 2; i++) {
                    *first++ = radius*cos(phi)*(*weight++);
                    radius *= sin(phi);
                    phi = M_PI*rng_.nextReal();
                }
                *first++ = radius*cos(2.0*phi)*(*weight++);
                *first = radius*sin(2.0*phi)*(*weight);
            }
            else {
                if (rng_.nextReal() < 0.5)
                    *first = -radius*(*weight);
                else
                    *first = radius*(*weight);
            }
        }
        inline void setDimension(Size dim) {
            dim_ = dim;
            weights_ = Array(dim, 1.0);
        }
        inline void setDimension(Size dim, const Array& weights) {
            QL_REQUIRE(dim == weights.size(), "Invalid weights");
            dim_ = dim;
            weights_ = weights;
        }
        /*!
        The isotropic random walk will not adjust its draw to be within the lower and upper bounds,
        but if the limits are provided, they are used to rescale the sphere so as to make it to an
        ellipsoid, with different radius in different dimensions.
        */
        inline void setDimension(Size dim,
            const Array& lowerBound, const Array& upperBound) {
            QL_REQUIRE(dim == lowerBound.size(),
                "Incompatible dimension and lower bound");
            QL_REQUIRE(dim == upperBound.size(),
                "Incompatible dimension and upper bound");
            //Find largest bound
            Array bounds = upperBound - lowerBound;
            Real maxBound = bounds[0];
            for (Size j = 1; j < dim; j++) {
                if (bounds[j] > maxBound) maxBound = bounds[j];
            }
            //weights by dimension is the size of the bound
            //divided by the largest bound
            maxBound = 1.0 / maxBound;
            bounds *= maxBound;
            setDimension(dim, bounds);
        }
      protected:
        mutable VariateGenerator variate_;
        MersenneTwisterUniformRng rng_;
        Array weights_;
        Size dim_;
    };
}
#endif


#pragma once

#include <Eigen/Dense>
#include <boost/gil.hpp>
#include <boost/gil/extension/io/png.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/zlib.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <cassert>
#include <cinttypes>
#include <fstream>
#include <iostream>
#include <limits>  // std::numeric_limits
#include <typeinfo>
#include <vector>

#include "fft.hpp"
#include "progressbar.hpp"

using namespace boost::gil;
using Eigen::MatrixXcd, Eigen::MatrixXd;
using std::vector;

// member typedefs provided through inheriting from std::iterator
template <typename Pixel_t, typename Original_t>
class channel_iterator
    : public std::iterator<
          std::random_access_iterator_tag,  // iterator_category
          Pixel_t                           // value_type
          > {
  const Original_t& original;
  size_t channel;

 public:
  explicit channel_iterator(const Original_t& original, size_t channel)
      : original(original), channel(channel){};
  const Pixel_t& operator[](size_t n) const { return original[n][channel]; };
};

template <typename ImgView>
vector<Eigen::MatrixXcd> dft(const ImgView& src) {
  typedef std::complex<double> complex;
  typedef Eigen::Matrix<complex, -1, -1, Eigen::ColMajor> mattype;

  typedef typename channel_type<ImgView>::type cs_t;

  auto h = src.height();
  auto w = src.width();
  constexpr auto nc = num_channels<ImgView>::value;
  vector<mattype> dfts;
  dfts.reserve(nc);
  for (size_t k = 0; k < nc; k++) dfts.push_back(mattype(h, w));

  progressbar bar((h + w) * nc);

  // first do fourier traffo of rows
  for (int y = 0; y < h; y++) {
    typename ImgView::x_iterator src_it = src.row_begin(y);
    for (size_t c = 0; c < nc; c++) {
      bar.update();
      std::vector<complex> buf;
      buf.resize(w);
      if constexpr (nc == 1) {
        fft(src_it, buf.data(), w);
      } else {
        auto src_channel_it =
            channel_iterator<cs_t, typename ImgView::x_iterator>(src_it, c);
        fft(src_channel_it, buf.data(), w);
      }
      for (int x = 0; x < w; x++) dfts[c](y, x) = buf[x];
    }
  }

  // now of cols
  for (int x = 0; x < w; x++) {
    for (size_t c = 0; c < nc; c++) {
      bar.update();
      std::vector<complex> buf;
      buf.resize(h);
      fft(&(dfts[c](0, x)), buf.data(), h);

      for (int y = 0; y < h; y++) dfts[c](y, x) = buf[y];
    }
  }

  return dfts;
}

template <typename ImgView>
vector<Eigen::MatrixXd> dct(const ImgView& src) {
  typedef Eigen::Matrix<double, -1, -1, Eigen::ColMajor> mattype;

  typedef typename channel_type<ImgView>::type cs_t;

  auto h = src.height();
  auto w = src.width();
  auto nc = num_channels<ImgView>::value;
  vector<mattype> dfts;
  dfts.reserve(nc);
  for (size_t k = 0; k < nc; k++) dfts.push_back(mattype(h, w));

  progressbar bar((h + w) * nc);

  // first do fourier traffo of rows
  std::vector<double> buf;
  buf.resize(w);
  for (int y = 0; y < h; y++) {
    typename ImgView::x_iterator src_it = src.row_begin(y);
    for (size_t c = 0; c < nc; c++) {
      bar.update();
      auto src_channel_it =
          channel_iterator<cs_t, typename ImgView::x_iterator>(src_it, c);
      dct(src_channel_it, buf.data(), w);
      for (int x = 0; x < w; x++) dfts[c](y, x) = buf[x];
    }
  }

  // now of cols
  buf.resize(h);
  for (int x = 0; x < w; x++) {
    for (size_t c = 0; c < nc; c++) {
      bar.update();
      dct(&(dfts[c](0, x)), buf.data(), h);

      for (int y = 0; y < h; y++) dfts[c](y, x) = buf[y];
    }
  }

  return dfts;
}

template <typename ImgView>
vector<Eigen::MatrixXd> idct(const ImgView& src) {
  typedef Eigen::Matrix<double, -1, -1, Eigen::ColMajor> mattype;

  typedef typename channel_type<ImgView>::type cs_t;

  auto h = src.height();
  auto w = src.width();
  auto nc = num_channels<ImgView>::value;
  vector<mattype> dfts;
  dfts.reserve(nc);
  for (size_t k = 0; k < nc; k++) dfts.push_back(mattype(h, w));

  progressbar bar((h + w) * nc);

  // first do fourier traffo of rows
  for (int y = 0; y < h; y++) {
    typename ImgView::x_iterator src_it = src.row_begin(y);
    for (size_t c = 0; c < nc; c++) {
      bar.update();
      auto src_channel_it =
          channel_iterator<cs_t, typename ImgView::x_iterator>(src_it, c);
      std::vector<double> buf;
      buf.resize(w);
      idct(src_channel_it, buf.data(), w);
      for (int x = 0; x < w; x++) dfts[c](y, x) = buf[x];
    }
  }

  // now of cols
  for (int x = 0; x < w; x++) {
    for (size_t c = 0; c < nc; c++) {
      bar.update();
      std::vector<double> buf;
      buf.resize(h);
      idct(&(dfts[c](0, x)), buf.data(), h);

      for (int y = 0; y < h; y++) dfts[c](y, x) = buf[y];
    }
  }

  return dfts;
}

std::tuple<double, double> mag_bounds(MatrixXcd& mat) {
  double max = 0;
  double min = std::numeric_limits<double>::max();
  for (Eigen::Index row = 0; row < mat.rows(); row++)
    for (Eigen::Index col = 0; col < mat.cols(); col++) {
      double val = std::abs(mat(row, col));
      if (val > max) max = val;
      if (val < min) min = val;
    }
  return {min, max};
}

std::tuple<double, double> bounds(MatrixXd& mat) {
  double max = std::numeric_limits<double>::min();
  double min = std::numeric_limits<double>::max();
  for (Eigen::Index row = 0; row < mat.rows(); row++)
    for (Eigen::Index col = 0; col < mat.cols(); col++) {
      double val = mat(row, col);
      if (val > max) max = val;
      if (val < min) min = val;
    }
  return {min, max};
}

std::tuple<double, double> real_bounds(MatrixXcd& mat) {
  double max = std::numeric_limits<double>::min();
  double min = std::numeric_limits<double>::max();
  for (Eigen::Index row = 0; row < mat.rows(); row++)
    for (Eigen::Index col = 0; col < mat.cols(); col++) {
      double val = mat(row, col).real();
      if (val > max) max = val;
      if (val < min) min = val;
    }
  return {min, max};
}

template <typename SrcView, typename DstView>
void dft(const SrcView& src, DstView& dst_mag, DstView& dst_phase,
         bool shifted = true, bool log = true) {
  assert(src.dimensions() == dst_mag.dimensions());
  assert(src.dimensions() == dst_phase.dimensions());

  typedef typename channel_type<DstView>::type cs_t;
  cs_t max_val = std::numeric_limits<cs_t>::max();

  auto mats = dft(src);

  std::vector<std::tuple<double, double>> limits;
  limits.reserve(mats.size());
  for (size_t k = 0; k < mats.size(); k++)
    limits.push_back(mag_bounds(mats[k]));

  auto w = dst_mag.width();
  auto h = dst_mag.height();

  for (int y = 0; y < h; y++) {
    typename DstView::x_iterator dst_mag_it = dst_mag.row_begin(y);
    typename DstView::x_iterator dst_phase_it = dst_phase.row_begin(y);
    for (int x = 0; x < w; x++)
      for (size_t c = 0; c < mats.size(); c++) {
        auto [min, max] = limits[c];
        Eigen::Index xcord, ycord;
        if (shifted) {
          ycord = (y + h / 2) % h;
          xcord = (x + w / 2) % w;
        } else {
          xcord = x;
          ycord = y;
        }
        if (log) {
          dst_mag_it[x][c] =
              (std::log(std::abs(mats[c](ycord, xcord))) - std::log(min)) /
              (std::log(max) - std::log(min)) * max_val;
        } else {
          dst_mag_it[x][c] = ((std::abs(mats[c](ycord, xcord))) - (min)) /
                             ((max) - (min)) * max_val;
        }
        dst_phase_it[x][c] =
            std::arg(mats[c](ycord, xcord)) / 2. / pi * max_val;
      }
  }
}

template <typename SrcView, typename DstView>
void dct(const SrcView& src, DstView& dst) {
  assert(src.dimensions() == dst.dimensions());

  // typedef typename channel_type<DstView>::type cs_t;
  // cs_t max_val = std::numeric_limits<cs_t>::max();

  auto mats = dct(src);

  std::vector<std::tuple<double, double>> limits;
  limits.reserve(mats.size());
  for (size_t k = 0; k < mats.size(); k++) limits.push_back(bounds(mats[k]));

  auto w = dst.width();
  auto h = dst.height();

  for (int y = 0; y < h; y++) {
    typename DstView::x_iterator dst_it = dst.row_begin(y);
    for (int x = 0; x < w; x++)
      for (size_t c = 0; c < mats.size(); c++) {
        // auto [min, max] = limits[c];
        dst_it[x][c] = mats[c](y, x);
      }
  }
}

template <typename SrcView, typename DstView>
void idct(const SrcView& src, DstView& dst) {
  assert(src.dimensions() == dst.dimensions());

  typedef typename channel_type<DstView>::type cs_t;
  cs_t max_val = std::numeric_limits<cs_t>::max();

  auto mats = idct(src);

  std::vector<std::tuple<double, double>> limits;
  limits.reserve(mats.size());
  for (size_t k = 0; k < mats.size(); k++) limits.push_back(bounds(mats[k]));

  auto w = dst.width();
  auto h = dst.height();

  for (int y = 0; y < h; y++) {
    typename DstView::x_iterator dst_it = dst.row_begin(y);
    for (int x = 0; x < w; x++)
      for (size_t c = 0; c < mats.size(); c++) {
        auto [min, max] = limits[c];
        dst_it[x][c] = (mats[c](y, x) - min) / (max - min) * max_val;
        ;
      }
  }
}

size_t closest_smaller_power2(size_t number) {
  size_t log2n = 0;
  while ((number >> ++log2n) > 0) {
  };

  return 1 << (log2n - 1);
}

template <typename DstView>
void to_image(vector<MatrixXcd>& src, DstView& dst) {
  assert(src[0].cols() == dst.width());
  assert(src[0].rows() == dst.height());

  typedef typename channel_type<DstView>::type cs_t;
  cs_t max_val = std::numeric_limits<cs_t>::max();

  std::vector<std::tuple<double, double>> limits;
  limits.reserve(src.size());
  for (size_t k = 0; k < src.size(); k++) limits.push_back(mag_bounds(src[k]));

  auto w = src[0].cols();
  auto h = src[0].rows();

  for (int y = 0; y < h; y++) {
    typename DstView::x_iterator dst_it = dst.row_begin(y);
    for (int x = 0; x < w; x++)
      for (size_t c = 0; c < src.size(); c++) {
        auto [min, max] = limits[c];

        dst_it[x][c] = std::abs(src[c](y, x)) / max * max_val;
      }
  }
}

template <typename DstView>
void to_image_mag(vector<MatrixXcd>& src, DstView& dst) {
  assert(src[0].cols() == dst.width());
  assert(src[0].rows() == dst.height());

  typedef typename channel_type<DstView>::type cs_t;
  cs_t max_val = std::numeric_limits<cs_t>::max();

  std::vector<std::tuple<double, double>> limits;
  limits.reserve(src.size());
  for (size_t k = 0; k < src.size(); k++) limits.push_back(mag_bounds(src[k]));

  auto w = src[0].cols();
  auto h = src[0].rows();

  for (int y = 0; y < h; y++) {
    typename DstView::x_iterator dst_it = dst.row_begin(y);
    for (int x = 0; x < w; x++)
      for (size_t c = 0; c < src.size(); c++) {
        auto [min, max] = limits[c];

        auto ycord = (y + h / 2) % h;
        auto xcord = (x + w / 2) % w;
        dst_it[x][c] = (std::log(std::abs(src[c](ycord, xcord)) + 1e-6) -
                        std::log(min + 1e-6)) /
                       (std::log(max) - std::log(min + 1e-6)) * max_val;
      }
  }
}

void apply_filter(const char* from, const char* to, const char* fs,
                  std::function<void(MatrixXcd&)> filter, bool gray = false) {
  rgb8_image_t img;
  read_image(from, img, png_tag());

  // typedef typename channel_type<rgb8_image_t>::type cs_t;
  // cs_t max_val = std::numeric_limits<cs_t>::max();

  std::vector<MatrixXcd> mats;
  if (gray) {
    mats = dft(color_converted_view<gray8_pixel_t>(view(img)));
  } else
    mats = dft(view(img));

  auto w = img.width();
  auto h = img.height();

  // apply filter
  for (size_t c = 0; c < mats.size(); c++) filter(mats[c]);

  if (gray) {
    gray8_image_t img_control(img.dimensions());
    to_image_mag(mats, view(img_control));
    write_view(fs, view(img_control), png_tag());
  } else {
    rgb8_image_t img_control(img.dimensions());
    to_image_mag(mats, view(img_control));
    write_view(fs, view(img_control), png_tag());
  }

  // transform back
  // first do fourier traffo of rows
  progressbar bar((h + w) * mats.size());
  std::vector<complex<double>> buf;
  buf.resize(w);
  for (int y = 0; y < h; y++) {
    for (size_t c = 0; c < mats.size(); c++) {
      bar.update();
      ifft(mats[c].row(y), buf.data(), w);
      for (int x = 0; x < w; x++) mats[c](y, x) = buf[x];
    }
  }
  // now of columns
  buf.resize(h);
  for (int x = 0; x < w; x++) {
    for (size_t c = 0; c < mats.size(); c++) {
      bar.update();
      ifft(mats[c].col(x), buf.data(), h);
      for (int y = 0; y < h; y++) mats[c](y, x) = buf[y];
    }
  }

  if (gray) {
    gray8_image_t sharpened(img.dimensions());
    to_image(mats, view(sharpened));

    write_view(to, view(sharpened), png_tag());
  } else {
    rgb8_image_t sharpened(img.dimensions());
    to_image(mats, view(sharpened));

    write_view(to, view(sharpened), png_tag());
  }
}

void sharpen(const char* from, const char* to, const char* fs, double radius,
             bool gray = false) {
  apply_filter(
      from, to, fs,
      [&](MatrixXcd& mat) {
        double limit = radius * std::min(mat.cols(), mat.rows()) / 2.;
        limit *= limit;
        for (int x = 0; x < mat.cols(); x++)
          for (int y = 0; y < mat.rows(); y++) {
            double pos_x =
                ((x + mat.cols() / 2) % mat.cols()) - (mat.cols() - 1) / 2.;
            double pos_y =
                ((y + mat.rows() / 2) % mat.rows()) - (mat.rows() - 1) / 2.;
            if (pos_x * pos_x + pos_y * pos_y < limit) {
              mat(y, x) = 0;
            }
          }
      },
      gray);
}

void sharpen_smooth(const char* from, const char* to, const char* fs,
                    double exp_fac, bool gray = false) {
  apply_filter(
      from, to, fs,
      [&](MatrixXcd& mat) {
        for (int x = 0; x < mat.cols(); x++)
          for (int y = 0; y < mat.rows(); y++) {
            double pos_x =
                ((x + mat.cols() / 2) % mat.cols()) - (mat.cols() - 1) / 2.;
            double pos_y =
                ((y + mat.rows() / 2) % mat.rows()) - (mat.rows() - 1) / 2.;

            double r2 = pos_x * pos_x + pos_y * pos_y;
            mat(y, x) *= (1 - std::exp(-r2 * exp_fac));
          }
      },
      gray);
}

void blur_smooth(const char* from, const char* to, const char* fs,
                 double exp_fac, bool gray = false) {
  apply_filter(
      from, to, fs,
      [&](MatrixXcd& mat) {
        for (int x = 0; x < mat.cols(); x++)
          for (int y = 0; y < mat.rows(); y++) {
            double pos_x =
                ((x + mat.cols() / 2) % mat.cols()) - (mat.cols() - 1) / 2.;
            double pos_y =
                ((y + mat.rows() / 2) % mat.rows()) - (mat.rows() - 1) / 2.;

            double r2 = pos_x * pos_x + pos_y * pos_y;
            mat(y, x) *= std::exp(-r2 * exp_fac);
          }
      },
      gray);
}

void blur(const char* from, const char* to, const char* fs, double radius,
          bool gray = false) {
  apply_filter(
      from, to, fs,
      [&](MatrixXcd& mat) {
        double limit = radius * std::min(mat.cols(), mat.rows()) / 2.;
        for (int x = 0; x < mat.cols(); x++)
          for (int y = 0; y < mat.rows(); y++) {
            double pos_x =
                ((x + mat.cols() / 2) % mat.cols()) - (mat.cols() - 1) / 2.;
            double pos_y =
                ((y + mat.rows() / 2) % mat.rows()) - (mat.rows() - 1) / 2.;
            if (pos_x * pos_x + pos_y * pos_y > limit * limit) {
              mat(y, x) = 0;
            }
          }
      },
      gray);
}

void rect_filter(const char* from, const char* to, const char* fs, double width,
                 double height, bool gray = false) {
  apply_filter(
      from, to, fs,
      [&](MatrixXcd& mat) {
        for (int x = 0; x < mat.cols(); x++)
          for (int y = 0; y < mat.rows(); y++) {
            double pos_x =
                ((x + mat.cols() / 2) % mat.cols()) - (mat.cols() - 1) / 2.;
            double pos_y =
                ((y + mat.rows() / 2) % mat.rows()) - (mat.rows() - 1) / 2.;
            if (pos_x > mat.cols() * width / 2 ||
                pos_x < -mat.cols() * width / 2 ||
                pos_y > mat.rows() * height / 2 ||
                pos_y < -mat.rows() * height / 2) {
              mat(y, x) = 0;
            }
          }
      },
      gray);
}

void anti_rect_filter(const char* from, const char* to, const char* fs,
                      double width, double height, bool gray = false) {
  apply_filter(
      from, to, fs,
      [&](MatrixXcd& mat) {
        for (int x = 0; x < mat.cols(); x++)
          for (int y = 0; y < mat.rows(); y++) {
            double pos_x =
                ((x + mat.cols() / 2) % mat.cols()) - (mat.cols() - 1) / 2.;
            double pos_y =
                ((y + mat.rows() / 2) % mat.rows()) - (mat.rows() - 1) / 2.;
            if (!(pos_x > mat.cols() * width / 2 ||
                  pos_x < -mat.cols() * width / 2 ||
                  pos_y > mat.rows() * height / 2 ||
                  pos_y < -mat.rows() * height / 2)) {
              mat(y, x) = 0;
            }
          }
      },
      gray);
}

typedef uint_least16_t pos_t;
typedef uint_least8_t channel_t;
typedef std::tuple<pos_t, pos_t, channel_t> index_t;
void compress_image(const char* from, const char* to,
                    double compression_level) {
  rgb8_image_t img;
  read_image(from, img, png_tag());
  std::cout << "Image read! Dimensions x: " << img.dimensions().x
            << " y: " << img.dimensions().y << std::endl;

  // typedef typename channel_type<rgb8_image_t>::type cs_t;
  // cs_t max_val = std::numeric_limits<cs_t>::max();

  auto mats = dct(view(img));
  // x, y, c
  std::vector<index_t> indices;

  indices.reserve(mats[0].cols() * mats[0].rows() * mats.size());
  for (Eigen::Index x = 0; x < mats[0].cols(); x++)
    for (Eigen::Index y = 0; y < mats[0].rows(); y++)
      for (size_t c = 0; c < mats.size(); c++) indices.push_back({x, y, c});

  std::sort(indices.begin(), indices.end(),
            [&](index_t& a, index_t& b) -> bool {
              auto [xa, ya, ca] = a;
              auto [xb, yb, cb] = b;
              return std::abs(mats[ca](ya, xa)) > std::abs(mats[cb](yb, xb));
            });

  std::fstream file(to, file.binary | file.trunc | file.out);
  if (!file.is_open()) {
    throw std::runtime_error("failed to open file");
  } else {
    boost::iostreams::filtering_ostream os;
    os.push(boost::iostreams::zlib_compressor());
    os.push(file);
    // write dimensions
    uint_least16_t width = mats[0].cols();
    uint_least16_t height = mats[0].rows();
    os.write(reinterpret_cast<char*>(&width), sizeof width);  // binary output
    os.write(reinterpret_cast<char*>(&height),
             sizeof height);  // binary output

    size_t lastk = indices.size() * compression_level;
    progressbar b2(lastk);

    for (size_t k = 0; k < lastk; k++) {
      b2.update();
      auto [x, y, c] = indices[k];
      float val = mats[c](y, x);

      os.write(reinterpret_cast<char*>(&x), sizeof x);      // binary output
      os.write(reinterpret_cast<char*>(&y), sizeof y);      // binary output
      os.write(reinterpret_cast<char*>(&c), sizeof c);      // binary output
      os.write(reinterpret_cast<char*>(&val), sizeof val);  // binary output
    }
  }
}

void decompress_image(const char* from, const char* to) {
  std::fstream file(from, file.binary | file.in);
  if (!file.is_open()) {
    throw std::runtime_error("failed to open file");
  }
  boost::iostreams::filtering_istream is;
  is.push(boost::iostreams::zlib_decompressor());
  is.push(file);
  // read dimensions
  uint_least16_t width, height;
  is.read(reinterpret_cast<char*>(&width), sizeof width);
  is.read(reinterpret_cast<char*>(&height), sizeof height);
  std::vector<MatrixXd> mats;
  for (int c = 0; c < 3; c++) mats.push_back(MatrixXd::Zero(height, width));

  while (!is.eof()) {
    pos_t x, y;
    channel_t c;
    float val;

    is.read(reinterpret_cast<char*>(&x), sizeof x);      // binary input
    is.read(reinterpret_cast<char*>(&y), sizeof y);      // binary input
    is.read(reinterpret_cast<char*>(&c), sizeof c);      // binary input
    is.read(reinterpret_cast<char*>(&val), sizeof val);  // binary input

    mats[c](y, x) = val;
  }

  // first do fourier traffo of rows
  std::vector<double> buf;
  buf.resize(width);
  for (int y = 0; y < height; y++) {
    for (size_t c = 0; c < 3; c++) {
      idct(mats[c].row(y), buf.data(), width);
      for (int x = 0; x < width; x++) mats[c](y, x) = buf[x];
    }
  }

  rgb8_image_t img(width, height);
  // typedef typename channel_type<rgb8_image_t>::type cs_t;
  // cs_t max_val = std::numeric_limits<cs_t>::max();
  auto img_view = view(img);

  // now of cols
  buf.resize(height);
  for (int x = 0; x < width; x++) {
    for (size_t c = 0; c < 3; c++) {
      idct(mats[c].col(x), buf.data(), height);
      for (int y = 0; y < height; y++) img_view(x, y)[c] = buf[y];
    }
  }

  write_view(to, img_view, png_tag());
}

void dft_image(const char* from, const char* to_mag, const char* to_phase,
               bool crop = true, bool log = true, bool shifted = true) {
  rgb8_image_t img;
  read_image(from, img, png_tag());
  std::cout << "Image read! Dimensions x: " << img.dimensions().x
            << " y: " << img.dimensions().y << std::endl;

  rgb8_image_t img_ft_mag, img_ft_phase;
  if (crop) {
    auto subw = closest_smaller_power2(img.width());
    auto subh = closest_smaller_power2(img.height());
    auto sub_img = subimage_view(view(img), 0, 0, subw, subh);
    img_ft_mag = rgb8_image_t(sub_img.dimensions());
    img_ft_phase = rgb8_image_t(sub_img.dimensions());
    dft(sub_img, view(img_ft_mag), view(img_ft_phase), shifted, log);
  } else {
    img_ft_mag = rgb8_image_t(img.dimensions());
    img_ft_phase = rgb8_image_t(img.dimensions());
    dft(view(img), view(img_ft_mag), view(img_ft_phase), shifted, log);
  }

  write_view(to_mag, view(img_ft_mag), png_tag());
  write_view(to_phase, view(img_ft_phase), png_tag());
}

void dct_image(const char* from, const char* to, bool crop = false) {
  rgb8_image_t img;
  read_image(from, img, png_tag());
  std::cout << "Image read! Dimensions x: " << img.dimensions().x
            << " y: " << img.dimensions().y << std::endl;

  rgb8_image_t img_dct;
  if (crop) {
    auto subw = closest_smaller_power2(img.width());
    auto subh = closest_smaller_power2(img.height());
    auto sub_img = subimage_view(view(img), 0, 0, subw, subh);
    img_dct = rgb8_image_t(sub_img.dimensions());
    dct(sub_img, view(img_dct));
  } else {
    img_dct = rgb8_image_t(img.dimensions());
    dct(view(img), view(img_dct));
  }

  write_view(to, view(img_dct), png_tag());
}

void idct_image(const char* from, const char* to, bool crop = false) {
  rgb8_image_t img;
  read_image(from, img, png_tag());
  std::cout << "Image read! Dimensions x: " << img.dimensions().x
            << " y: " << img.dimensions().y << std::endl;

  rgb8_image_t img_dct;
  if (crop) {
    auto subw = closest_smaller_power2(img.width());
    auto subh = closest_smaller_power2(img.height());
    auto sub_img = subimage_view(view(img), 0, 0, subw, subh);
    img_dct = rgb8_image_t(sub_img.dimensions());
    idct(sub_img, view(img_dct));
  } else {
    img_dct = rgb8_image_t(img.dimensions());
    idct(view(img), view(img_dct));
  }

  write_view(to, view(img_dct), png_tag());
}

void image_test() {
  using namespace boost::gil;

  std::string filename("images/webb.png");
  rgb8_image_t img;
  read_image(filename, img, png_tag());
  std::cout << "Image read! Dimensions x: " << img.dimensions().x
            << " y: " << img.dimensions().y << std::endl;

  gray8_image_t img_ft_mag(img.dimensions());
  gray8_image_t img_ft_phase(img.dimensions());
  std::cout << "Now performing a gray naive fourier transform" << std::endl;

  // dft(color_converted_view<gray8_pixel_t>(view(img)), view(img_ft_mag),
  // view(img_ft_phase));
  std::cout << "Saving image" << std::endl;
  write_view("build/output/test_mag.png", view(img_ft_mag), png_tag());
  write_view("build/output/test_phase.png", view(img_ft_phase), png_tag());

  std::cout
      << "Now performing a fourier transform of closest power of two subimage"
      << std::endl;
  auto subw = closest_smaller_power2(img.width());
  auto subh = closest_smaller_power2(img.height());
  auto sub_img = subimage_view(color_converted_view<gray8_pixel_t>(view(img)),
                               0, 0, subw, subh);
  gray8_image_t img_sub_ft_mag(sub_img.dimensions());
  gray8_image_t img_sub_ft_phase(sub_img.dimensions());

  dft(sub_img, view(img_sub_ft_mag), view(img_sub_ft_phase));
  std::cout << "Saving image" << std::endl;
  write_view("build/output/sub_test_mag.png", view(img_sub_ft_mag), png_tag());
  write_view("build/output/sub_test_phase.png", view(img_sub_ft_phase),
             png_tag());

  std::cout << "Now performing a colored fourier transform of closest power of "
               "two subimage"
            << std::endl;
  auto sub_img_c = subimage_view(view(img), 0, 0, subw, subh);
  rgb8_image_t img_sub_c_ft_mag(sub_img_c.dimensions());
  rgb8_image_t img_sub_c_ft_phase(sub_img_c.dimensions());

  dft(sub_img_c, view(img_sub_c_ft_mag), view(img_sub_c_ft_phase));
  std::cout << "Saving image" << std::endl;
  write_view("build/output/sub_c_test_mag.png", view(img_sub_c_ft_mag),
             png_tag());
  write_view("build/output/sub_c_test_phase.png", view(img_sub_c_ft_phase),
             png_tag());

  std::cout << "Now performing a colored cosine transform of closest power of "
               "two subimage"
            << std::endl;
  rgb8_image_t img_sub_c_dct(sub_img_c.dimensions());

  dct(sub_img_c, view(img_sub_c_dct));
  std::cout << "Saving image" << std::endl;
  write_view("build/output/sub_c_test_dct.png", view(img_sub_c_dct), png_tag());
}

void sharpen_test() {
  sharpen("images/dune.png", "build/output/dune_sharp.png",
          "build/output/dune_sharp_mask.png", .1);
  blur("images/dune.png", "build/output/dune_blur.png",
       "build/output/dune_blur_mask.png", .5);
}

void compress_test() {
  compress_image("images/dune.png", "build/output/dune_cmp.ldw", .01);
  decompress_image("build/output/dune_cmp.ldw", "build/output/reconst.png");
}


#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <sm/assert_macros.hpp>
#include <sm/kinematics/rotations.hpp>
#include <sm/kinematics/three_point_methods.hpp>

namespace sm {
namespace kinematics {

// Original code from the ROS vslam package pe3d.cpp
// uses the SVD procedure for aligning point clouds
//   SEE: Arun, Huang, Blostein: Least-Squares Fitting of Two 3D Point Sets
Eigen::Matrix4d threePointSvd(Eigen::MatrixXd const& p0, Eigen::MatrixXd const& p1) {
    using namespace Eigen;

    SM_ASSERT_EQ_DBG(std::runtime_error, p0.rows(), 3, "p0 must be a 3xK matrix");
    SM_ASSERT_EQ_DBG(std::runtime_error, p1.rows(), 3, "p1 must be a 3xK matrix");
    SM_ASSERT_EQ_DBG(std::runtime_error, p0.cols(), p1.cols(), "p0 and p1 must have the same number of columns");

    Vector3d c0 = p0.rowwise().mean();
    Vector3d c1 = p1.rowwise().mean();

    Matrix3d H(Matrix3d::Zero());
    // subtract out
    // p0a -= c0;
    // p0b -= c0;
    // p0c -= c0;
    // p1a -= c1;
    // p1b -= c1;
    // p1c -= c1;

    // Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
    // 	p1c*p0c.transpose();
    for (int i = 0; i < p0.cols(); ++i) {
        H += (p0.col(i) - c0) * (p1.col(i) - c1).transpose();
    }

    // do the SVD thang
    JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
    Matrix3d V = svd.matrixV();
    Matrix3d R = V * svd.matrixU().transpose();
    double det = R.determinant();

    if (det < 0.0) {
        V.col(2) = V.col(2) * -1.0;
        R = V * svd.matrixU().transpose();
    }
    Vector3d tr = c0 - R.transpose() * c1;  // translation

    // transformation matrix, 3x4
    Matrix4d tfm(Matrix4d::Identity());
    //        tfm.block<3,3>(0,0) = R.transpose();
    //        tfm.col(3) = -R.transpose()*tr;
    tfm.topLeftCorner<3, 3>() = R.transpose();
    tfm.topRightCorner<3, 1>() = tr;

    return tfm;
}

Eigen::Matrix3d qMethod(Eigen::MatrixXd const& p0, Eigen::MatrixXd const& p1, const Eigen::VectorXd& w) {
    SM_ASSERT_EQ_DBG(std::runtime_error, p0.rows(), 3, "p0 must be a 3xK matrix");
    SM_ASSERT_EQ_DBG(std::runtime_error, p1.rows(), 3, "p1 must be a 3xK matrix");
    SM_ASSERT_EQ_DBG(std::runtime_error, p0.cols(), p1.cols(), "p0 and p1 must have the same number of columns");
    SM_ASSERT_EQ_DBG(std::runtime_error, w.size(), p0.cols(), "w must have the same number of columns as p0");

    Eigen::MatrixXd W = p0;
    Eigen::MatrixXd V = p1;

    for (int i = 0; i < p0.cols(); i++) {
        double wi = sqrt(w[i]);
        SM_ASSERT_NEAR_DBG(std::runtime_error, p0.col(i).norm(), 1.0, 1e-4,
                           "Column " << i << " of p0 was not a unit vector");
        SM_ASSERT_NEAR_DBG(std::runtime_error, p1.col(i).norm(), 1.0, 1e-4,
                           "Column " << i << " of p1 was not a unit vector");

        W.col(i) = wi * W.col(i);
        V.col(i) = wi * V.col(i);
    }

    Eigen::MatrixXd B = W * V.transpose();
    Eigen::MatrixXd Q = B + B.transpose();

    Eigen::Vector3d Z(B(1, 2) - B(2, 1), B(2, 0) - B(0, 2), B(0, 1) - B(1, 0));
    double sigma = B(0, 0) + B(1, 1) + B(2, 2);

    Eigen::Matrix4d K;
    K.topLeftCorner<3, 3>() = Q - sigma * Eigen::Matrix3d::Identity();
    K.topRightCorner<3, 1>() = Z;
    K.bottomLeftCorner<1, 3>() = Z.transpose();
    K(3, 3) = sigma;

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(K);

    Eigen::Vector4d eigenvalues = eigensolver.eigenvalues();
    // Find the maximum eigenvalue

    double maxVal = eigenvalues(0);
    int maxValIdx = 0;
    for (int i = 1; i < 4; i++) {
        if (eigenvalues(i) > maxVal) {
            maxVal = eigenvalues(i);
            maxValIdx = i;
        }
    }

    // The corresponding eigenvector is the quaternion q_01
    Eigen::Vector4d q_01 = eigensolver.eigenvectors().col(maxValIdx);
    q_01 /= q_01.norm();

    Eigen::Vector3d qv = q_01.head<3>();
    double qs = q_01(3);

    Eigen::Matrix3d C_01 = (qs * qs - qv.dot(qv)) * Eigen::Matrix3d::Identity() + 2.0 * qv * qv.transpose() -
                           2.0 * qs * sm::kinematics::crossMx(qv);

    return C_01;
}

Eigen::Matrix3d qMethod(Eigen::MatrixXd const& p0, Eigen::MatrixXd const& p1) {
    return qMethod(p0, p1, Eigen::VectorXd::Ones(p1.cols()));
}

}  // namespace kinematics
}  // namespace sm


#pragma once

#include <cmath>
#include <cstdint>

#include <Eigen/Core>

#include "surface_normal/base.hpp"
#include "surface_normal/cuda_compatibility.hpp"
#include "svd3_cuda.hpp"

namespace surface_normal {

__host__ __device__ __forceinline__ uint8_t f2b(float x) {
  return static_cast<uint8_t>(127.5 * (1 - x));
}

template <typename T>
__host__ __device__ __forceinline__ void
depth_to_normals_rgb_inner(const ImageView<const T> &depth, ImageView<uint8_t, 3> &normals,
                           const CameraIntrinsics &intrinsics, int radius, float max_rel_depth_diff,
                           int center_row, int center_col) {
  float center_depth = depth.at(center_row, center_col);
  if (center_depth == 0) {
    return;
  }

  float f_inv = 1.f / intrinsics.f;
  float cx    = intrinsics.cx;
  float cy    = intrinsics.cy;
  Eigen::Vector3f mid{(center_col - cx) * center_depth * f_inv,
                      (center_row - cy) * center_depth * f_inv, center_depth};
  int n                          = 0;
  Eigen::Vector3f centroid       = Eigen::Vector3f::Zero();
  Eigen::Matrix3f outer_prod_sum = Eigen::Matrix3f::Zero();
  for (int i = -radius; i <= radius; i++) {
    for (int j = -radius; j <= radius; j++) {
      int x = center_col + j;
      int y = center_row + i;

      if (x < 0 || x >= depth.width || y < 0 || y >= depth.height) {
        continue;
      }

      float z = depth.at(y, x);
      if (z == 0 || std::abs(z - center_depth) > max_rel_depth_diff * center_depth) {
        continue;
      }

      Eigen::Vector3f p{(x - cx) * z * f_inv, (y - cy) * z * f_inv, z};
      p -= mid; // subtract midpoint for improved numeric stability in outer product
      centroid += p;
      // '* 1' to suppress
      // warning: calling a __host__ function from a __host__ __device__ function is not allowed
      outer_prod_sum += p * p.transpose() * 1;
      n++;
    }
  }

  if (n < 3)
    return;

  centroid /= n;
  Eigen::Matrix3f cov = (outer_prod_sum - n * centroid * centroid.transpose()) / (n - 1);

  Eigen::Matrix3f U, V;
  Eigen::Vector3f S;
  svd(cov(0, 0), cov(0, 1), cov(0, 2), cov(1, 0), cov(1, 1), cov(1, 2), cov(2, 0), cov(2, 1),
      cov(2, 2),                                                                       // cov
      U(0, 0), U(0, 1), U(0, 2), U(1, 0), U(1, 1), U(1, 2), U(2, 0), U(2, 1), U(2, 2), // output U
      S(0), S(1), S(2),                                                                // output S
      V(0, 0), V(0, 1), V(0, 2), V(1, 0), V(1, 1), V(1, 2), V(2, 0), V(2, 1), V(2, 2)  // output V
  );
  Eigen::Vector3f normal = V.col(2).normalized();

  if (mid.dot(normal) < 0) {
    normal *= -1;
  }

  normals.at(center_row, center_col, 0) = f2b(normal(0));
  normals.at(center_row, center_col, 1) = f2b(normal(2));
  normals.at(center_row, center_col, 2) = f2b(normal(1));
}
} // namespace surface_normal


// Copyright Matt Overby 2021.
// Distributed under the MIT License.

#ifndef MCL_COMPUTEMASSES_HPP
#define MCL_COMPUTEMASSES_HPP 1

#include <Eigen/Dense>

namespace mcl
{

// V are vertices (n x 2 or 3)
// P are primitives (m x 3 or 4)
// M is n x 1 of per-vertex masses
// Computes volume (or area) weighted masses with unit-volume density.
// If negative, defaults are used: 1100 for volumetric, 0.4 for cloth/2D
// See: https://www.engineeringtoolbox.com/density-solids-d_1265.html.
// Masses between the min and max indices in P are set.
// If unreferenced, they are set to zero.
// Returns true if all masses in span are positive.
template <typename DerivedV, typename DerivedP, typename Scalar>
static inline bool compute_masses(
	const Eigen::MatrixBase<DerivedV> &V,
	const Eigen::MatrixBase<DerivedP> &P,
	Eigen::Matrix<Scalar,Eigen::Dynamic,1> &M,
	double density_kgd = -1)
{
	using namespace Eigen;
	int V_dim = V.cols();
	int P_dim = P.cols();
	if (V_dim < 2 || V_dim > 3) { return false; }
	if (P_dim < 3 || P_dim > 4) { return false; }

	// Use 3D vec for calculation even if 2D
	auto Vi = [&](int idx)
	{
		Vector3d vi = Vector3d::Zero();
		vi.head(V_dim) = V.row(idx).template cast<double>();
		return vi;
	};

	// Default densities
	if (density_kgd < 0)
	{
		if (V_dim == 2 || P_dim == 3) {
			density_kgd = 0.4;
		}
		else if (V_dim == 3 && P_dim == 4) {
			density_kgd = 1100;
		}
	}

	// Resize masses if needed and set span to zero
	int min_Pi = P.minCoeff();
	int max_Pi = P.maxCoeff();
	if (M.rows() < max_Pi) {
		M.conservativeResize(max_Pi+1);
	}
	M.segment(min_Pi, max_Pi-min_Pi+1).array() = 0;

	// Compute mass contrib from each element
	int np = P.rows();
	for (int i=0; i<np; ++i)
	{
		if (P_dim == 4)
		{
			Vector3d p_verts[4] = {
				Vi(P(i,0)),
				Vi(P(i,1)),
				Vi(P(i,2)),
				Vi(P(i,3)) };
			Matrix<double,3,3> E;
			E.col(0) = p_verts[1] - p_verts[0];
			E.col(1) = p_verts[2] - p_verts[0];
			E.col(2) = p_verts[3] - p_verts[0];
			double vol = std::abs(E.determinant()/6.0);
			double tet_mass = density_kgd * vol;
			M[P(i,0)] += Scalar(tet_mass / 4.0);
			M[P(i,1)] += Scalar(tet_mass / 4.0);
			M[P(i,2)] += Scalar(tet_mass / 4.0);
			M[P(i,3)] += Scalar(tet_mass / 4.0);
		}
		else if (P_dim == 3)
		{
			Vector3d p_verts[3] = {
				Vi(P(i,0)),
				Vi(P(i,1)),
				Vi(P(i,2)) };
			Vector3d e0 = p_verts[1] - p_verts[0];
			Vector3d e1 = p_verts[2] - p_verts[0];
			double area = 0.5 * (e0.cross(e1)).norm();
			double tri_mass = density_kgd * area;
			M[P(i,0)] += Scalar(tri_mass / 3.0);
			M[P(i,1)] += Scalar(tri_mass / 3.0);
			M[P(i,2)] += Scalar(tri_mass / 3.0);
		}
	}

	double min_mass = M.segment(min_Pi, max_Pi-min_Pi+1).minCoeff();
	return min_mass > 0;
}

} // ns mcl

#endif


#include <CGAL/Cartesian.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_2.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_traits_2.h>
#include <CGAL/Periodic_4_hyperbolic_triangulation_2/internal/Periodic_4_hyperbolic_triangulation_dummy_14.h>
#include <CGAL/Hyperbolic_octagon_translation.h>
#include <CGAL/Algebraic_kernel_for_circles_2_2.h>
#include <CGAL/Circular_kernel_2.h>

#include <boost/tuple/tuple.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/variate_generator.hpp>

#include <iostream>

typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_traits_2<>               Traits;
typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_2<Traits>                Triangulation;
typedef Triangulation::Face_handle                                                  Face_handle;
typedef Triangulation::Vertex_handle                                                Vertex_handle;
typedef Triangulation::Locate_type                                                  Locate_type;
typedef Triangulation::Hyperbolic_translation                                       Hyperbolic_translation;
typedef Triangulation::Point                                                        Point;

std::ostream& operator<<(std::ostream& s, const Locate_type& lt)
{
  switch(lt)
  {
    case Triangulation::VERTEX:
      s << "VERTEX";
      break;
    case Triangulation::FACE:
      s << "FACE";
      break;
    case Triangulation::EDGE:
      s << "EDGE";
      break;
  }

  return s;
}

int main(int, char**)
{
  Triangulation tr;

  assert(tr.is_valid());

  Locate_type lt;
  int li;
  Face_handle fh;

  std::cout << "---- locating dummy points (all should be vertices) ----" << std::endl;
  for(int j=0; j<14; ++j) {
    Point query = tr.get_dummy_point(j);
    fh = tr.hyperbolic_locate(query, lt, li);
    assert(lt == Triangulation::VERTEX);
    std::cout << "   dummy point " << j << ": OK " << std::endl;
  }

  std::cout << "---- locating the midpoint of a Euclidean segment ----" << std::endl;
  Point p1 = tr.get_dummy_point(0), p2 = tr.get_dummy_point(1);
  Point query = midpoint(p1, p2);
  fh = tr.hyperbolic_locate(query, lt, li);
  assert(lt == Triangulation::EDGE);
  std::cout << "   located as edge OK" << std::endl;

  std::cout << "---- inserting a single point and locating it ----" << std::endl;
  Vertex_handle v = tr.insert(Point(-0.4, -0.1));
  fh = tr.hyperbolic_locate(v->point(), lt, li);
  assert(lt == Triangulation::VERTEX);
  std::cout << "   located as vertex OK" << std::endl;

  // TODO: add a test case for a circular edge!

  return EXIT_SUCCESS;
}


#ifndef INTEGRALS_HPP_
#define INTEGRALS_HPP_
/**
 * @file integrals.hpp
 * @author Adam Lamson
 * @brief Probility Density Function integrals for KMC algorithm. Used to create
 * lookup tables
 * @version 0.1
 * @date 2019-04-15
 *
 * @copyright Copyright (c) 2019
 *
 */

#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <cassert>
#include <cstdio>
#include <iostream>
#include <string>

/*! \brief Integrate exponential factor with the form of
 * e^{-M * (\sqrt{ s^2 + lm^2} - ell0)^2}
 * from sbound0 to sbound1  with respect to the variable s.
 *
 * \param lm Physically, this is the perpendicular distance above rod
 * \param sbound lowerr limit of integral
 * \param sbound Upper limit of integral
 * \param M exponential constant factor. Physically, this is the product of
 (1-load_sensitivity)*spring_const/(k_B * Temperature)
 * \param ell0 Shift of the integrands mean. Physically, protein rest length
 * \return result The value of the integration

 */
inline double integral(double lm, double sbound0, double sbound1, double M,
                       double ell0) {
    if (sbound0 >= sbound1) {
        return 0;
    }
    auto integrand = [&](double s) {
        // lambda capture variabls ell0 and M
        const double exponent = sqrt(s * s + lm * lm) - ell0;
        return exp(-M * exponent * exponent);
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, sbound0, sbound1, 10, 1e-6, &error);
    return result;
}

inline double bind_vol_integral(double sbound, double M, double ell0) {
    assert(sbound > 0);
    auto integrand = [&](double s) {
        // lambda capture variabls ell0 and M
        const double exponent = s - ell0;
        return s * s * exp(-M * exponent * exponent);
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, 0, sbound, 10, 1e-6, &error);
    return 4. * M_PI * result;
}

/*! \brief Integrate exponential factor with the form of
 * e^{-M * [ (1-e_fact)\sqrt{s^2 + lm^2} - ell0)^2 -
 *          fdep_length * (\sqrt{ s^2 + lm^2} - ell0) ] }
 * from sbound0 to sbound1  with respect to the variable s.
 *
 * \param lm Physically, this is the perpendicular distance above rod
 * \param sbound lowerr limit of integral
 * \param sbound Upper limit of integral
 * \param M exponential constant factor. Physically, this is the product of
 spring_const/(k_B * Temperature)
 * \param e_fact energy(load) sensitivity to unbinding.
 * \param fdep_length Characteristic length for force dependent unbinding.
 * \param ell0 Shift of the integrands mean. Physically, protein rest length
 * \return result The value of the integration

 */
inline double fdep_integral(double lm, double sbound0, double sbound1, double M,
                            double e_fact, double fdep_length, double ell0) {
    if (sbound0 >= sbound1) {
        return 0;
    }
    auto integrand = [&](double s) {
        const double rprime = sqrt(s * s + lm * lm) - ell0;
        const double energy_term = .5 * (1. - e_fact) * rprime * rprime;
        const double force_term = fdep_length * rprime;
        return exp(-M * (energy_term - force_term));
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, sbound0, sbound1, 10, 1e-6, &error);
    return result;
}

inline double fdep_bind_vol_integral(double sbound, double M, double e_fact,
                                     double fdep_length, double ell0) {
    assert(sbound > 0);
    auto integrand = [&](double s) {
        const double rprime = s - ell0;
        const double energy_term = .5 * (1. - e_fact) * rprime * rprime;
        const double force_term = fdep_length * rprime;
        return exp(-M * (energy_term - force_term));
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, 0, sbound, 10, 1e-6, &error);
    return 4. * M_PI * result;
}

/*! \brief Integrate exponential factor with the form of
 * e^{-M * [ (1-e_fact)\sqrt{s^2 + lm^2} - ell0)^2 -
 *          fdep_length * (\sqrt{ s^2 + lm^2} - ell0) ] }
 * from sbound0 to sbound1  with respect to the variable s.
 *
 * \param lm Physically, this is the perpendicular distance above rod
 * \param sbound lower limit of integral
 * \param sbound Upper limit of integral
 * \param M1 exponential constant factor when spring is compressed.
 *    Physically, this is the product of spring_const_1/(k_B * Temperature)
 * \param M2 exponential constant factor when spring is stretched.
 *    Physically, this is the product of spring_const_2/(k_B * Temperature)
 * \param e_fact energy(load) sensitivity to unbinding.
 * \param fdep_length Characteristic length for force dependent unbinding.
 * \param ell0 Shift of the integrands mean. Physically, protein rest length
 * \return result The value of the integration

 */
inline double asym_integral(double lm, double sbound0, double sbound1,
                            double M1, double M2, double e_fact,
                            double fdep_length, double ell0) {
    if (sbound0 >= sbound1) {
        return 0;
    }
    auto integrand = [&](double s) {
        const double rprime = sqrt(s * s + lm * lm) - ell0;
        const double energy_term = .5 * (1. - e_fact) * rprime * rprime;
        const double force_term = fdep_length * rprime;
        const double M = rprime < 0. ? M1 : M2;
        return exp(-M * (energy_term - force_term));
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, sbound0, sbound1, 10, 1e-6, &error);
    return result;
}

inline double asym_bind_vol_integral(double sbound, double M1, double M2,
                                     double e_fact, double fdep_length,
                                     double ell0) {
    assert(sbound > 0);
    auto integrand = [&](double s) {
        const double rprime = s - ell0;
        const double energy_term = .5 * (1. - e_fact) * rprime * rprime;
        const double force_term = fdep_length * rprime;
        const double M = rprime < 0. ? M1 : M2;
        return exp(-M * (energy_term - force_term));
    };
    double error = 0;
    double result =
        boost::math::quadrature::gauss_kronrod<double, 21>::integrate(
            integrand, 0, sbound, 10, 1e-6, &error);
    return 4. * M_PI * result;
}

#endif /* INTEGRALS_HPP_ */


/*
 * Software License Agreement (BSD License)
 *
 *  Beta-SLAM - Simultaneous localization and grid mapping with beta distributions
 *  Copyright (c) 2013-2019, Joachim Clemens, Thomas Reineking, Tobias Kluth
 *  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 BSLAM nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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.
 */

#include <cmath>
#include <assert.h>

#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/beta.hpp>

#include "bslam/utils/Factorial.h"

namespace bslam {

BetaDistribution::BetaDistribution( float alpha, float beta ) :
		m_alpha( alpha ),
		m_beta( beta )
{
	// Nothing else to do here
}

bool
BetaDistribution::operator==( const BetaDistribution &other ) const {
	return m_alpha == other.m_alpha && m_beta == other.m_beta;
}

double
BetaDistribution::B( double alpha, double beta ) {
	//return gamma( alpha ) * gamma( beta ) / gamma( alpha + beta );
	return boost::math::beta( alpha, beta );
}

double
BetaDistribution::Binv( double alpha, double beta ) {
	//return gamma( alpha + beta ) / gamma( alpha ) * gamma( beta );
	return 1.0 / B( alpha, beta );
}

uint32_t
BetaDistribution::choose( uint32_t n, uint32_t k ) {
	// iterative
	uint32_t res = 1;
	for( uint32_t i = 1; i <= k; i++ )
		res *= (n + 1 - i) / i;
	return res;

	// recursive
	//return k == 0 ? 1 : (n * choose( n - 1, k - 1 )) / k;
}

double
BetaDistribution::gamma( double x ) {
	//return std::tgamma( x );
	return boost::math::tgamma( x );
}

double
BetaDistribution::digamma( double x ) {
	/*
	// According to http://web.science.mq.edu.au/~mjohnson/code/digamma.c
	assert(x > 0);

	double 	result = 0,
			xx, xx2, xx4;

	for ( ; x < 7; ++x)
		result -= 1/x;

	x	-= 1.0/2.0;
	xx	= 1.0/x;
	xx2	= xx*xx;
	xx4	= xx2*xx2;

	result += log( x ) + (1. / 24.) * xx2 - (7.0 / 960.0) * xx4 + (31.0 / 8064.0) * xx4 * xx2 - (127.0 / 30720.0) * xx4 * xx4;

	return result;
	*/

	return boost::math::digamma( x );
}

double
BetaDistribution::pdf( double x ) const {
	return pdf( x, m_alpha, m_beta );
}

double
BetaDistribution::pdf( double x, double alpha, double beta ) {
	return Binv( alpha, beta ) * pow( x, alpha - 1 ) * pow( 1 - x, beta - 1 );
}

double
BetaDistribution::pmf( uint32_t k, uint32_t n, double alpha, double beta ) {
	return choose( n, k ) * B( k + alpha, n - k + beta ) * Binv( alpha, beta );
}

double
BetaDistribution::cdf( double x ) const {
	return cdf( x, m_alpha, m_beta );
}

double
BetaDistribution::cdf( double x, double alpha, double beta ) {
	return boost::math::ibeta( alpha, beta, x ); // regularized incomplete beta function
}

double
BetaDistribution::cdfComp( double x, double alpha, double beta ) {
	return boost::math::ibetac( alpha, beta, x ); // regularized incomplete beta function
}

double
BetaDistribution::cdf( uint32_t k, uint32_t n, double alpha, double beta ) {
	throw std::runtime_error( "Not implemented yet" );
	//return 1 - B( beta + n - k - 1, alpha + k + 1 ) * hyp3F2( 1, alpha + k + 1, -n + k + 1; k + 2, -beta - n + k + 2; 1 ) / (B( alpha, beta ) * B( n - k, k + 2 ) * (n + 1));
	return 0.0;
}

double
BetaDistribution::mean() const {
	return mean( m_alpha, m_beta );
}

constexpr double
BetaDistribution::mean( double alpha, double beta ) {
	return alpha / (alpha + beta);
}

constexpr double
BetaDistribution::mean( uint32_t n, double alpha, double beta ) {
	return n * alpha / (alpha + beta);
}

constexpr double
BetaDistribution::mean( uint32_t k, uint32_t n, double alpha, double beta ) {
	return (alpha + k) / (alpha + beta + n);
}

double
BetaDistribution::mode() const {
	return mode( m_alpha, m_beta );
}

constexpr double
BetaDistribution::mode( double alpha, double beta ) {
	/*
	assert( alpha >= 1 );
	assert( alpha + beta > 2 );
	*/
	return (alpha - 1) / (alpha + beta - 2);
}

double
BetaDistribution::var() const {
	return var( m_alpha, m_beta );
}

constexpr double
BetaDistribution::var( double alpha, double beta ) {
	return alpha * beta / ((alpha + beta + 1) * (alpha + beta) * (alpha + beta));
}

double
BetaDistribution::entropy() const {
	return entropy( m_alpha, m_beta );
}

double
BetaDistribution::entropy( double alpha, double beta ) {
	return log( B( alpha, beta ) ) - (alpha - 1)*digamma( alpha ) - (beta - 1)*digamma( beta ) + (alpha + beta - 2)*digamma( alpha + beta );
}

constexpr double
BetaDistribution::var( uint32_t n, double alpha, double beta ) {
	return n * alpha * beta * (alpha + beta + n) / ((alpha + beta + 1) * (alpha + beta) * (alpha + beta));
}

constexpr double
BetaDistribution::var( uint32_t k, uint32_t n, double alpha, double beta ) {
	return var( alpha + k, beta + n - k );
}

/*
double
BetaDistribution::pdf( double x, int alpha, int beta ) {
	assert( alpha > 0 );
	assert( beta > 0 );
	return Binv( alpha, beta ) * pow( x, alpha - 1 ) * pow( 1 - x, beta - 1 );
}

double
BetaDistribution::B( int alpha, int beta ) {
	return gamma( alpha ) * gamma( beta ) / gamma( alpha + beta );
}

double
BetaDistribution::Binv( int alpha, int beta ) {
	return gamma( alpha + beta ) / gamma( alpha ) * gamma( beta );
}

double
BetaDistribution::gamma( int x ) {
	return Factorial::value( x - 1 );
}
*/

double
BetaDistribution::ignorance( double priorAlpha, double priorBeta ) const {
	return ( priorAlpha + priorBeta + 1 ) / ( m_alpha + m_beta + 1 );
}

double
BetaDistribution::dissonance() const {
	double	curMean = mean();

	// Shannon entropy
	return -curMean * log( curMean ) - (1.0 - curMean) * log( 1.0 - curMean );
}

double
BetaDistribution::conflict( double epsilon ) const {
	return cdf( 0.5 + epsilon ) - cdf( 0.5 - epsilon );
}

} /* namespace bslam */


/*
 *  VSCSound.cpp
 *  SynthStation
 *
 *  Created by Jonathan Thorpe on 22/10/2011.
 *  Copyright 2011 NXP. All rights reserved.
 *
 */

#include "VSCSound.h"

#include <boost/assert.hpp>

#include <cmath>
#include <cassert>

VSC::Sound::Pitch::Pitch() : mReferenceAFrequency(440.0)
{
    computeMidiNoteFrequencies();
}

VSC::Float VSC::Sound::Pitch::logFrequencyToFrequency(Float logFreq)
{
	return std::pow(10.0, logFreq);
}

VSC::Float VSC::Sound::Pitch::frequencyToLogFrequency(Float freq)
{
	return std::log10(freq);
}

VSC::Float VSC::Sound::Pitch::frequencyForMidiNote(Float midiNote)
{
	return frequencyForMidiNote((unsigned int)midiNote);
}

VSC::Float VSC::Sound::Pitch::frequencyForMidiNote(unsigned int midiNote)
{
	BOOST_ASSERT_MSG(midiNote >= 0 && midiNote < 127, "MIDI note should be in range [0-127]");
	return mMIDINoteFrequencies[midiNote];
}

void VSC::Sound::Pitch::setReferenceAFrequency(Float f)
{
	mReferenceAFrequency = f;
    computeMidiNoteFrequencies();
}

VSC::Float VSC::Sound::Pitch::getReferenceAFrequency(void)
{
	return mReferenceAFrequency;
}

void VSC::Sound::Pitch::computeMidiNoteFrequencies(void)
{
	mMIDINoteFrequencies.resize(128);
	for (int x = 0; x < 127; ++x)
    {
		Float freq = (mReferenceAFrequency / 32.0) * (std::pow(2.0, ((x - 9.0) / 12.0)));
		mMIDINoteFrequencies[x] = freq;
	}
}


#include "aux/eigen2hdf.hpp"
#include "aux/message.hpp"
#include "aux/timer.hpp"
#include "post_processing/macroscopic_quantities.hpp"
#include "post_processing/mass.hpp"
#include "post_processing/momentum.hpp"
#include "quadrature/qhermite.hpp"
#include "spectral/basis/spectral_basis.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/spectral_elem.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"
#include "spectral/basis/spectral_function/hermite_polynomial.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"
#include "spectral/utility/mass_matrix.hpp"

#include "spectral/polar_to_hermite.hpp"
#include "spectral/shift_hermite_2d.hpp"

#include <Eigen/Sparse>
#include <boost/program_options.hpp>
#include <fstream>
#include <iomanip>
#include <iostream>

template <typename T>
struct show_name
{
};

#define PI 3.141592653589793238462643383279502884197

using namespace std;
using namespace boltzmann;

namespace po = boost::program_options;

#ifdef EXTENDED_PRECISION
typedef long double numeric_t;
#else
typedef double numeric_t;
#endif

int main(int argc, char *argv[])
{
  Timer<> timer;
  po::options_description options("options");
  options.add_options()
      ("help", "show help message");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << options << "\n";
    return 0;
  }

  // read polar basis from file
  typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
  polar_basis_t polar_basis;
  SpectralBasisFactoryKS::create(polar_basis, "spectral_basis.desc");
  //  SpectralBasisFactoryKS::write_basis_descriptor(polar_basis, "spectral_basis.desc");

  int max_deg = spectral::get_max_k(polar_basis);
  const unsigned int K = max_deg + 1;
  // create corresponding Hermite basis
  typedef typename SpectralBasisFactoryHN::basis_type hermite_basis_t;
  hermite_basis_t hermite_basis;
  SpectralBasisFactoryHN::create(hermite_basis, max_deg + 1, 2);
  SpectralBasisFactoryHN::write_basis_descriptor(hermite_basis, "hermite_basis.desc");

  if (hermite_basis.n_dofs() != polar_basis.n_dofs()) {
    throw runtime_error("Hermite basis does not match!");
    return 1;
  }

  cout << "size(polar basis) = " << polar_basis.n_dofs() << endl
       << "size(hermite basis) = " << hermite_basis.n_dofs();

  cout << "\n--------------------\n";
  cout << "Test 2: (P->H) -> (H->P) show coefficients\n";

  /*
   * load coefficients (polar basis) from HDF5
   */
  const unsigned int N = polar_basis.n_dofs();
  Eigen::VectorXd coeffs(N);
  hid_t h5_init = H5Fopen("init.h5", H5F_ACC_RDONLY, H5P_DEFAULT);
  eigen2hdf::load(h5_init, "coeffs", coeffs);
  H5Fclose(h5_init);
  coeffs.setZero();
  coeffs[0] = 1;
  {
    cout << "input: ||cp||^2: " << coeffs.cwiseAbs2().sum() << endl;
    ofstream fout("cp.dat");
    fout << coeffs;
    fout.close();
  }

  // compute bulk velocity
  Mass mass;
  mass.init(polar_basis);
  Momentum momentum;
  momentum.init(polar_basis);

  {
    auto entries = momentum.entries();
    for (auto entry : entries) {
      cout << entry.first << " " << entry.second << "\n";
    }
  }

  MQEval mqtsc(polar_basis);
  auto mq_eval = mqtsc.evaluator();
  mq_eval(coeffs.data(), N);
  cout << "correct mass: " << mq_eval.m << endl;
  cout << "correct momentum: " << mq_eval.v.transpose() << endl;

  const double m = mass.compute(coeffs.data());
  Eigen::Vector2d u = momentum.compute(coeffs.data()) / m;
  cout << "\n----- input -----\n"
       << "\n";
  cout << scientific << setprecision(8) << "mass: " << m << endl
       << "momentum: " << u(0) << ", " << u(1) << endl;

  // compute hermite coefficients
  Polar2Hermite<polar_basis_t, hermite_basis_t> P2H(polar_basis, hermite_basis);
  // print_timer(timer.stop(), "init P2H");

  Eigen::VectorXd buf(N);
  P2H.to_hermite(buf, coeffs);

  if (sizeof(numeric_t) == 16) {
    cout << "Using *extended precision*  in ShiftHermite\n";
  } else if (sizeof(numeric_t) == 8) {
    cout << "Using double precision in ShiftHermite\n";
  }
  std::vector<numeric_t> cH(buf.data(), buf.data() + N);
  typedef Eigen::Array<numeric_t, Eigen::Dynamic, 1> array_t;
  Eigen::Map<const array_t> vec_cH(cH.data(), cH.size());
  cout << "||c_H||^2: " << vec_cH.cwiseAbs2().sum() << "\n";
  ShiftHermite2D<hermite_basis_t, numeric_t> shift_hermite(hermite_basis);
  shift_hermite.init();
  timer.start();
  shift_hermite.shift(cH.data(), u(0), u(1));
  //  print_timer(timer.stop(), "shift Hermite coefficients");

  // convert to double
  std::transform(cH.begin(), cH.end(), buf.data(), [](numeric_t x) { return double(x); });

  // -> Polar coordinates
  Eigen::VectorXd Cc(N);
  P2H.to_polar(Cc, buf);

  const double mc = mass.compute(Cc.data());
  Eigen::Vector2d uc = momentum.compute(Cc.data()) / mc;
  cout << "\n----- centered -----\n";
  cout << "mass: " << scientific << setprecision(8) << mc << "\t(diff = " << std::abs(m - mc) << ")"
       << endl
       << "momentum: " << uc(0) << ", " << uc(1) << endl;

  // write new coefficients to disk
  hid_t h5_shifted = H5Fcreate("shifted.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  Eigen::Map<Eigen::VectorXd> Cc_eigen(Cc.data(), Cc.size());
  eigen2hdf::save(h5_shifted, "coeffs", Cc_eigen);
  // export hermite coefficients
  Eigen::Map<Eigen::VectorXd> cH_eigen(buf.data(), buf.size());
  eigen2hdf::save(h5_shifted, "coeffs_hermite", cH_eigen);
  H5Fclose(h5_shifted);

  // do some cheap scattering
  // ...

  // Move back to original position
  timer.start();
  shift_hermite.shift(cH.data(), -u(0), -u(1));
  //  print_timer(timer.stop(), "shift Hermite coefficients (back)");

  // go back to polar coordinates
  std::transform(cH.begin(), cH.end(), buf.data(), [](numeric_t x) { return double(x); });
  Eigen::VectorXd Cc2(N);
  P2H.to_polar(Cc2, buf);

  const double m1 = mass.compute(Cc2.data());
  Eigen::Vector2d u1 = momentum.compute(Cc2.data()) / m1;
  // stop here

  auto M = make_mass_matrix(polar_basis, polar_basis);

  cout << "----- move to original pos. -----\n";
  cout << "mass: " << scientific << setprecision(8) << m1 << "\t(diff = " << std::abs(m - m1) << ")"
       << endl
       << "momentum: " << scientific << setprecision(8) << u1(0) << ", " << u1(1)
       << "\t(diff = " << (u - u1).squaredNorm() << ")" << endl;

  Eigen::Map<Eigen::VectorXd> coeffs2(Cc2.data(), Cc2.size());
  Eigen::VectorXd tmp = (coeffs - coeffs2).array().square();
  double shift_error = sqrt((M * tmp).sum());

  cout << "shift_error: " << scientific << setprecision(8) << shift_error << endl;

  return 0;
}


// Boost.Geometry

// Copyright (c) 2015 Oracle and/or its affiliates.

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

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

#include <boost/geometry/algorithms/detail/flattening.hpp>

namespace boost { namespace geometry { namespace detail
{

/*!
\brief The solution of the inverse problem of geodesics on latlong coordinates,
       Forsyth-Andoyer-Lambert type approximation with first order terms.
\author See
    - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
      http://www.dtic.mil/docs/citations/AD0627893
    - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
      http://www.dtic.mil/docs/citations/AD703541
*/
template <typename CT>
class andoyer_inverse
{
public:
    template <typename T1, typename T2, typename Spheroid>
    andoyer_inverse(T1 const& lon1,
                    T1 const& lat1,
                    T2 const& lon2,
                    T2 const& lat2,
                    Spheroid const& spheroid)
        : m_a(get_radius<0>(spheroid))
        , m_b(get_radius<2>(spheroid))
        , m_f(detail::flattening<CT>(spheroid))
        , m_is_result_zero(false)
    {
        // coordinates in radians

        if ( math::equals(lon1, lon2)
          && math::equals(lat1, lat2) )
        {
            m_is_result_zero = true;
            return;
        }

        CT const pi_half = math::pi<CT>() / CT(2);

        if ( math::equals(math::abs(lat1), pi_half)
          && math::equals(math::abs(lat2), pi_half) )
        {
            m_is_result_zero = true;
            return;
        }

        CT const dlon = lon2 - lon1;
        m_sin_dlon = sin(dlon);
        m_cos_dlon = cos(dlon);
        m_sin_lat1 = sin(lat1);
        m_cos_lat1 = cos(lat1);
        m_sin_lat2 = sin(lat2);
        m_cos_lat2 = cos(lat2);

        // H,G,T = infinity if cos_d = 1 or cos_d = -1
        // lat1 == +-90 && lat2 == +-90
        // lat1 == lat2 && lon1 == lon2
        m_cos_d = m_sin_lat1*m_sin_lat2 + m_cos_lat1*m_cos_lat2*m_cos_dlon;
        m_d = acos(m_cos_d);
        m_sin_d = sin(m_d);

        // just in case since above lat1 and lat2 is checked
        // the check below is equal to cos_d == 1 || cos_d == -1 || d == 0
        if ( math::equals(m_sin_d, CT(0)) )
        {
            m_is_result_zero = true;
            return;
        }
    }

    inline CT distance() const
    {
        if ( m_is_result_zero )
        {
            // TODO return some approximated value
            return CT(0);
        }

        CT const K = math::sqr(m_sin_lat1-m_sin_lat2);
        CT const L = math::sqr(m_sin_lat1+m_sin_lat2);
        CT const three_sin_d = CT(3) * m_sin_d;
        // H or G = infinity if cos_d = 1 or cos_d = -1
        CT const H = (m_d+three_sin_d)/(CT(1)-m_cos_d);
        CT const G = (m_d-three_sin_d)/(CT(1)+m_cos_d);

        // for e.g. lat1=-90 && lat2=90 here we have G*L=INF*0
        CT const dd = -(m_f/CT(4))*(H*K+G*L);

        return m_a * (m_d + dd);
    }

    inline CT azimuth() const
    {
        // it's a situation when the endpoints are on the poles +-90 deg
        // in this case the azimuth could either be 0 or +-pi
        if ( m_is_result_zero )
        {
            return CT(0);
        }

        CT A = CT(0);
        CT U = CT(0);
        if ( ! math::equals(m_cos_lat2, CT(0)) )
        {
            CT const tan_lat2 = m_sin_lat2/m_cos_lat2;
            CT const M = m_cos_lat1*tan_lat2-m_sin_lat1*m_cos_dlon;
            A = atan2(m_sin_dlon, M);
            CT const sin_2A = sin(CT(2)*A);
            U = (m_f/CT(2))*math::sqr(m_cos_lat1)*sin_2A;
        }

        CT V = CT(0);
        if ( ! math::equals(m_cos_lat1, CT(0)) )
        {
            CT const tan_lat1 = m_sin_lat1/m_cos_lat1;
            CT const N = m_cos_lat2*tan_lat1-m_sin_lat2*m_cos_dlon;
            CT const B = atan2(m_sin_dlon, N);
            CT const sin_2B = sin(CT(2)*B);
            V = (m_f/CT(2))*math::sqr(m_cos_lat2)*sin_2B;
        }

        // infinity if sin_d = 0, so cos_d = 1 or cos_d = -1
        CT const T = m_d / m_sin_d;
        CT const dA = V*T-U;

        return A - dA;
    }

private:
    CT const m_a;
    CT const m_b;
    CT const m_f;

    CT m_sin_dlon;
    CT m_cos_dlon;
    CT m_sin_lat1;
    CT m_cos_lat1;
    CT m_sin_lat2;
    CT m_cos_lat2;

    CT m_cos_d;
    CT m_d;
    CT m_sin_d;

    bool m_is_result_zero;
};

}}} // namespace boost::geometry::detail

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP


#include "C_DL1D.h"
#include <vector>
#include <MatrixOper.h>
#include "IM_IO.h"
#include "C_OMP.h"
using namespace std;
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdio.h>
#include <fcntl.h>
// #include <gsl/gsl_matrix.h>
// #include <gsl/gsl_linalg.h>
#include <armadillo>
using namespace arma;
dblarray TabTrain;

C_DL1D::C_DL1D(dblarray &training_set)
{
	TabTrain = training_set; // training set, with samples as column
	Npix = TabTrain.nx(); // atom/training sample length
	eps = 2.220446049250313e-16;
}
C_DL1D::~C_DL1D()
{
}

dblarray C_DL1D::dl1d(dblarray &training_set, dblarray &initD, int IterationNumber,int SparsityTarget,double ErrorTarget,bool Verb)
{
	Na = initD.ny(); // number of atoms in dictionary
	int Ntrain = TabTrain.ny(); // number of training samples
	dblarray sample(Npix); // training sample
	double Amean,sample_mean; // atom and training sample mean
	dblarray D = initD; // learned dictionary initialized with initD
	dblarray X(Na,Ntrain);	 // sparse coding coefficients of TabTrain
	dblarray Xt(Ntrain,Na); // transpose of X
	dblarray Xtpinv(Na,Ntrain); // pseudoinverse of Xt
	dblarray Xpinv(Ntrain,Na); // X pseudo-inverse
	dblarray Scurrent(Npix); // Current sample
	dblarray Acurrent(Npix); // Current atom, during normalization
	double Anorm; // Current atom norm
	double minT; // eigenvalue threshold used to compute pseudo inverse
	int atom_usage; // number of sample using a given atom
	int new_sample_ind; // index of the new sample used in place of an useless atom
	int atoms_replaced; // number of unused atoms replaced with training samples
	MatOper terminator;
	double average_sparsity = 0;
	double average_error = 0;
	dblarray sparse_approx(Npix,Ntrain); // sparse approximation of TabTrain in the current dictionary
	dblarray sparse_sample(Npix); // sparse approximation of current training sample
	mat armaXt(Ntrain,Na);
	dblarray At(Na,Ntrain);
	dblarray Vt(Na,Na);
	dblarray V(Na,Na);
	dblarray S(Na,Na);
	mat armaInv;
	gsl_rng *rng;
	const gsl_rng_type * T;
	T = gsl_rng_default;
	rng = gsl_rng_alloc (T);
	/*gsl_matrix * gslA = gsl_matrix_alloc(Ntrain,Na);
		gsl_matrix * gslV = gsl_matrix_alloc(Na,Na);
		gsl_vector * gslS = gsl_vector_alloc(Na);
		gsl_vector * work = gsl_vector_alloc(Na);*/

	//Removing atoms mean and normalizing atoms with norm > 1
	for (int m=0;m<Na;m++)
	{
		for (int p=0;p<Npix;p++)
			Acurrent(p) = D(p,m); // reading atom p
		Amean = Acurrent.mean();
		for (int p=0;p<Npix;p++) // computing atom mean
			Acurrent(p) = Acurrent(p) - Amean; // removing atom mean
		Anorm = sqrt(Acurrent.energy()); // computing 0-mean atom norm
		if (Anorm > 1)
		{
			for (int p=0;p<Npix;p++)
				D(p,m) = (D(p,m) - Amean) / Anorm; // removing mean and normalizing
		}
		else 	for (int p=0;p<Npix;p++) D(p,m) = D(p,m) - Amean; // only removing mean
	}
	C_OMP coder(D); // building OMP sparse coder
	// Removing training sample mean
	for (int k=0; k<Ntrain; k++)
	{
		for (int i=0; i<Npix; i++) // reading sample k
			sample(i) = TabTrain(i,k);
		sample_mean = sample.mean(); // computing sample mean
		for (int i=0; i<Npix; i++)
			TabTrain(i,k) = sample(i) - sample_mean;  // subtracting mean
	}
	// iterating sparse coding and dictionary update steps
	if (Verb == true)
	{
		cout << "Learning dictionary of " << Na << " atoms of " << Npix << " pixels" << endl;
		cout << "Starting Dictionary Learning for " << IterationNumber << " iterations" << endl;
		cout << "Using OMP with SparsityTarget = "<< SparsityTarget << " and ErrorTarget = " << ErrorTarget << endl;
	}
	for (int i=0;i<IterationNumber;i++)
	{
		if (Verb == true)
		{
			if (IterationNumber > 1000)
			{
				if ((i+1)%(IterationNumber/10) == 1)
					cout << "DL iteration " << i+1 << " / " << IterationNumber << ", average sparsity " << average_sparsity << ", average error " << average_error << endl;
			}
			else cout << "DL iteration " << i+1 << " / " << IterationNumber << ", average sparsity " << average_sparsity << ", average error " << average_error << endl;
		}
		// sparse coding training sample in dictionary
		X = coder.omp(TabTrain,SparsityTarget,ErrorTarget,False);
		if (Verb == true)
		{
			// Computing average sparsity given sparse encoding coefficients
			average_sparsity = 0;
			for (int i=0;i<X.nx();i++)
				for (int j=0;j<X.ny();j++)
					if (X(i,j) !=0)
						average_sparsity++;
			average_sparsity = average_sparsity / (Ntrain);
		}

		// Computing sparse coefficients matrix pseudo inverse for dictionary update
		if (Verb == True)
		{
			cout << "Sparse coding complete, average sparsity " << average_sparsity << endl;
			cout << "Updating dictionary ..." << endl;
		}
		//		terminator.inv_mat_svd(X,Xpinv,minT);  previous method, too slow for large data
		// Transposing matrix X before computing its pseudoinverse
		for (int p=0;p<X.nx();p++)
			for (int q=0;q<X.ny();q++)
				Xt(q,p) = X(p,q);
		// Filling matrix armaXt with coefficients from Xt
		for (int p=0;p<Ntrain;p++)
			for (int q=0;q<Na;q++)
				armaXt(p,q) = Xt(p,q);
		//Chosing eigenvalue threhsold value
		minT = Ntrain*eps * sqrt(Xt.energy());
		// Computing pseudoinverse
		armaInv = pinv(armaXt,minT);
		for (int p=0;p<Na;p++)
			for (int q=0;q<Ntrain;q++)
				Xtpinv(p,q) = armaInv(p,q);

		// gsl_matrix_set(gslA,p,q,Xt(p,q));
		// gsl_matrix_free(gslA);
		// gslA = gsl_matrix_alloc(Ntrain,Na);
		// for (int p=0;p<Ntrain;p++)
		// 	for (int q=0;q<Na;q++)
		// 		gsl_matrix_set(gslA,p,q,Xt(p,q));
		// Computing SVD from A
		// gsl_linalg_SV_decomp (gslA,gslV,gslS,work);
		// gsl_linalg_SV_decomp_jacobi (gslA,gslV,gslS);
		// Thresholding/inverting eigenvalues
		// S.init(0);
		/*for (int q=0;q<Na;q++)
			if (gsl_vector_get(gslS,q)<minT)
				S(q,q) = 0;
			else
				S(q,q) = 1/(gsl_vector_get(gslS,q));
		// Reading remaining gsl matrices
		for (int p=0;p<Ntrain;p++)
			for (int q=0;q<Na;q++)
				At(q,p) = gsl_matrix_get(gslA,p,q);
		for (int p=0;p<Na;p++)
			for (int q=0;q<Na;q++)
				V(p,q) = gsl_matrix_get(gslV,p,q);
		// Computing pseudo inverse by multiplying matrices
		Xtpinv = mult(V,mult(S,At));*/

		// Applying MOD dictionary update
		for (int p=0;p<D.nx();p++)
			for (int q=0;q<D.ny();q++)
			{
				D(p,q) = 0;
				for (int k=0;k<Ntrain;k++)
					D(p,q) += TabTrain(p,k)*Xtpinv(q,k);
			}
		// Computing sparse approximation and average quadratic error
		average_error = 0;
		sparse_approx = mult(D,X);
		for (int k=0;k<Ntrain;k++)
		{
			for (int np=0;np<Npix;np++)
				sparse_sample(np) = sparse_approx(np,k) - TabTrain(np,k);
			average_error =+ sqrt(sparse_sample.energy())/Ntrain;
		}
		// Throwing away unused atoms and replacing them by random training samples
		atoms_replaced = 0;
		for (int m=0;m<Na;m++)
		{
			atom_usage = 0;
			for (int p=0;p<Ntrain;p++)
				if (X(m,p)!=0) atom_usage++;
			if (atom_usage == 0)
			{
				new_sample_ind	= gsl_rng_uniform_int(rng, Ntrain-1);
				for (int k=0;k<Npix;k++)
					Acurrent(k) = TabTrain(k,new_sample_ind);
				Amean = Acurrent.mean();
				for (int k=0;k<Npix;k++)
					Acurrent(k) = Acurrent(k) - Amean;
				Anorm = sqrt(Acurrent.energy()); // computing 0-mean atom norm
				if (Anorm > 1)
				{
					for (int k=0;k<Npix;k++)
						D(k,m) = Acurrent(k) / Anorm; // removing mean and normalizing
				}
				else
					for (int k=0;k<Npix;k++)
						D(k,m) = Acurrent(k); // only removing mean
				atoms_replaced++;
			}
		}
		if (Verb == True && atoms_replaced !=0)
			if (IterationNumber > 0)
				if ((i+1)%(IterationNumber/10) == 1) cout << "Replaced " << atoms_replaced << " unused atoms with random training samples" << endl;
				else
					cout << "Replaced " << atoms_replaced << " unused atoms with random training samples" << endl;

		// Normalizing atoms with norm > 1
		for (int m=0;m<Na;m++)
		{
			for (int p=0;p<Npix;p++)
				Acurrent(p) = D(p,m);
			Anorm = sqrt(Acurrent.energy());
			if (Anorm > 1)
				for (int p=0;p<Npix;p++)
					D(p,m) = D(p,m) / Anorm;
		}

		// Updating sparse coder with new version of dictionary
		coder.update_dictionary(D);
	}
	return D;
}